二连杆机器人的动力学

clc;clear;close;

%syms L1 L2 m1 m2 g q1 q2 dq1 dq2 ddq1 ddq2 real
Ts=0.01;
L1=1;
L2=1;
m1=10;
m2=10;
ORI=[1 0 0;0 1 0;0 0 1];
q1=30*pi/180;
q2=10*pi/180;
g=-9.81;
dq1=30*pi/180;
dq2=30*pi/180;
ddq1=40*pi/180;
ddq2=5*pi/180;

T12=[cos(q2) -sin(q2) 0 L2*cos(q2);sin(q2) cos(q2) 0 L2*sin(q2);0 0 1 0;0 0 0 1];
T01=[cos(q1) -sin(q1) 0 L1*cos(q1);sin(q1) cos(q1) 0 L1*sin(q1);0 0 1 0;0 0 0 1];
T23=eye(4,4);
T={T01;T12;T23}
V{1}=zeros(6,1);
dV{1}=[0;0;0;0;g;0];
A1=[0;0;1;0;L1;0];
A2=[0;0;1;0;L2;0];
A={A1;A2};
dq=[dq1;dq2];
ddq=[ddq1;ddq2];
for i=1:2
   V{i+1}=(Adjoint(TransInv(T{i}))*V{i}+A{i}*dq(i));
   dV{i+1}=(Adjoint(TransInv(T{i}))*dV{i}+ad(V{i+1})*A{i}*dq(i)+A{i}*ddq(i));
end

g1=[zeros(3,3) zeros(3,3);zeros(3,3) m1*eye(3,3)];
g2=[zeros(3,3) zeros(3,3);zeros(3,3) m2*eye(3,3)];

ga={g1;g2};
F{3}=zeros(6,1);

for i=2:-1:1
    F{i}=(Adjoint(TransInv(T{i+1}))'*F{i+1}+ga{i}*dV{i+1}-ad(V{i+1})'*(ga{i}*V{i+1}));
    Tt{i}=(F{i}'*A{i})
end

 

前两条曲线是算法算出来的，后两条是matlab仿真出来的。