Active rotation with quaternions: from initial orientation given with Euler angles get the new orientation after an active rotation described by the qr quaternion.

Hello everyone!

I'm sorry to be so verbose, but I'm quite sure my question will help others too.

My aim is to create a get_rotated_pose() function - attached - which takes as input a point in space, A, and returns point B, i.e. the point A to which the rotation described by the qr quaternion has been applied.

Points A and B are described as follows: [x, y, z, Ry, Rx, Rz], where x, y and z are the Cartesian coordinates of the point and Ry, Rx and Rz the orientation of the point in space, relative to the robot reference (UR5 Base in the picture below), according to the Universal Robots convention.

To test the effectiveness of my code, I used the RoboDK software (see img_1). I created a first Target, Target A, representing the position and orientation of point A (with a randomly chosen orientation). I then calculated the pose (position and orientation) of point B using my function described above and the lines of code below:

pose_A = [197 197 0 deg2rad(90) deg2rad(0) deg2rad(45)]; % see target A in the diagram 
desired_rotation = deg2rad(45); % 45 degrees of desired rotation
rot_axis = [0 0 1];             % rotation around z axis (of the reference frame)
pose_B(1:6) = get_rotated_pose(pose_A, desired_rotation, rot_axis);
display(pose_B);

pose_B =

   -0.0000  278.6001         0    1.1107    1.1107    0.7854

The rotation I wanted to apply to point A is a 45-degree rotation around the z-axis of my reference Frame (ie. 'UR5 Base' on the image). I then displayed point B (Target B) in roboDK.

Result interpretation and issue:

The coordinates of point B are as expected. However, the orientation of point B is not as expected and I have absolutely no idea what's going on. So I'd be extremely grateful if someone could help me to find out why the orientation of B is not as expected. This is not that clear in the image above, but the center of the reference frame attached to target A and B are actually in the (x, y) plane of the UR5 Base.

Probable problem location:

I believe the bug comes from this part of my code (the whole function can be found attached):

%   -------      first compute the future orientation    -------     

% We need to compute an active rotation.
% The formula gives us: p' = qr^(-1) * p * qr
% p is described as follows: p = (0, x, y, z), where (x, y, z) are the
% coordinates of the point to be rotated (ie. A).
% p' is descirbed in the same way as p.
% finally, qr is the rotation quaternion

pa = [0, pose_in(5), pose_in(4), pose_in(6)]; % XYZ convention
if rotation_of_180_degrees
    % we need first to rotate of 180 degrees before rotating of 'angle'
    axang = [rot_axis, pi];
    qr_180 = axang2quat(axang);
    pb = quatmultiply(quatinv(qr_180), pa);
    pb = quatmultiply(pb, qr_180);
else
    % desired_rotation is between -180 and 180 degrees
    pb = pa;
    clear pa
    angle = desired_rotation;
end

axang = [rot_axis, angle];
qr = axang2quat(axang);
pb = quatmultiply(quatinv(qr), pb);
pb = quatmultiply(pb, qr);

I've used the following convention (to avoid unexpected results): if the desired rotation angle desired_rotation is greater than 180 degrees, we decompose it as follows: desired_rotation = 180 + angle (where angle < 180 degrees)

Here are more informations about the sources I used to create this code:

Quaternions (danceswithcode.net)

Difference between 2 orientations - MATLAB Answers - MATLAB Central (mathworks.com)

Rotation order of quatrotate - MATLAB Answers - MATLAB Central (mathworks.com)

Thanks a lot in advance!

And best regards,

Nicolas C.

[James Tursa]

It is not clear to me what you want as resulting orientation. Do you want to rotate both the position and orientation of the object with your rotation quaternion? E.g., like you were on a merry-go-round and both your position and orientation changes as it rotates. Or do you want only the position rotated but the orientation to remain the same? E.g., you are on the merry-go-round but reverse spinning with respect to the merry-go-round just the right amount to negate the orientation rotation so that you can constantly stare at a fixed tree off to the side. How is your original orientation represented in the above code?

[Paul]

"Points A and B are described as follows: [x, y, z, Ry, Rx, Rz], where x, y and z are the Cartesian coordinates of the point and Ry, Rx and Rz the orientation of the point in space"

What is the definition of the orientation of a point ? Isn't a point just defined by its coordinates in prescribed coordinate frame?

[Nicolas CRETIN]

Thanks you so much Paul and James Tursa for helping me!

"E.g., like you were on a merry-go-round and both your position and orientation changes as it rotates"

Yes, exactly; that's what I'm trying to do.

"How is your original orientation represented in the above code?"

I edited ma question to add this information. But here again is the pose_A that I gave as input to my function:

pose_A = [197 197 0 deg2rad(90) deg2rad(0) deg2rad(45)]; % see target A in the diagram 
desired_rotation = deg2rad(45); % 45 degrees of desired rotation
rot_axis = [0 0 1];             % rotation around z axis (of the reference frame)
pose_B(1:6) = get_rotated_pose(pose_A, desired_rotation, rot_axis);
display(pose_B);

pose_B =

    -0.0000  278.6001         0    1.1107    1.1107    0.7854

[Nicolas CRETIN]

"What is the definition of the orientation of a point ? Isn't a point just defined by its coordinates in prescribed coordinate frame?"

I believe that Ry, Rx and Rz are the RPY (roll pitch yaw) angles in relation to the reference frame. But I don't quite understand the difference between RPY angles and Euler angles. So I may have confused them. I'm going to check it right now.

Otherwise, (x, y, z) are the cartesian coordinates relative to the reference frame.

[Nicolas CRETIN]

"What is the definition of the orientation of a point ? Isn't a point just defined by its coordinates in prescribed coordinate frame?"

Okay, I believe I was wrong.

The convention that seems to be used by the universal robots is actually the axis-angle representation (according to Universal Robots - Explanation on robot orientation (universal-robots.com))

Thank you so much! I will try to change my code accordingly

Best regards

[Nicolas CRETIN]

Hi everyone!

I believe I've found my problem, but I'm not 100 percent sure.

I used the following formula to calculate the new orientation: qb = qa.qr (where ". "stands for quaternion multiplication). But I'm really struggling to find a source that would mathematically justify this formula. Is it mathematically right?

I've added below the part of the code of my function which computes the new orientation (and which was causing the issue):

% Now we rotate the input orientation qa of the rotation quaternion qr 
% to obtain the output quaternion qb, using the formula: qb = qa qr
qa = axang2quat(axang_a);
if rotation_of_180_degrees
    % we need first to rotate of 180 degrees before rotating of 'angle'
    axang = [rot_axis, pi];
    qr_180 = axang2quat(axang);
    qb = quatmultiply(qa, qr_180);
else
    % desired_rotation is between -180 and 180 degrees
    qb = qa;
    angle = desired_rotation;
end

axang = [rot_axis, angle];
qr = axang2quat(axang);
qb = quatmultiply(qb, qr);
clear qr qr_180

% [u_x, u_y, u_z, theta]
axang_b = quat2axang(qb);

The result seems rather coherent in any case, as the following image shows (but not completely actually):

I'm also enclosing the whole funuction.

So I'd be very grateful if someone could tell me whether this solution is mathematically correct or not!

Best regards and have a nice day,

Nicolas C.

Answer 1

[James Tursa]

Yes, for your convention q' = q * qr is mathematically correct. But note that this formula only works for certain quaternion conventions, which seems to match your use case. For other quaternion conventions, this would be backwards. So be careful when comparing to online formulae for this because some of them will appear backwards.

[Nicolas CRETIN]

Okay, thank you very much for your help!!