How to compute orientation error for a Puma end effector

Question

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!

Answer 1

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!

Answer 2

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