參考資料：

Robot Dynamics Lecture Notes：Robitics System Lab，ETH Zurich，HS 2017

Exercises 1

Matlab Coding 1

• quatMult(q_AB,q_BC) 兩個四元數相乘
• quatToRotMat(q) 四元數轉換為旋轉矩陣
• rotMatToQuat(R) 旋轉矩陣轉換為四元數
• rotVecWithQuat(q_BA,A_r) 用四元數將r從A轉換到B
• rotMatToRotVec(C) 旋轉矩陣轉換為旋轉向量

function q_AC = quatMult(q_AB,q_BC)
  % Input: two quaternions to be multiplied
  % Output: output of the multiplication

  s = q_AB(1); v = q_AB(2:4);
  t = q_BC(1); u = q_BC(2:4);
  q_AC = [s*t-v'*u;s*u+t*v+cross(v,u)];
end
 

function R = quatToRotMat(q)
  % Input: quaternion [w x y z]
  % Output: corresponding rotation matrix
  s = q(1);
  v = q(2:4);
  R = (2*s*s-1)*eye(3) + 2*s*skew(v)+2*v*v';
end

function q = rotMatToQuat(R)
  % Input: rotation matrix
  % Output: corresponding quaternion [w x y z]
  
  % 勿忘1/2！！！！！！！！！！！！！
  q = 0.5*[sqrt(1+trace(R));
  sign(R(3,2)-R(2,3))*sqrt(R(1,1)-R(2,2)-R(3,3)+1);
  sign(R(1,3)-R(3,1))*sqrt(-R(1,1)+R(2,2)-R(3,3)+1);
  sign(R(2,1)-R(1,2))*sqrt(-R(1,1)-R(2,2)+R(3,3)+1);
  ];
end
 

function B_r = rotVecWithQuat(q_BA,A_r)
  % Input: the orientation quaternion and the coordinate of the vector to be mapped
  % Output: the coordinates of the vector in the target frame
  
  % PLACEHOLDER FOR OUTPUT -> REPLACE WITH SOLUTION
  R_BA = quatToRotMat(q_BA);
  B_r = R_BA*A_r;
end

function phi = rotMatToRotVec(C)
% Input: a rotation matrix C
% Output: the rotational vector which describes the rotation C

th = real(acos(0.5*(C(1,1)+C(2,2)+C(3,3)-1)));

if (abs(th)<eps)               % prevent division by 0 in 1/(2*sin(th))
    n = zeros(3,1);
else
    n = 1/(2*sin(th))*[C(3,2) - C(2,3);
                       C(1,3) - C(3,1);
                       C(2,1) - C(1,2)];
end
phi = th*n;
end
 

Exercises 2