Simulink搭建Baxter机器人的计算力矩控制器-(利用Robotics system toolbox函数block)

1、前记：完全参考 机械臂的轨迹跟踪安全控制（转），计算力矩公式在链接中有就不再贴出来了。其实在这篇MATLAB机器人力控制（1）中的（4）sl_ctorque计算力矩控制模型是一样的，对应的公式如出一辙。

2、模型如下：

根据初始位置和目标位置生成运动轨迹，利用计算力矩控制器驱动机械臂运动。

%% initial
baxter=importrobot('baxter.urdf');
baxter.DataFormat='row';
baxter.Gravity=[0 0 -9.80665];
objectPos = 0.4;
refPos = 0.185;
%% assign the initial and target configuration. 
%initial configuration 
q0=[0;0.6;-0.6;-0.6;1.26;-0.8;1.67;1.5;-0.12;-1;1.18;1.94;-0.67;1.03;0.5];
%the target configuration.
q1=[0;-0.016;-0.5;-0.8;1.66;-0.9;1.27;1.5;-0.12;-1;1.18;1.94;-0.67;1.03;0.5];
%% Generate the desired motion trajectory for each joint
tStart = 0.5;
tDuration = 3;
p = exampleHelperPolynomialTrajectory(q0,q1,tDuration);

           

结果如下：