# Kinematics and trajectory planning simulation of PUMA 560 based on Matlab

Posted by wrong_move18 on Tue, 30 Jun 2020 08:33:44 +0200

# Kinematics analysis of PUMA560

Reference textbook: Cai Zixing, the third edition of Robotics # PUMA560 modeling and simulation matlab code

## matlab Toolbox: robotics toolbox

② Open matlab, click Set path - > > add and include subfolders, then select the "rvctools" folder, and finally save - > > close

③ Open the startup of the "rvctool" folder_ RVC. M runs, and the roboticstoolbox is installed. Or enter startup rvc on the matlab command line to install.

④ matlab command line input ver to see whether to install

``````>>ver
MATLAB edition: 9.5.0.944444 (R2018b)
Robotics Toolbox for MATLAB                           Version 10.3.1
``````

## Program description

• According to the formula derivation in the textbook, four groups of solutions of the robot can be calculated, and then the other four groups of solutions can be obtained according to the "turnover" of wrist joint. (8 groups of solutions in total). In the process of planning, only one group of solutions is used, and the optimal solution is not selected. (the rest of the solutions have been commented out by me. If you are interested, you can try the other solutions 👏)

## main program

``````% In trajectory planning, firstly, the robot model is established, 6 R Robot model, name modified puma560.
% Defining robots a--Connecting rod length, d--Connecting rod offset
a2=0.4318;a3=0.02032;d2=0.14909;d4=0.43307;
%			 thetai    di      ai-1        alphai-1
L1 = Link([pi/2    0       0               0], 'modified');
L2 = Link([0       d2      0           -pi/2], 'modified');
L3 = Link([-pi/2   0       a2              0], 'modified');
L4 = Link([0       d4      a3          -pi/2], 'modified');
L5 = Link([0       0  	   0            pi/2], 'modified');
L6 = Link([0       0       0           -pi/2], 'modified');
robot.name = 'modified puma560';
%   robot.display();
%	robot.teach();
% Define initial joint angle in trajectory planning( First_Theta)And end joint angle( Final_Theta),The number of steps is 777.
% First_Theta = [0     pi/2  -pi/2   0       0     0];%Ready state
% Final_Theta = [0     pi/4    pi    0       pi/4  0];%Dexterous state
% 6 Angle change
First_Theta = [0     pi/2  -pi/2   0       0     0];
Final_Theta = [pi/6  pi/4    pi    pi/3   pi/4  pi/2];
% jtraj Trajectory planning of function joint angle space
step = 777;
[q,qd,qdd] = jtraj(First_Theta,Final_Theta,step);

%The plane is divided into two*4=8 Sub drawing interval, a total of two lines, each line of four
%Position information of the first sub picture on the first line.
subplot(2,4,1);
i = 1:6;
plot(q(:,i)); grid on;
title('position');
%In the first line, the second sub picture speed information.
subplot(2,4,2);
i = 1:6;
plot(qd(:,i));grid on;
title('speed');
%In the second line, the first sub picture acceleration information.
subplot(2,4,5);
i = 1:6;
plot(qdd(:,i));grid on;
title('acceleration');

%according to First_Theta and Final_Theta The starting and ending pose matrices are obtained.
%Using the self-contained function to solve
%     T0 = robot.fkine(First_Theta);
%     Tf = robot.fkine(Final_Theta);
%According to the improvement DH The self-made function of the model, kinematics Forward kinematics solution
T0=kinematics(First_Theta);
T0=SE3(T0);
Tf=kinematics(Final_Theta);
Tf=SE3(Tf);
%utilize ctraj Planning trajectories in Cartesian space.
Tc = ctraj(T0,Tf,step);
%The moving variable is extracted from the homogeneous rotation matrix, which is equivalent to the position of points in Cartesian coordinate system.
Tjtraj = transl(Tc);
%On the second line, the second sub picture p1 reach p2 Straight track.
subplot(2,4,6);
plot2(Tjtraj,'r');grid on;
title('T0 reach Tf Straight track');

%     hold on;
%Three or four subgraphs in the first row and three or four subgraphs in the second row are equivalent to drawing the entire right half
subplot(2,4,[3,4,7,8]);

for Var = 1:777
T1=Tc(1,Var);
T2=T1.T;
% Inverse_kinematics Inverse kinematics solution
qq(:,Var) = Inverse_kinematics(T2);
end
plot2(Tjtraj,'r');
robot.plot(qq');
``````

## Kinematics function (forward kinematics)

``````function T0_6=kinematics(theta1_6)
a2=0.4318;a3=0.02032;d2=0.14909;d4=0.43307;
theta1=theta1_6(1);
theta2=theta1_6(2);
theta3=theta1_6(3);
theta4=theta1_6(4);
theta5=theta1_6(5);
theta6=theta1_6(6);
c1=cos(theta1);s1=sin(theta1);
c2=cos(theta2);s2=sin(theta2);
c3=cos(theta3);s3=sin(theta3);
c4=cos(theta4);s4=sin(theta4);
c5=cos(theta5);s5=sin(theta5);
c6=cos(theta6);s6=sin(theta6);
%6 Transformation matrix of connecting rod
T1=[c1 -s1 0 0;s1 c1 0 0;0 0 1 0;0 0 0 1];
T2=[c2 -s2 0 0;0 0 1 d2;-s2 -c2 0 0;0 0 0 1];
T3=[c3 -s3 0 a2;s3 c3 0 0 ;0 0 1 0;0 0 0 1];
T4=[c4 -s4 0 a3;0 0 1 d4;-s4 -c4 0 0;0 0 0 1];
T5=[c5 -s5 0 0 ;0 0 -1 0;s5 c5 0 0;0 0 0 1];
T6=[c6 -s6 0 0 ;0 0 1 0;-s6 -c6 0 0;0 0 0 1];
%Forward kinematics equation
T0_6 = T1*T2*T3*T4*T5*T6;
end
``````

## Inverse_kinematics function (inverse kinematics)

``````function theta_Med=Inverse_kinematics(T)
% a--Connecting rod length, d--Connecting rod offset
a2=0.4318;a3=0.02032;d2=0.14909;d4=0.43307;

nx=T(1,1); ny=T(2,1); nz=T(3,1);
ox=T(1,2); oy=T(2,2); oz=T(3,2);
ax=T(1,3); ay=T(2,3); az=T(3,3);
px=T(1,4); py=T(2,4); pz=T(3,4);

% For the convenience of calculation, the m Series vector
% Solving joint angle 1
theta1_1 = atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2));
theta1_2 = atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2));
% Solving joint angle 3
m3_1 = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);
theta3_1 = atan2(a3,d4)-atan2(m3_1,sqrt(a3^2+d4^2-m3_1^2));
theta3_2 = atan2(a3,d4)-atan2(m3_1,-sqrt(a3^2+d4^2-m3_1^2));
% Solving joint angle 2
ms2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*...
(a2*sin(theta3_1)-d4);
mc2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*...
(a2*cos(theta3_1)+a3);
theta23_1 = atan2(ms2_1,mc2_1);
theta2_1 = theta23_1 - theta3_1;

ms2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*...
(a2*sin(theta3_1)-d4);
mc2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*...
(a2*cos(theta3_1)+a3);
theta23_2 = atan2(ms2_2,mc2_2);
theta2_2 = theta23_2 - theta3_1;

ms2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*...
(a2*sin(theta3_2)-d4);
mc2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*...
(a2*cos(theta3_2)+a3);
theta23_3 = atan2(ms2_3,mc2_3);
theta2_3 = theta23_3 - theta3_2;

ms2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*...
(a2*sin(theta3_2)-d4);
mc2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*...
(a2*cos(theta3_2)+a3);
theta23_4 = atan2(ms2_4,mc2_4);
theta2_4 = theta23_4 - theta3_2;
% Solving joint angle 4
ms4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);
mc4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*...
cos(theta23_1)+az*sin(theta23_1);
theta4_1=atan2(ms4_1,mc4_1);

ms4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);
mc4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*...
cos(theta23_2)+az*sin(theta23_2);
theta4_2=atan2(ms4_2,mc4_2);

ms4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);
mc4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*...
cos(theta23_3)+az*sin(theta23_3);
theta4_3=atan2(ms4_3,mc4_3);

ms4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);
mc4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*...
cos(theta23_4)+az*sin(theta23_4);
theta4_4=atan2(ms4_4,mc4_4);
% Solving joint angle 5
ms5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+...
sin(theta1_1)*sin(theta4_1))-...
ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))...
+az*(sin(theta23_1)*cos(theta4_1));
mc5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))...
+az*(-cos(theta23_1));
theta5_1=atan2(ms5_1,mc5_1);

ms5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+...
sin(theta1_2)*sin(theta4_2))-...
ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))...
+az*(sin(theta23_2)*cos(theta4_2));
mc5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))...
+az*(-cos(theta23_2));
theta5_2=atan2(ms5_2,mc5_2);

ms5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+...
sin(theta1_1)*sin(theta4_3))-...
ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))...
+az*(sin(theta23_3)*cos(theta4_3));
mc5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))...
+az*(-cos(theta23_3));
theta5_3=atan2(ms5_3,mc5_3);

ms5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+...
sin(theta1_2)*sin(theta4_4))-...
ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))...
+az*(sin(theta23_4)*cos(theta4_4));
mc5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))...
+az*(-cos(theta23_4));
theta5_4=atan2(ms5_4,mc5_4);
% Solving joint angle 6
ms6_1=-nx*(cos(theta1_1)*cos(theta23_1)*...
sin(theta4_1)-sin(theta1_1)*cos(theta4_1))...
-ny*(sin(theta1_1)*cos(theta23_1)*...
sin(theta4_1)+cos(theta1_1)*cos(theta4_1))...
+nz*(sin(theta23_1)*sin(theta4_1));
mc6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)...
+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)...
-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)...
+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)...
+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)...
-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)...
-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)...
+cos(theta23_1)*sin(theta5_1));
theta6_1=atan2(ms6_1,mc6_1);

ms6_2=-nx*(cos(theta1_2)*cos(theta23_2)*...
sin(theta4_2)-sin(theta1_2)*cos(theta4_2))...
-ny*(sin(theta1_2)*cos(theta23_2)*...
sin(theta4_2)+cos(theta1_2)*cos(theta4_2))...
+nz*(sin(theta23_2)*sin(theta4_2));
mc6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)...
+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)...
-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)...
+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)...
+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)...
-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)...
-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)...
+cos(theta23_2)*sin(theta5_2));
theta6_2=atan2(ms6_2,mc6_2);

ms6_3=-nx*(cos(theta1_1)*cos(theta23_3)*...
sin(theta4_3)-sin(theta1_1)*cos(theta4_3))...
-ny*(sin(theta1_1)*cos(theta23_3)*...
sin(theta4_3)+cos(theta1_1)*cos(theta4_3))...
+nz*(sin(theta23_3)*sin(theta4_3));
mc6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)...
+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)...
-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)...
+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)...
+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)...
-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)...
-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)...
+cos(theta23_3)*sin(theta5_3));
theta6_3=atan2(ms6_3,mc6_3);

ms6_4=-nx*(cos(theta1_2)*cos(theta23_4)*...
sin(theta4_4)-sin(theta1_2)*cos(theta4_4))...
-ny*(sin(theta1_1)*cos(theta23_4)*...
sin(theta4_4)+cos(theta1_2)*cos(theta4_4))...
+nz*(sin(theta23_4)*sin(theta4_4));
mc6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)...
+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)...
-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)...
+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)...
+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)...
-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)...
-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)...
+cos(theta23_4)*sin(theta5_4));
theta6_4=atan2(ms6_4,mc6_4);

% Four groups of nonsingular inverse kinematics solutions are obtained
theta_Med_1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;
%theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2;
%theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3;
%theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4;
];
% The joint will be manipulated'Flip'Four other groups of solutions can be obtained
theta_Med_2 = ...
[ %theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi;
%theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi;
%theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi;
%theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi;
];
theta_Med=[theta_Med_1;theta_Med_2];
end
``````

## Operation results Topics: MATLAB REST