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)
Show older comments
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
Accepted Answer
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)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!