clear;
clc;
%EstablishrobotModel
%Define the D-H parameters of the connecting rod
% theta d a alpha offset
L1=Link([0 0 0.0 pi 0 ]);=[0,pi/6];
L2=Link([0 0 30 pi/2 0 ]);=[0,pi/6];
L3=Link([0 0 70 0 0 ]);=[0,pi/6];
L4=Link([0 0 80 -pi/2 0 ]);=[0,pi/6];
R=SerialLink([L1,L2,L3,L4]); %Build a robot with defined joints
qz=[0 0 pi/4 -pi/3]; The angle variable values of %6 joints are set to 0, which can be changed
(qz,'workspace',[-200,200,-200,200,-200,-200,200]); %Displays the image of the robot
%();
T2=transl(140,0,15);%From the given starting point, the starting point position is obtained 150 1 140 15 130 30 140 45 150 60
T5=transl(130,0,30);
%traj_1=ctraj(T2,T5,100);
%JTA=transl(traj_1);
%plot2(JTA,'b')
%T7=transl(145,0,30);%Get the termination point position according to the given termination point
q1=(T2,'mask',[1,1,1,0,0,0]);%Get the joint angle of the starting point according to the position of the starting point
%fprintf('q1=%f\n',q1)
q6=(T5,'mask',[1,1,1,0,0,0]);%The joint angle of the terminal point is obtained according to the position of the terminal point.
%fprintf('q2=%f\n',q6)
%[q ,qd, qdd]=jtraj(q1,q6,50); %Five-order polynomial trajectory, obtain joint angle, angular velocity, angular acceleration, 50 is the number of sampling points
%subplot(2,2,1);plot(q);xlabel('Time');ylabel('p');
%subplot(2,2,2);plot(qd);xlabel('Time');ylabel('pd');
%subplot(2,2,3);plot(qdd);xlabel('Time');ylabel('pdd');
q=jtraj(q1,q6,0:0.1:2);%Construct the trajectory
JTA=transl((q));
plot2(JTA,'b')
(q);% animation demonstration;
Trajectory planning in Cartesian space:
1. traj_1=ctraj(T0,T1,length(t)); % First call the ctraj function to obtain the control point of the planned trajectory in Cartesian space and assign it to the variable traj_1, where T0 and T1 are the initial and target pose matrices of the end effector of the robot arm, and length(t) is the number of trajectory control points;
2. JTA=transl(traj_1); % Use the transl function to find the position coordinates of each track control point in the Cartesian coordinate system, and assign the position coordinate value to JTA;
3. plot2(JTA,'b') % Draw all tracks with blue dots.
Trajectory planning in joint space:
1. traj_1=jtraj(Q0,Q1,t); % First call the jtraj function to obtain the joint angle value corresponding to the robot configuration planned in the joint space, and assign it to the variable traj_1, where Q0 and Q1 are the joint coordinate values corresponding to the initial and target configurations of the robot arm, t is used to control the number of trajectory control points, which is set to t=0:0.1:2;
2. JTA=transl((traj_1)); % The function is used to obtain the position coordinates of the end effector corresponding to the control position of each track of the robot in Cartesian space, and assign the position coordinate value to JTA; where Rbt is the function name of the robot model established using the SerialLink function; fkine is a function for positive kinematic analysis, and the function outputs the 4×4 end effector pose matrix; the transl function proposes the position matrix from the 4×4 pose matrix.
3. plot2(JTA,'b') % Draw all tracks with blue dots.