空间机械臂力与位置协调控制仿真代码

function C = genC(u) % generate C; % x0 = [xb0;yb0;q0;q1;q2;dxb0;dyb0;dq0;dq1;dq2]; % % 1 5 6 10 % The parameters of the manipulator r0=0.45; r1=0.45; l1=0.55; r2=0.55; l2=0.55; m0=40; m1=4; m2=3; I0=6.667; I1=0.333; I2=0.25; q = u(1:5,1); dq = u(6:10,1); q0= q(3); q1= q(4); q2= q(5); dq0= dq(3); dq1= dq(4); dq2= dq(5); C = zeros(5); C(1,1) = 0; C(1,2) = 0; C(1,3) = -m1*(dq0*(cos(q0)*r0+cos(q0+q1)*l1)+dq1*cos(q0+q1)*l1)-m2*(dq0*(cos(q0)*r0+cos(q0+q1)*(r1+l1)+cos(q0+q1+q2)*l2)+dq1*(cos(q0+q1)*(r1+l1)+cos(q0+q1+q2)*l2)+dq2*cos(q0+q1+q2)*l2); C(1,4) = -m1*(dq0*cos(q0+q1)*l1+dq1*cos(q0+q1)*l1)-m2*((dq0+dq1)*(cos(q0+q1)*(r1+l1)+cos(q0+q1+q2)*l2)+dq2*cos(q0+q1+q2)*l2); C(1,5) = -m2*(dq0+dq1+dq2)*cos(q0+q1+q2)*l2; C(2,1) = 0; C(2,2) = 0; C(2,3) = -m1*(dq0*(sin(q0)*r0+sin(q0+q1)*l1)+dq1*sin(q0+q1)*l1)-m2*(dq0*(sin(q0)*r0+sin(q0+q1)*(r1+l1)+sin(q0+q1+q2)*l2)+dq1*(sin(q0+q1)*(r1+l1)+sin(q0+q1+q2)*l2)+dq2*sin(q0+q1+q2)*l2); C(2,4) = -m1*(dq0+dq1)*sin(q0+q1)*l1-m2*((dq0+dq1)*(sin(q0+q1)*(r1+l1)+sin(q0+q1+q2)*l2)+dq2*sin(q0+q1+q2)*l2); C(2,5) = -m2*(dq0+dq1+dq2)*sin(q0+q1+q2)*l2; C(3,1) = 0; C(3,2) = 0; C(3,3) = -m1*dq1*r0*l1*sin(q1)-m2*(dq1*(r0*(r1+l1)*sin(q1)+r0*l2*sin(q1+q2))+dq2*((r1+l1)*l2*sin(q2)+r0*l2*sin(q1+q2))); C(3,4) = -m1*(dq0+dq1)*r0*l1*sin(q1)-m2*((dq0+dq1)*r0*((r1+l1)*sin(q1)+l2*sin(q1+q2))+dq2*((r1+l1)*sin(q2)+r0*sin(q1+q2))*l2); C(3,5) = -m2*(dq0+dq1+dq2)*((r1+l1)*sin(q2)+r0*sin(q1+q2))*l2; C(4,1) = 0; C(4,2) = 0; C(4,3) = m1*dq0*r0*l1*sin(q1)+m2*(dq0*r0*((r1+l1)*sin(q1)+l2*sin(q1+q2))-dq2*l2*(r1+l1)*sin(q2)); C(4,4) = -m2*dq2*(r1+l1)*l2*sin(q2); C(4,5) = -m2*(dq0+dq1+dq2)*(r1+l1)*l2*sin(q2); C(5,1) = 0; C(5,2) = 0; C(5,3) = m2*((dq0+dq1)*(r1+l1)*sin(q2)+dq0*r0*sin(q1+q2))*l2; C(5,4) = m2*(dq0+dq1)*(r1+l1)*sin(q2)*l2; C(5,5) = 0;