1
$\begingroup$

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!

$\endgroup$

2 Answers 2

1
$\begingroup$

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!

$\endgroup$
0
$\begingroup$

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
$\endgroup$
1
  • $\begingroup$ 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! $\endgroup$
    – AleB
    Commented Feb 28, 2023 at 23:20

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.