【笔记】Robot Dynamics

参考资料：

• Robot Dynamics Lecture Notes：Robitics System Lab，ETH Zurich，HS 2017
• Robotics Dynamics Matlab Exercises from TA Teams

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
  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

Matlab Coding 2

• jointToTransformij(q) 获得各个关节之间的变换矩阵
• jointToPosition(q) 获得末端执行器的X_e
• jointToRotMat(q) 获得末端执行器的X_R
• jointToRotJac(q) 获得旋转几何雅克比矩阵JeoR
• jointToPosJac(q) 获得位置几何雅克比矩阵JeoP

function TI0 = getTransformI0()
  % Input: void
  % Output: homogeneous transformation Matrix from frame 0 to the inertial frame I. T_I0
  TI0 = eye(4);
end

function T6E = getTransform6E()
  % Input: void
  % Output: homogeneous transformation Matrix from the end-effector frame E to frame 6. T_6E
  T6E = eye(4);
end

function T01 = jointToTransform01(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 1 to frame 0. T_01
  T01 = [cos(q(1)), -sin(q(1)), 0,0;
         sin(q(1)), cos(q(1)), 0, 0;
         0, 0, 1, 145/1000;
         0, 0, 0, 1];
end

function T12 = jointToTransform12(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 2 to frame 1. T_12
  T12 = [cos(q(2)),0,sin(q(2)),0;
      0,1,0,0;
      -sin(q(2)),0,cos(q(2)),0.145;
      0,0,0,1];
end

function T23 = jointToTransform23(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 3 to frame 2. T_23
  T23 = [cos(q(3)),0,sin(q(3)),0;
      0,1,0,0;
      -sin(q(3)),0,cos(q(3)),0.270;
      0,0,0,1];
end

function T34 = jointToTransform34(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 4 to frame 3. T_34
  T34 = [1,0,0,0.134;
      0, cos(q(4)),-sin(q(4)),0;
      0, sin(q(4)),cos(q(4)),0.070;
      0,0,0,1];
end

function T45 = jointToTransform45(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 5 to frame 4. T_45
  T45 = [cos(q(5)),0,sin(q(5)),0.168;
      0,1,0,0;
      -sin(q(5)),0,cos(q(5)),0;
      0,0,0,1];
end

function T56 = jointToTransform56(q)
  % Input: joint angles
  % Output: homogeneous transformation Matrix from frame 6 to frame 5. T_56
  T56 = [1,0,0,0.072;
      0, cos(q(6)),-sin(q(6)),0;
      0, sin(q(6)), cos(q(6)),0;
      0,0,0,1];
end

function I_r_IE = jointToPosition(q)
  % Input: joint angles
  % Output: position of end-effector w.r.t. inertial frame. I_r_IE
  T_IE = getTransformI0()*jointToTransform01(q)*jointToTransform12(q)*jointToTransform23(q)*jointToTransform34(q)*jointToTransform45(q)*jointToTransform56(q)*getTransform6E();
  I_r_IE = T_IE(1:3,4);
end

function C_IE = jointToRotMat(q)
  % Input: joint angles
  % Output: rotation matrix which projects a vector defined in the
  % end-effector frame E to the inertial frame I, C_IE.
  T_IE = getTransformI0()*jointToTransform01(q)*jointToTransform12(q)*jointToTransform23(q)*jointToTransform34(q)*jointToTransform45(q)*jointToTransform56(q)*getTransform6E();
  C_IE = T_IE(1:3,1:3);
end

function J_R = jointToRotJac(q)
  % Input: vector of generalized coordinates (joint angles)
  % Output: Jacobian of the end-effector orientation which maps joint
  % velocities to end-effector angular velocities in I frame.

  % Compute the relative homogeneous transformation matrices.
  T_I0 = getTransformI0();
  T_01 = jointToTransform01(q);
  T_12 = jointToTransform12(q);
  T_23 = jointToTransform23(q);
  T_34 = jointToTransform34(q);
  T_45 = jointToTransform45(q);
  T_56 = jointToTransform56(q);
 
  % Compute the homogeneous transformation matrices from frame k to the
  % inertial frame I.
  T_I1 = T_I0*T_01;
  T_I2 = T_I1*T_12;
  T_I3 = T_I2*T_23;
  T_I4 = T_I3*T_34;
  T_I5 = T_I4*T_45;
  T_I6 = T_I5*T_56;
 
  % Extract the rotation matrices from each homogeneous transformation matrix.
  R_I1 = T_I1(1:3,1:3);
  R_I2 = T_I2(1:3,1:3);
  R_I3 = T_I3(1:3,1:3);
  R_I4 = T_I4(1:3,1:3);
  R_I5 = T_I5(1:3,1:3);
  R_I6 = T_I6(1:3,1:3);
 
  % Define the unit vectors around which each link rotates in the precedent
  % coordinate frame.
  n_1 = [0 0 1]';
  n_2 = [0 1 0]';
  n_3 = [0 1 0]';
  n_4 = [1 0 0]';
  n_5 = [0 1 0]';
  n_6 = [1 0 0]';
  % Compute the rotational jacobian.
  J_R = [ R_I1*n_1 R_I2*n_2 R_I3*n_3 R_I4*n_4 R_I5*n_5 R_I6*n_6];

end

function J_P = jointToPosJac(q)
  % Input: vector of generalized coordinates (joint angles)
  % Output: Jacobian of the end-effector translation which maps joint
  % velocities to end-effector linear velocities in I frame.
  
  % Compute the relative homogeneous transformation matrices.
  T_I0 = getTransformI0();
  T_01 = jointToTransform01(q);
  T_12 = jointToTransform12(q);
  T_23 = jointToTransform23(q);
  T_34 = jointToTransform34(q);
  T_45 = jointToTransform45(q);
  T_56 = jointToTransform56(q);
  
  % Compute the homogeneous transformation matrices from frame k to the
  % inertial frame I.
  T_I1 = T_I0*T_01;
  T_I2 = T_I1*T_12;
  T_I3 = T_I2*T_23;
  T_I4 = T_I3*T_34;
  T_I5 = T_I4*T_45;
  T_I6 = T_I5*T_56;
  
  % Extract the rotation matrices from each homogeneous transformation
  % matrix.
  R_I1 = T_I1(1:3,1:3);
  R_I2 = T_I2(1:3,1:3);
  R_I3 = T_I3(1:3,1:3);
  R_I4 = T_I4(1:3,1:3);
  R_I5 = T_I5(1:3,1:3);
  R_I6 = T_I6(1:3,1:3);
  
  % Extract the position vectors from each homogeneous transformation
  % matrix.
  r_I_I1 = T_I1(1:3,4);
  r_I_I2 = T_I2(1:3,4);
  r_I_I3 = T_I3(1:3,4);
  r_I_I4 = T_I4(1:3,4);
  r_I_I5 = T_I5(1:3,4);
  r_I_I6 = T_I6(1:3,4);
  
  % Define the unit vectors around which each link rotates in the precedent
  % coordinate frame.
  n_1 = [0 0 1]';
  n_2 = [0 1 0]';
  n_3 = [0 1 0]';
  n_4 = [1 0 0]';
  n_5 = [0 1 0]';
  n_6 = [1 0 0]';
  
  % Compute the end-effector position vector.
  r_I_IE = jointToPosition_solution(q);
  
  % Compute the translational jacobian.
  J_P = [   cross(R_I1*n_1, r_I_IE - r_I_I1) ...
            cross(R_I2*n_2, r_I_IE - r_I_I2) ...
            cross(R_I3*n_3, r_I_IE - r_I_I3) ...
            cross(R_I4*n_4, r_I_IE - r_I_I4) ...
            cross(R_I5*n_5, r_I_IE - r_I_I5) ...
            cross(R_I6*n_6, r_I_IE - r_I_I6) ...
        ];
  
end

Exercises3

Matlab Coding 3

• pseudoInverseMat(A,lambda) 获得A的Moore-Penrose pseudoinverse （Matlab自带命令：pinv(A)）
• q = inverseKinematics(I_r_IE_des, C_IE_des, q_0, tol) 用数值方法求解运动学逆解

function [ pinvA ] = pseudoInverseMat(A, lambda)
% Input: Any m-by-n matrix, and a damping factor.
% Output: An n-by-m pseudo-inverse of the input according to the Moore-Penrose formula

% Get the number of rows (m) and columns (n) of A
[m,n] = size(A);

% Compute the pseudo inverse for both left and right cases
if (m>n)
  % Compute the left pseudoinverse.
  pinvA = (A'*A + lambda*lambda*eye(n,n))A';
elseif (m<=n)
  % Compute the right pseudoinverse.
  pinvA = A'/(A*A' + lambda*lambda*eye(m,m));
end
end

function [ q ] = inverseKinematics(I_r_IE_des, C_IE_des, q_0, tol)
% Input: desired end-effector position, desired end-effector orientation (rotation matrix),
%        initial guess for joint angles, threshold for the stopping-criterion
% Output: joint angles which match desired end-effector position and orientation


% 0. Setup
it = 0;
max_it = 100;       % Set the maximum number of iterations. 
lambda = 0.001;     % Damping factor
alpha = 0.5;        % Update rate

close all;
loadviz;

% 1. start configuration
q = q_0;

% 2. Iterate until terminating condition.
while (it==0 || (norm(dxe)>tol && it < max_it))
    % 3. evaluate Jacobian for current q
    I_J = [jointToPosJac(q);jointToRotJac(q)];
    
    % 4. Update the psuedo inverse
    I_J_pinv = pseudoInverseMat(I_J,lambda);
    
    % 5. Find the end-effector configuration error vector
    % position error
    dr = I_r_IE_des - jointToPosition(q); 
    % rotation error
    C_IE = jointToRotMat(q);
    dph = C_IE * rotMatToRotVec(C_IE'*C_IE_des);

% pose error
    dxe = [dr;dph];
    
    % 6. Update the generalized coordinates
    q = q + alpha*I_J_pinv*dxe;
     
    % Update robot
    abbRobot.setJointPositions(q);
    drawnow;
    pause(0.1);
    
    it = it+1;
end

% Get final error (as for 5.)
% position error
dr = I_r_IE_des - jointToPosition(q);
% rotation error
 C_IE = jointToRotMat(q);
 dph = C_IE * rotMatToRotVec(C_IE'*C_IE_des);

fprintf('Inverse kinematics terminated after %d iterations.
', it);
fprintf('Position error: %e.
', norm(dr));
fprintf('Attitude error: %e.
', norm(dph));
end