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

8 次查看（过去 30 天）

显示 更早的评论

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 个评论

### 采纳的回答

James Tursa
2024-3-3

编辑：James Tursa
2024-3-4

### 更多回答（0 个）

### 另请参阅

### 类别

### Community Treasure Hunt

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

Start Hunting!