Normalization and singularities in the polar-azimuth representation

[rsantos88]

I am trying to solve point 7 of the issue https://github.com/HUMASoft/yarp-devices/issues/7 but I am in doubt whether kinematicRepresentationLib has the possibility to work with the pitch-roll representation form. Previously, our project has worked with POLAR_AZIMUTH as a orientation system to encode and decode the pose of inclination and orientation. However, if we wanted to put positive and negative angles in pitch and roll, what would be the correct orientation system that would respect the signs and angles?

[PeterBowman]

What is exactly pitch-roll, i.e. which angles get affected (XYZ, local or global) and what is the sequence? Is it actually different from the current polar-azimuthal system? What happens if you pass negative values? Does it need normalization if the azimuthal angle exceed the [-180, 180] range?

[rsantos88]

Currently we pass the roll-pitch values connecting to the port ../rpc_transform:s using the movj(roll pitch) command. The device is launched by adding the parameters --coordRepr none --angleRepr polarAzimuth --angularUnits degrees. Therefore, internally movj works with a vector describing a three-dimensional pose in coordinate_system :: CARTESIAN and orientation_system :: AXIS_ANGLE_SCALED systems. When we want to retrieve the roll-pitch values indicated by RPC through this port, we need to apply decodePose currently configured with orientation_system::POLAR_AZIMUTH. The problem is that the values are distorted, thus leaving the result of the targetPose vector: movj 0 0 -> targetPose = [0, -0] movj 10 0 -> targetPose = [10 -0] movj 0 10 -> targetPose = [0 -0] movj 10 10 -> targetPose = [10 10] movj -10 0 -> targetPose = [10 180] movj 0 -10 -> targetPose = [0 0 ] movj -10 -10 -> targetPose = [10 170]

It is surely an absurd error, but I am trying to follow the structure with which it was commanded using movj with inclination and orientation. I think that since these values are always positive, the targetPose correctly returned its values when passing through the decodePose function with Polar-Azimuth orientation system.

[PeterBowman]

If you think about it, these conversion pairs actually refer to the same orientation. From the Wikipedia article on spherical coordinates:

Conversion to scaled axis-angle:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/cbec406a1d36ec674027e291e931c753cdf93587/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp#L145-L149

Conversion from scaled axis-angle:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/cbec406a1d36ec674027e291e931c753cdf93587/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp#L272-L274

Note there is both a symmetry (just add 180º to the azimuth to represent negative polar angles) and a singularity (infinite orientations match a polar angle equal to 0º or 180º), therefore:

• polar 0°, azimuthal 10° = polar 0°, any azimuthal angle value (due to the singularity)
• polar -10°, azimuthal 0º = polar 10º, azimuthal 180º (because you "turn around" and "bend" towards the opposite direction)
• polar -10°, azimuthal -10° = polar 10°, azimuthal 170° (same as above, due to symmetry)

So far it looks fine, you must keep these properties in mind when working with symmetrical values or when exactly in a singular pose. I think it also nice that the converter always (or at least it seems so) returns positive polar angle values. You can normalize it in any way you wish, though, in the cartesian controller implementation.

Note that despite representing a position in the 3D space, it was decided that the concepts of polar and azimuthal angles cope nicely with the convention used internally by the soft-neck, and so it was implemented to represent an orientation using this terminology. Now I see it might have been preferrable to align it with the existing EULER_ZYZ orientation type: local_Z->local_Y->local_Z (i.e. pitch-yaw). In this scenario, you don't need the last local_Z (pass 0) and you should be able to preserve the initial Z orientation.

To sum up, if you really need to avoid the singularity, give the EULER_ZYZ a try. Remember you need to pass three elements, but only the first two are required, and the last one should always be 0. So effectively it's an Euler ZY, where Z is your azimuthal angle, and Y the polar one, i.e. the order has been reversed: (azimuthal, polar).

[rsantos88]

Thanks for the explanation. It makes perfect sense that the function tries to transform the output into always positive angles, preserving the equivalence in terms of polar-azimuth orientation. The point is that in the case that we work, both the sensor and the controller work with roll-pitch, also the target that we want to introduce. Therefore, I would like the decodePose function to respect exactly the same values entered on its output. Testing your proposal to use EULER_ZYZ, I've verified that I would still get unwanted transforms for what I am looking for. For example, the cases that wouldn't match (as I would like it to be) are:

input> 0.000000 -10.000000 0.000000
encodePose> 0.000000 0.000000 0.000000 0.000000 -0.174533 0.000000
decodePose> -180.000000 10.000000 180.000000

input> 10.000000 -10.000000 0.000000
encodePose> 0.000000 0.000000 0.000000 0.015231 -0.174089 0.174089
decodePose> -170.000000 10.000000 -180.000000

input> -10.000000 -10.000000 0.000000
encodePose> 0.000000 0.000000 0.000000 -0.015231 -0.174089 -0.174089
decodePose> 170.000000 10.000000 180.000000 

I think that on second thought, perhaps the most effective solution is to continue introducing inclination-orientation values that match perfectly with the polar-azimuth input with which we have been working until now and make the conversion to roll-pitch before passing it to the controller. . In this way, our system would continue to use the same way to represent the orientation as in previous controls.

[rsantos88]

I think that on second thought, perhaps the most effective solution is to continue introducing inclination-orientation values that match perfectly with the polar-azimuth input with which we have been working until now and make the conversion to roll-pitch before passing it to the controller. . In this way, our system would continue to use the same way to represent the orientation as in previous controls.

Finally, this will be the solution to be applied. I close this issue.