[robot] review forward kinematics

DH parameters

1. find common normal of $Z_{i}$ and $Z_{i+1}$.

2. place new $X$ axis $X_{i+1}$ along this common normal.

3. $d$ is the depth from the origin of $Z_{i}$ to the common normal along the $Z_{i+1}$ axis

4. $\theta$ rotates about previous $Z$ axis to align its $X$ with the new origin

5. $a$ is the distance along the rotated $X$ axis.

6. $\alpha$ rotates about the new $X$ axis to put $Z$ in its desired orientation.

Special Case: two $Z$ axes are parallel.
choose any convenient $d$.

example

example

d	$\theta$	a	$\alpha$
3	$\theta_{1}$	0	$-\pi/2$
0	-$\pi/2$+$\theta_{2}$	5.75	0
0	$\theta_{3}$+$\pi/2$	3.375	0
0	$\theta_{4}$	0	-$\pi/2$
4.125	$\theta_{5}$	0	0

function [ pos ] = arm_forward_kinematics( theta1, theta2, theta3, theta4, theta5, g )
%The input to the function will be the joint
%    angles of the robot in radians, and the distance between the gripper pads in inches.
%    The output must contain 10 positions of various points along the robot arm as specified
%    in the question.
    % parameters
    a = 3;
    b = 5.75;
    c = 7.375;
    d = 4.125;
    e = 1.125;
    pos = zeros(10, 4);
    % transformation matrix A1
    A1 = compute_dh_matrix(0, -0.5*pi, a, theta1);

    % transformation matrix A2
    A2 = compute_dh_matrix(b, 0, 0, theta2-0.5*pi);
    % transformation matrix A4
    A3 = compute_dh_matrix(c, 0, 0, theta3+0.5*pi);
    % transformation matrix A4
    A4 = compute_dh_matrix(0, -0.5*pi, 0, theta4-0.5*pi);
    % transformation matrix A5
   %A5 = compute_dh_matrix(0, 0, d, theta5);
    pos(1,4) = 1;
    value = A1*([0,0,0,1]');
    pos(2,:) = value;
    value = A1*A2*([0,0,0,1]');
    pos(3,:) = value;
    value = A1*A2*A3*([0,0,0,1]');
    pos(4,:) = value;
    value = A1*A2*A3*A4*([0,0,0,1]');
    pos(5,:) = value;
    %
    A = A1*A2*A3*A4;
    %temp = A*([c+d,0,a+b,1]');
    temp = A * compute_dh_matrix(0,0,d-e,theta5);
    pos(6,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(0.5*g, 0, d-e,theta5);
    pos(7,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(-0.5*g, 0, d-e,theta5);
    pos(8,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(0.5*g, 0, d,theta5);
    pos(9,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(-0.5*g, 0, d,theta5);
    pos(10,:) =  temp*[0,0,0,1]';
    pos = pos(:,1:3);
end

function A = compute_dh_matrix(a, alpha, d, theta)
%% standard DH parameters
ctheta = cos(theta);
stheta = sin(theta);
salpha = sin(alpha);
calpha = cos(alpha);
A = [ctheta, -stheta*calpha, stheta*salpha, a*ctheta;
    stheta, ctheta*calpha, -ctheta*salpha, a*stheta;
    0,salpha, calpha,d;
    0,0,0,1];
end

example

function A = compute_dh_matrix(a, alpha, d, theta)
%% standard DH parameters
ctheta = cos(theta);
stheta = sin(theta);
salpha = sin(alpha);
calpha = cos(alpha);
A = [ctheta,-stheta,0,a;
    stheta*calpha,ctheta*calpha,-salpha,-salpha*d;
    stheta*salpha,ctheta*salpha,calpha,calpha*d;
    0,0,0,1];
end
>> theta1 = 15/180*pi;
>> theta2=-40/180*pi;
>> theta3=-50/180*pi;
>> theta4=pi/6;
>> theta5=70/180*pi;
>> theta5=25/180*pi;
>> theta5=70/180*pi;
>> theta6=25/180*pi;
>> t1 = compute_dh_matrix(0,0,0,theta1);
>> t2=compute_dh_matrix(0,-pi/2,220,theta2);
>> t3=compute_dh_matrix(430,0,-90,theta3);
>> t4=compute_dh_matrix(0,-pi/2,430,theta4);
>> t5=compute_dh_matrix(0,pi/2,0,theta5);
>> t6=compute_dh_matrix(0,-pi/2,0,theta6);
>> t1*t2*t3*t4 % frame{4} 下的原点在frame{0}下的坐标[699.88,322.12,276.40]

ans =

    0.1294    0.2241    0.9659  699.8767
   -0.4830   -0.8365    0.2588  322.1173
    0.8660   -0.5000   -0.0000  276.3987
         0         0         0    1.0000

>> t1*t2*t3*t4*t5*t6

ans =

    0.9575   -0.1992    0.2088  699.8767
   -0.2828   -0.7911    0.5424  322.1173
    0.0571   -0.5783   -0.8138  276.3987
         0         0         0    1.0000