Contents
clc clear all close all
This example does an animation of a two link manipulator with two rotational joint
Specify parameters and get transformation matrices
%D-H for link 1 a1 = 1; alpha1 = 0; d1=0; theta1 = pi/2; A01 = DH(a1,alpha1,d1,theta1); %A^0_1 %D-H for link2 a2 = 1; alpha2 = 0; d2=0; theta2 = pi/4; A12 = DH(a2,alpha2,d2,theta2); %A^1_2
Get locations of joints
%Location of joint 1 endOfLink1 = A01(1:3,4); %Location of joint 2 A02 = A01*A12; endOfLink2 = A02(1:3,4); %end-effector position and orientation. position_of_end_effector = A02(1:3,4) orientation_of_end_effector = A02(1:3,1:3)
Draw lines from one joint to another
%Draw line from origin to end of link 1 line([0 endOfLink1(1)],[0 endOfLink1(2)],[0 endOfLink1(3)],.... 'LineWidth',5,'Color','red'); %Draw line from end of link 1 to end of link 2 line([endOfLink1(1) endOfLink2(1)],... [endOfLink1(2) endOfLink2(2)],... [endOfLink1(3) endOfLink2(3)],... 'LineWidth',5,'Color','blue'); xlabel('x'); ylabel('y'); zlabel('z'); grid on; %if you want the grid to show up. axis('equal'); %make the axis equal, to avoid scaling effect % These set the x and y limits for the axis (will need adjustment) xlim([-2 2]); ylim([-2 2]); zlim([-2 2]);
position_of_end_effector =
-0.7071
1.7071
0
orientation_of_end_effector =
-0.7071 -0.7071 0
0.7071 -0.7071 0
0 0 1.0000
