Modern Robotics - Course 2 Module1: Chapter 4 Forward Kinematics - Graded Assignment 答案

    最近在Coursera上Modern Robotics的课，在做Chapter 4 Forward Kinematics的Graded Assignment的时候，发现在网上没有答案，部分习题看了很久的书才做出来，因此写这篇文章给有需要的同学参考。

The URRPR spatial open chain robot is shown below in its zero position

Q1

     For L = 1 determine the end-effector zero configuration M. The maximum allowable error for any number is 0.01, so give enough decimal places where necessary

答案：[[1,0,0,3.73],[0,1,0,0],[0,0,1,2.73],[0,0,0,1]]

答案为frame {b}在frame {0}下的坐标，直接写出即可

Q2

    Referring back to Question 1, determine the screw axes Si​ in {0} when the robot is in its zero position. Again L=1. Give the axes as a 6x6 matrix with the form [S1,S2,…,S6], i.e., each column is a screw axis. The maximum allowable error for any number is 0.01, so give enough decimal places where necessary.

答案：[[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,0,1,-0.73,0,0],[-1,0,0,0,0,-3.73],[0,1,2.73,3.73,1,0]]

解题步骤（Matlab代码）

   在此题中，虽然旋转轴和线速度可以通过观察简单地写出，但这里还是基于概念用代数的方法进行计算，加深理解。

Step 1.写出各个joint在frame {0}下的坐标

%The p_r_b denotes the coorrdinate of joint number_r in frame {0}
p_1_s = [1 0 0]';
p_2_s = [1 0 0]';
p_3_s = [sqrt(3)+1 0 -1]';
p_4_s = [sqrt(3)+2 0 sqrt(3)-1]';
p_6_s = [2+sqrt(3) 0 sqrt(3)+1]';

Step 2.写出各个joint的screw axis

%The w_r_b denotes the screw axis of roation number_r in frame {s}
w_1_s = [0 0 1]';
w_2_s = [0 1 0]';
w_3_s = [0 1 0]';
w_4_s = [0 1 0]';
w_6_s = [0 0 1]';

Step 3.在教学视频里，有

S_i = - [w_i] * p_i

使用该公式计算S_s

%Use v = -w x p to calculate the linear velocity (参考书pp.99)
v_1_s = -VecToso3(w_1_s)*p_1_s;
S_1_s = [w_1_s;v_1_s];
v_2_s = -VecToso3(w_2_s)*p_2_s;
S_2_s = [w_2_s;v_2_s];
v_3_s = -VecToso3(w_3_s)*p_3_s;
S_3_s = [w_3_s;v_3_s];
v_4_s = -VecToso3(w_4_s)*p_4_s;
S_4_s = [w_4_s;v_4_s];
%注意，S_5为Prismatic joint,方向为[0 0 1]，因此S_5可以直接写出
S_5_s = [0 0 0 0 0 1]';
v_6_s = -VecToso3(w_6_s)*p_6_s;
S_6_s = [w_6_s;v_6_s];
S_s = [S_1_s S_2_s S_3_s S_4_s S_5_s S_6_s]

得到

Q3

    Referring back to Question 1, determine the screw axes Bi​ in {0} when the robot is in its zero position. Again L=1. Give the axes as a 6x6 matrix with the form [B1,B2,…,B6]. The maximum allowable error for any number is 0.01, so give enough decimal places where necessary.

答案：[[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,2.73,3.73,2,0,0],[2.73,0,0,0,0,0],[0,-2.73,-1,0,1,0]]

解题方法和Q2类似

Matlab代码

%The p_r_b denotes the coorrdinate of point number_r in frame {b}
p_1_b = [-(sqrt(3)+1) 0 -(sqrt(3)+1)]';
p_2_b = [-(sqrt(3)+1) 0 -(sqrt(3)+1)]';
p_3_b = [-1 0 -(sqrt(3)+2)]';
p_4_b = [0 0 -2]';
p_6_b = [0 0 0]';
%The w_r_b denotes the screw axis of roation number_r in frame {b}
w_1_b = [0 0 1]';
w_2_b = [0 1 0]';
w_3_b = [0 1 0]';
w_4_b = [0 1 0]';
w_6_b = [0 0 1]';
% 使用v = -[w] * p计算线速度
v_1_b = -VecToso3(w_1_b)*p_1_b;
S_1_b = [w_1_b;v_1_b];
v_2_b = -VecToso3(w_2_b)*p_2_b;
S_2_b = [w_2_b;v_2_b];
v_3_b = -VecToso3(w_3_b)*p_3_b;
S_3_b = [w_3_b;v_3_b];
v_4_b = -VecToso3(w_4_b)*p_4_b;
S_4_b = [w_4_b;v_4_b];
S_5_b = [0 0 0 0 0 1]';
v_6_b = -VecToso3(w_6_b)*p_6_b;
S_6_b = [w_6_b;v_6_b];
S_b = [S_1_b S_2_b S_3_b S_4_b S_5_b S_6_b]

得到

Q4

     Referring back to Question 1 and 2, given L = 1 and joint variable values θ=(−π/2,π/2,π/3,−π/4,1,π/6), use the function FKinSpace in the given software to find the end-effector configuration T∈SE(3). The maximum allowable error for any number is 0.01, so give enough decimal places where necessary.

答案：[[0.50,0.86,0,1],[0.22,-0.13,-0.97,-1.90],[-0.84,0.48,-0.26,-4.51],[0,0,0,1]]

Matlab代码

此题需要用到Q2得出的S_s，注意FKinSpace函数中thetalist的参数需为Vector！Vector！

M = [1 0 0 (1+sqrt(3)+1);0 1 0 0;0 0 1 (sqrt(3)+2-1);0 0 0 1];
thetalist = [-pi/2 pi/2 pi/3 -pi/4 1 pi/6]'; %转置为Vector
T = FKinSpace(M,S_s,thetalist)

Q5

    Referring back to Question 1 and 2, given L = 1 and joint variable values θ=(−π/2,π/2,π/3,−π/4,1,π/6), use the function FKinBody in the given software to find the end-effector configuration T∈SE(3). The maximum allowable error for any number is 0.01, so give enough decimal places where necessary.

答案：[[0.50,0.86,0,1],[0.22,-0.13,-0.97,-1.90],[-0.84,0.48,-0.26,-4.51],[0,0,0,1]]

此题需要用到Q3得出的S_b，注意FKinBody函数中thetalist的参数也是Vector！

Matlab代码

M = [1 0 0 (1+sqrt(3)+1);0 1 0 0;0 0 1 (sqrt(3)+2-1);0 0 0 1];
thetalist = [-pi/2 pi/2 pi/3 -pi/4 1 pi/6]';
T = FKinBody(M,S_b,thetalist)