【matlab源码】机械臂阻抗控制matlab仿真

function [sys,x0,str,ts]=s_function(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); case 3, sys=mdlOutputs(t,x,u); case {2, 4, 9 } sys = []; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys=simsizes(sizes); x0=[0.3;0.3;0.3;0.5]; str=[]; ts=[]; function sys=mdlDerivatives(t,x,u) q1=x(1); dq1=x(2); q2=x(3); dq2=x(4); m1=1; m2=1; L1=2; L2=2; g=10; M=[m2*(L2)^2+(m1+m2)*(L1)^2+2*m2*L1*L2*cos(q2) m2*(L2)^2+m2*L1*L2*cos(q2); m2*(L2)^2+m2*L1*L2*cos(q2) m2*(L2)^2]; B=[-2*m2*L1*L2*sin(q2)*dq2 -m2*L1*L2*sin(q2)*dq2; m2*L1*L2*sin(q2)*dq1 0]; G=[(m1+m2)*L1*g*cos(q1)+m2*L2*g*cos(q1+q2); m2*L2*g*cos(q1+q2)]; tol(1)=u(1); tol(2)=u(2); ddq=inv(M)*(tol'-B*[dq1;dq2]-G); sys(1)=x(2); sys(2)=ddq(1); sys(3)=x(4); sys(4)=ddq(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1); sys(2)=x(2); sys(3)=x(3); sys(4)=x(4);