DH parameters
find common normal of Zi and Zi+1 .
place new X axis
Xi+1 along this common normal.d is the depth from the origin of Zi to the common normal along the Zi+1 axis
θ rotates about previous Z axis to align its
X with the new origina is the distance along the rotated
X axis.α rotates about the new X axis to put
Z in its desired orientation.
Special Case: two
Z
axes are parallel.
choose any convenient
example
example
d | θ | a | α |
---|---|---|---|
3 | θ1 | 0 | −π/2 |
0 | - π/2 + θ2 | 5.75 | 0 |
0 | θ3 + π/2 | 3.375 | 0 |
0 | θ4 | 0 | - π/2 |
4.125 | θ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