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

用MATLAB做的Simulink仿真程序，力与位置的协调控制，

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:51）; dq = u（6:101）;
q0=  q（3）;   q1=  q（4）;   q2=  q（5）;
dq0= dq（3）;  dq1= dq（4）;  dq2= dq（5）;

C = zeros（5）;
C（11） = 0;   C（12） = 0;
C（13） = -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（14） = -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（15） = -m2*（dq0+dq1+dq2）*cos（q0+q1+q2）*l2;
C（21） = 0;  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-07-23 12:15  新建文件夹\
     文件        1857  2017-06-13 22:49  新建文件夹\genC.m
     文件        1372  2017-05-18 10:50  新建文件夹\GenC2tV.m
     文件        1417  2017-06-13 22:49  新建文件夹\genH.m
     文件         504  2017-06-13 22:50  新建文件夹\genJx.m
     文件        1803  2017-05-22 22:39  新建文件夹\GenV2tV.m
     文件         181  2017-05-29 22:14  新建文件夹\invH.m
     文件         183  2017-05-27 16:08  新建文件夹\simCdq.m
     文件         408  2017-05-27 16:09  新建文件夹\simDist.m
     文件          87  2017-05-27 16:11  新建文件夹\simdXm.m
     文件         218  2017-06-13 22:50  新建文件夹\simF.m
     文件       79522  2017-06-13 23:11  新建文件夹\simFFR_12a.mdl
     文件         185  2017-05-27 16:10  新建文件夹\siminvHu.m
     文件         294  2017-06-05 20:17  新建文件夹\simJu.m
     文件         187  2017-05-27 16:08  新建文件夹\simJxTF.m
     文件       22901  2017-06-05 14:53  新建文件夹\Simkine.mdl
     文件         173  2017-05-27 16:06  新建文件夹\simTJCctrl.m