机器人控制系列教程之任务空间运动控制器搭建(2)

Simulink中的实例

推文《机器人控制系列教程之任务空间运动控制器搭建(1)》中，我们详细的讲解了Simulink中的taskSpaceMotionModel模块，实现的方式可以按照如下的步骤。

可以控制器模型替换为taskSpaceMotionModel模块后，该模块的输入分别为一个4 * 4的齐次矩阵和一个1 * 6的速度矩阵，模块中的设置可按照如下方式进行。

m文件实现

编写m程序文件也可实现对机器人的任务空间的控制具体步骤如下：

1.导入机器人模型

导入我们之前使用的iiwa7机器人模型，设置重力方向和大小。

clear; clc;
close all;
iiwa7 = importrobot('iiwa7.urdf');
iiwa7.DataFormat='column';
iiwa7.Gravity = [0 0 -9.81];

2.设置仿真参数

设置仿真时间0到1s，步长为0.02s，设置机器人的初始状态：home构型+零初始速度。

tspan = 0:0.02:1;
initialState = [homeConfiguration(iiwa7);zeros(7,1)];

3.设置任务空间目标位置

定义一个参考目标状态，包括目标位置和零初始速度，此时旋转矩阵为I，位置向量变为[0.6 -0.1 0.5]。

refPose = trvec2tform([0.6 -.1 0.5]); 
refVel = zeros(6,1);

4.创建运动模型

在比例微分（PD）控制下进行系统建模,指定iiwa7刚体树模型，指定末端执行器模型.

motionModel = taskSpaceMotionModel("RigidBodyTree",iiwa7,"EndEffectorName","EndEffector_Link");

5.开始计算

使用刚性求解器在1秒钟内进行仿真，以更有效地捕获机器人动力学，使用ode15可以在变化率较高的区域周围实现更高的精度。

[t,iiwa7State] = ode15s(@(t,state)derivative(motionModel,state,refPose,refVel),tspan,initialState);

6.绘制机器人模型和仿真

% 画出机器人的位置，并将目标位置用符号"X"标记出来
figure
show(iiwa7,initialState(1:7));
hold all
plot3(refPose(1,4),refPose(2,4),refPose(3,4),"x","MarkerSize",20)
% 通过将机器人绘制成5Hz循环来观察响应
r = rateControl(5);
for i = 1:size(iiwa7State,1)
    show(iiwa7,iiwa7State(i,1:7)',"PreservePlot",false);
    
    % 画出末端轨迹
    poseNow = getTransform(iiwa7, iiwa7State(i, 1: 7)', "iiwa_link_7");
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20)
    waitfor(r);
end