Rpy2rotvec() function

[hiroyuki.tanaka]

Hi,
I would like to know the exact behavior of rpy2rotvec function of URScript to do some work outside of UR Controller.

I read these info:

Pose Rotation Order

In the description of a Pose, it says the last 3 values are “an axis-angle”, but if you look at a Waypoint, it shows a Rotation Vector as RX, RY, RZ. I am used to working with compound Euler angles (Yaw, Pitch, Roll), but haven’t worked with a rotation vector before. Is there a way to convert between RPY and a Rotation Vector, either through math or a built-in script function, or a way a pose can be defined directly using YPR? I can’t find any examples in the manual or an explanation any furt…

https://www.universal-robots.com/articles/ur/programming/rpy-tofrom-rotation-vector/

And I confirmed that the calculation in the script works most of the times, but not when sth is zero or close to zero.
Is it possible to show us how the calculation is done in rpy2rotvec when sth is zero or close to zero?

The images below show some examples.
When RPY = (-3/10*pi, 0, -9/10*pi), the output of rpy2rotvec() and rp2rv() (defined in rpy.script from the above link, but modified to accept values in radian),
but when RPY = (-pi, 0, -9/10*pi), the output from the two functions are not identical, I assume that is because sth is very close to zero (like 1.2246467991473532e-16).

I also tried Rodrigues() in opencv, but the results was different ([0.49145337, -3.10291443, 0], the sings are opposite)

>>> rotmat = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
>>> cv2.Rodrigues(rotmat)[0]
array([[ 0.49145337],
       [-3.10291443],
       [ 0.        ]])

The software version is 5.16.0 (build date April 10, 2024) on the docker

Thanks a lot!
Hiro Tanaka

[cg1]

One of the problems might be that vector rotations are not unique, as you can at any point have 2 different axis angle rotations describing the same overall 3d rotation.
And at certain angles the axis angle system even has singularities where that are an infinite number of solutions that gives the same rotation.

There can’t therefore be an any guarantees that the conversion between it and another rotation and back again will ever give you the same results.

Axis angles are not unique about this, the same is true about rpy Euler angles, which don’t have unique descriptions for a certain rotation either.

So you are basically switching between two systems of rotation notations where the overall 3D rotation can be described in multiple way and allways expecting them to give the same result.

It should, on the otherhand, still be the same overall rotation just described with other values.

The other is that a well formulated axis angle vector is of the form [x, y, z]*rotation where the vector [x, y, z] us a unit vector (which is vector of length one) and the rotation is a rotation about this unit vector.
The rotation should ideally be within ±pi (±180deg) as a lager rotation could otherwise be described by a smaller rotation with the opposite sign.
And as far as I can see both of you examples have rotations that exceeds pi/2.

Rotations in 3D is unfortunate much more complicated then it has any right to be, but hopefully this brought some clarification.

[hiroyuki.tanaka]

Thank you very much for the comment.

Yes, I know that there are multiple rotation vectors that can describe an identical 3d rotation.
But I am not sure if that’s the point here.

In order to check if two rotation vectors are identical 3D rotation, you can for example calculate the rotation matrix, and see if they are identical.
For example, [pi, 0, 0] and [-pi, 0, 0] are identical 3D rotation, and those rotation matrices are

>>> scipy.spatial.transform.Rotation.from_rotvec([math.pi, 0, 0]).as_matrix()
array([[ 1.0000000e+00,  0.0000000e+00,  0.0000000e+00],
   	[ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16],
   	[ 0.0000000e+00,  1.2246468e-16, -1.0000000e+00]])
>>> scipy.spatial.transform.Rotation.from_rotvec([-math.pi, 0, 0]).as_matrix()
array([[ 1.0000000e+00, -0.0000000e+00,  0.0000000e+00],
   	[ 0.0000000e+00, -1.0000000e+00,  1.2246468e-16],
   	[-0.0000000e+00, -1.2246468e-16, -1.0000000e+00]])

The RPY angle [pi, 0, 9/10 pi] is converted to [-0.49145, 3.10291, 0] with rpy2rotvec() in the URScript, while it is converted to [-0.07688, 0.4854, 0] with the script in the past post.
These two rotation vectors don’t seem to describe the same 3D rotation, because the rotation matrices are different.

>>> scipy.spatial.transform.Rotation.from_rotvec([-0.49145, 3.10291, 0]).as_matrix()
array([[-9.51057034e-01, -3.09015401e-01,  4.84641805e-06],
   	[-3.09015401e-01,  9.51057034e-01,  7.67593050e-07],
   	[-4.84641805e-06, -7.67593050e-07, -1.00000000e+00]])
>>> scipy.spatial.transform.Rotation.from_rotvec([-0.07688, 0.4854, 0]).as_matrix()
array([[ 0.8845455 , -0.01828624,  0.46609534],
   	[-0.01828624,  0.99710374,  0.07382243],
   	[-0.46609534, -0.07382243,  0.88164924]])

So my question is still, how the calculation is handled when sth is close to zero in the script of Universal Robots - RPY to/from rotation vector ?
Is it safe to assume that similar calculation is done as in opencv’s Rodrigues function ?
opencv/modules/calib3d/src/calibration.cpp at 3901426d8515cd8d1c07654ec7e9e8bc3b51a06f · opencv/opencv · GitHub

---

When I calculate the conversion with openCV’s Rodrigues function, I got different results, but actually these 3D rotation are very close,

>>> scipy.spatial.transform.Rotation.from_rotvec([-0.49145, 3.10291, 0]).as_matrix()
array([[-9.51057034e-01, -3.09015401e-01,  4.84641805e-06],
   	[-3.09015401e-01,  9.51057034e-01,  7.67593050e-07],
   	[-4.84641805e-06, -7.67593050e-07, -1.00000000e+00]])
>>> scipy.spatial.transform.Rotation.from_rotvec([0.49145, -3.10291, 0]).as_matrix()
array([[-9.51057034e-01, -3.09015401e-01, -4.84641805e-06],
   	[-3.09015401e-01,  9.51057034e-01, -7.67593050e-07],
   	[ 4.84641805e-06,  7.67593050e-07, -1.00000000e+00]])

---

The other is that a well formulated axis angle vector is of the form [x, y, z]*rotation where the vector [x, y, z] us a unit vector (which is vector of length one) and the rotation is a rotation about this unit vector.
The rotation should ideally be within ±pi (±180deg) as a lager rotation could otherwise be described by a smaller rotation with the opposite sign.
And as far as I can see both of you examples have rotations that exceeds pi/2.

I am converting a Roll-Pitch-Yaw angle to a rotation vector, so as far as the all of the roll, pitch and yaw angles are within a specific range (+/- pi), I think rpy2rotvec() should be able to handle the rotation angle to be within the +/- pi range, shouldn’t it?

Thanks,