# How to compute orientation error for a Puma end effector

i hope that you'll be ok!

I'm trying to control a Puma robot using the inverse kinematics in Matlab. I used unit quaternions to express the orientation of End Effector frame and orientation of desired frame from Rotation matrix, like in Sciavicco-Siciliano. This is the code:

Rd = [-sin(beta)  0  cos(beta);  % desired orientation matrix of ee
cos(beta)  0  sin(beta);
0         -1  0        ];

Re = H0e(1:3,1:3); % orientation matrix of EE from direct kinematics

u0 = @(R) 0.5* sqrt(1+R(1,1)+R(2,2)+R(3,3));
u1 = @(R) 0.5* sign(R(3,2)-R(2,3)) * sqrt(1+R(1,1)-R(2,2)-R(3,3));
u2 = @(R) 0.5* sign(R(1,3)-R(3,1)) * sqrt(1-R(1,1)+R(2,2)-R(3,3));
u3 = @(R) 0.5* sign(R(2,1)-R(1,2)) * sqrt(1-R(1,1)-R(2,2)+R(3,3));
S = @(x,y,z) [0 -z y; z 0 -x; -y x 0];

Qd = [u0(Rd) u1(Rd) u2(Rd) u3(Rd)]';  % {eta_d, epsilon_d} desired unit quaternion
Qe = [u0(Re) u1(Re) u2(Re) u3(Re)]';  % {eta_e, epsilon_e} unit quaternion of ee


The problem is that two different rotation matrix produce the same unit quaternion and therefore the error is zero.

These are the two matrices:

Rd =

0.7250         0    0.6887
0.6887         0   -0.7250
0   -1.0000         0

Re =

0.7250   -0.6887    0.0000
-0.0000   -0.0000    1.0000
-0.6887   -0.7250   -0.0000


Where am i doing wrong? Could you help me?

Thank a lot!

I found the error, the rotation matrix Rd is not a rotation matrix because its determinant is equal to -1:

det(Rd) = -1


I'm so sorry for your time! Forgive me!

Where did you get those equations from for the conversion to quaternion? I went to this question and implemented two of the answers there in Matlab (Octave), and both answers gave the same results:

>> Qd2
Qd2 =

0.10469  -0.26218  -0.26218   0.65670

>> Qe2
Qe2 =

0.65670  -0.26218  -0.26218   0.65670


I converted this answer, and your question meets the (b1_squared >= 0.25) criteria, so I could use it, but it's not suitable for all conversions because not all cases are implemented there ("I will leave it as an exercise to the OP to fill out the other three.")

Then I converted this answer and got the same results. That answer does apply to all conversions, so I'll give it to you here:

function q = MakeQFromR(R)
if (R(3,3) < 0)
if (R(1,1) >R(2,2))
t = 1 + R(1,1) -R(2,2) -R(3,3);
q = [t, R(1,2)+R(2,1), R(3,1)+R(1,3), R(2,3)-R(3,2)];
else
t = 1 -R(1,1) + R(2,2) -R(3,3);
q = [R(1,2)+R(2,1), t, R(2,3)+R(3,2), R(3,1)-R(1,3)];
end
else
if (R(1,1) < -R(2,2))
t = 1 -R(1,1) -R(2,2) + R(3,3);
q = [R(3,1)+R(1,3), R(2,3)+R(3,2), t, R(1,2)-R(2,1)];
else
t = 1 + R(1,1) + R(2,2) + R(3,3);
q = [R(2,3)-R(3,2), R(3,1)-R(1,3), R(1,2)-R(2,1), t];
end
end
q = q * 0.5 / sqrt(t);
end

• Thanks a lot Chuck, i found the error. I'm a chicken, the matrix Rd is not a rotation matrix ( its determinant is equal -1). I'm sorry for your lost time! Thanks a lot again!
– AleB
Commented Feb 28, 2023 at 23:20