Clear Filters
Clear Filters

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

13 views (last 30 days)
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:
Thanks a lot in advance!
And best regards,
Nicolas C.
  6 Comments
Nicolas CRETIN
Nicolas CRETIN on 29 Feb 2024
Edited: Nicolas CRETIN on 29 Feb 2024
"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
Nicolas CRETIN on 2 Mar 2024
Edited: Nicolas CRETIN on 2 Mar 2024
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.

Sign in to comment.

Accepted Answer

James Tursa
James Tursa on 3 Mar 2024
Edited: James Tursa on 4 Mar 2024
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.

More Answers (0)

Products


Release

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!