Questions about questions about my_gurdy_demo.py

[MeineLiebeAxt]

I have some questions about my_gurdy_demo.py in this case. (“URDF for robot modelling” - “create the URDF files for a Gurdy Robot from scratch”).

First question:
In solution, in order to check if the joints fulfill the desired angle, there is a function:

def assertAlmostEqualAngles(self, x, y,):
    c2 = (sin(x) - sin(y)) ** 2 + (cos(x) - cos(y)) ** 2
    angle_diff = acos((2.0 - c2) / 2.0)
    return angle_diff

I don’t really understant what is going on here. Would you please inform me of what this geometric formular is, and what is “2.0” here? Is it related with the “error=0.1” in next function:

def gurdy_check_continuous_joint_value(self, joint_name, value, error=0.1):
    """
    Check the joint by name 'base_waist_joint', 'body_head_joint', 'waist_body_joint is near the value given
    We have to convert the joint values removing whole revolutions and converting negative versions
    of the same angle
    :param joint_name:
    :param value:
    :param error: In radians
    :return:
    """
    joint_reading = self.gurdy_joint_dictionary.get(joint_name)
    if not joint_reading:
        print("self.gurdy_joint_dictionary="+str(self.gurdy_joint_dictionary))
        print("joint_name===>"+str(joint_name))
        assert "There is no data about that joint"
    clean_joint_reading = self.convert_angle_to_unitary(angle=joint_reading)
    clean_value = self.convert_angle_to_unitary(angle=value)

    dif_angles = self.assertAlmostEqualAngles(clean_joint_reading, clean_value)
    similar = dif_angles <= error

    return similar

Second question:
If I want to change the revolutional velocity of joints, I know we can achieve that by changing the PID-values in controllers.yaml. But for example, I need 1st movement to rotate the joints faster, but 2nd movement slower. Changing PID can only change the velocity once and for all, it is not flexible as soon as PID is decided. Can we do that in “my_gurdy_demo.py”? Or is there other ways?

Thank you.

[rzegers]

Hello @MeineLiebeAxt,

this formula:
c2 = (sin(x) - sin(y)) ** 2 + (cos(x) - cos(y)) ** 2
uses the squared Euclidean distance between two points on the unit circle. The squared Euclidean distance the same equation as the Euclidean distance metric, but does not take the square root. As a result, the squared Euclidean distance is faster than the regular Euclidean distance.

Here:
angle_diff = acos((2.0 - c2) / 2.0)
The we use the law of cosines to get the absolute difference between two angles. The law of cosines can be used to find the measure of an angle when the length of the three sides of a triangle are known.
In the formula below a , b and c are sides of the triangle. And the opposite angles to the sides a, b and c of a triangle are respectively A, B and C. Then the law of cosines says:

cos C = (a2 + b2 − c2)/2ab

But we are working with an unit circle, so a = b = 1.
This gives:
cos C = (1 + 1 − c2)/2*1*1
or:
cos C = (2.0 -c2) / 2.0
We use 2.0 instead of 2 to tell Python that this two is a floating point number.
so:
angle_diff = acos((2.0 - c2) / 2.0)

This works with radians. If the angle is in degrees, you must do a conversion.

Hope this helps,

Roberto

[MeineLiebeAxt]

Thank you @rzegers very much! That is really comprehensive!