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;
空间机械臂力与位置协调控制仿真.zip （16个子文件） 
新建文件夹 
simJxTF.m 187B GenC2tV.m 1KB simDist.m 408B genC.m 2KB GenV2tV.m 2KB simdXm.m 87B genJx.m 504B siminvHu.m 185B simJu.m 294B simCdq.m 183B simF.m 218B simTJCctrl.m 173B simFFR_12a.mdl 78KB Simkine.mdl 22KB invH.m 181B genH.m 1KB
评论0
最新资源