由于本人水平太菜，姑且改编个现成的仿真实例作为专栏开篇，各位看官多多包涵。这个现成的例子来自于MATLAB的自带帮助文档，网页版链接如下(也可通过MATLAB本地打开)：

Control LBR Manipulator Motion Through Joint Torque Commands​www.mathworks.com/help/robotics/examples/control-lbr-manipulator-motion-through-joint-torque.html?s_tid=srchtitle

简而言之，这个例子通过MATLAB的ROS Toolbox实现了MATLAB和Gazebo的通信，通过Robotics Systems Toolbox实现LBR/iiwa机械臂的动力学建模与相关计算，实现了经典的计算力矩(Computed Torque)控制仿真。本文需要改编的地方就是把Gazebo换成V-REP，实现V-REP和MATLAB的通信与数据交互。

在正式开始前，我们先建几个文件夹，本人有点小洁癖。项目文件夹下分为三个子文件夹：vrep_scenes(放场景.ttt文件和模型.ttm文件)，vrep_matlab_communication(放V-REP和MATLAB通信所需的文件)和iiwa14_computed_torque_control(放控制.m文件)以及一个把这些文件路径添加到MATLAB文件路径列表的adddir.m，像这样：

另外注意一下，MATLAB的版本必须不早于2018b，否则没有importrobot函数将无法加载urdf文件。

1. 制作场景文件(iiwa14_conputed_torque.ttt)

本人不仅菜而且懒，所以直接白嫖了MATLAB例子中自带的LBR/iiwa机械臂的URDF文件及对应三维模型文件，具体路径是MATLAB安装目录下的\toolbox\robotics\robotexamples\robotmanip\data\iiwa_description。

接下来打开V-REP，通过Plugins菜单下的URDF Import加载URDF文件即可加载LBR/iiwa模型，保存为.ttt文件放到vrep_scenes文件夹下即可。这里可能会发生一点小插曲，如果你不是在Debian/Ubuntu系统且未安装ROS的话，需要手动批量修改URDF文件里模型文件所在路径。

弄好之后，大概是这个样子：

2. 通信准备

按照V-REP官方手册的指示，把remApi.m，remoteApiProto.m，remoteApi.dll和remoteApi.so四个文件复制到vrep_matlab_communication文件夹下。

然后，为了显得逼格高一点，又从如下白嫖了vrchk.m文件，其主要内容是根据V-REP与MATLAB的通信失败类型输出相应的信息。

An Open-source Recipe for Teaching (and Learning) Robotics with a Simulator​ulgrobotics.github.io/trs/

最后，为了看起来更像专业码农，我们把获取机械臂关节句柄(handle)以及需要事先进行streaming初始化的关节位置与速度获取等动作整在一起，弄一个iiwa14_computer_torque_control_workcell_init.m函数文件，内容如下：

function handles = workcell_init(vrep,id)
robot_name = 'LBR_iiwa_14_R820';

handles = struct('id',id);
%% arm joints
armJoints = -ones(1,7);
for i=1:7 % get handles of joints 
    [res,armJoints(i)] = vrep.simxGetObjectHandle(id,[robot_name,'_joint',num2str(i)],vrep.simx_opmode_oneshot_wait);vrchk(vrep, res);
end
handles.armJoints = armJoints;

%% streaming joint positions and velocities
for i=1:7
    vrep.simxGetJointPosition(id,armJoints(i),vrep.simx_opmode_streaming);vrchk(vrep, res, true); % joint position
    vrep.simxGetObjectFloatParameter(id,armJoints(i),2012,vrep.simx_opmode_streaming);vrchk(vrep, res, true); % joint velocity
end
% Make sure that all streaming data has reached the client at least once
vrep.simxGetPingTime(id);
end

3. 编写通信代码

从这里直至项目完成，我们都在iiwa14_computed_torque_control文件夹下耕耘。

首先，建立一个iiwa14_computed_torque_control.m文件，插入如下通信代码(这个可以看做固定框架，做成个模板文件想必是极好的，无奈本人太懒)。有疑问请阅读注释。

%% connect to vrep
disp('Program started');
vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1);      % just in case, close all opened connections
id=vrep.simxStart('127.0.0.1',19997,true,true,5000,5); % connect to vrep server
if (id>-1)
    disp('Connected to remote API server');
    % control
    ...
    % stop or pause simulation
    % vrep.simxStopSimulation(id,vrep.simx_opmode_oneshot_wait);
    % vrep.simxPauseSimulation(id,vrep.simx_opmode_oneshot_wait);
    % close the connection to V-REP
    vrep.simxFinish(id);
else
    disp('Failed connecting to remote API server1');
end
vrep.delete(); % call the destructor!
% postprocessing
...

4. 编写控制代码

这里就是填充3中control后面的省略号部分。

首先，由于是关节力矩控制，本菜建议使用同步(synchronous)模式。这个模式通俗的讲就是你喊一声“V-REP，给老子动”(调用vrep.simxSynchronousTrigger函数)，V-REP才动。vrep在动的过程中是“两耳不闻窗外事”的(就是说dt时间段内的控制输入始终是你dt开始的时刻发给V-REP的)，直到预先设置的仿真周期dt之后。这个dt你可以直接在V-REP软件界面中设置，也可以通过V-REP的API设置。同步模式的控制代码框架罗列如下:

    % simulation period
    dt = 1e-3;
    % set simulation period by API
    vrep.simxSetFloatingParameter(id,vrep.sim_floatparam_simulation_time_step,dt,vrep.simx_opmode_oneshot_wait);
    % set synchronous mode
    vrep.simxSynchronous(id,true);
    % start the simulation:
    vrep.simxStartSimulation(id,vrep.simx_opmode_oneshot_wait);
    % ask V-REP to simulate for dt
    vrep.simxSynchronousTrigger(id);
    % initialize some variables for control
    ...
    % control loop
    while vrep.simxGetConnectionId(id)~=-1
       % get current time
       ...
       % get current state
       ...
       % if control target achieved
       ...
       % compute control input 
       ...
       % send control input to V-REP
       ...
       % ask V-REP to simulate for dt
       vrep.simxSynchronousTrigger(id);
    end

然后，开始往上面各个省略号处塞东西。在开动之前，我们再捋一捋，也就是搞清楚MATLAB官方的例子干了些什么，能白嫖的尽量白嫖。官方先是调用importrobot函数建立了RigidBodyTree结构体(可以理解为就是建立了一个机器人)，然后加载了预先定义的几个waypoints(带有时刻信息的关节位置值)，给定一个时间间隔，得到等间隔时间序列，调用exampleHelperJointTrajectoryGeneration函数进行轨迹插值，得到与时间序列对应的关节位置，速度与加速度值，进一步调用inverseDynamics函数得到对应的关节力矩值，也就是控制输入中的前馈力矩值。进入控制循环以后，每个周期获得当前Gazebo反馈的机器人状态值，计算反馈力矩，找到对应时刻的前馈力矩值，两者相加即为控制力矩，发送给Gazebo。捋完之后发现基本可以完全白嫖，除了把Gazebo改成V-REP。具体的代码就不贴出来了(请参看源文件)。

这里有一个地方可能要专门拧出来说一下，关于V-REP中关节驱动器的三种控制模式的设置问题。V-REP支持关节位置，速度和力矩模式，这三种的共同点是都要在关节的Properties界面设置关节的Mode为Torque/force mode，如下图所示：

不同点是在Joint Dynamic Properties界面(点击上图中最下面的Show dynamic properties dialog按钮后弹出)的设置以及调用API，具体如下：

• 位置控制

设置方面，使能电机，使能关节位置控制循环。

调用API是vrep.simxSetJointTargetPosition函数。

• 速度控制

设置方面，使能电机即可，注意把最大力矩值设置为比较大的值或者对标你的实际物理电机最大力矩。

调用API是vrep.simxSetJointTargetVelocity函数。

• 力矩控制

设置方面和速度控制相同。

调用API是vrep.simxSetJointTargetVelocity和vrep.simxSetJointForce两个函数。阅读V-REP的手册我们发现vrep.simxSetJointTargetVelocity函数可以设置任意实数，而vrep.simxSetJointForce只能设置非负数。更进一步了解V-REP物理引擎的工作过程，我们发现电机使能的时候，物理引擎会以关节目标速度作为奋斗目标，尽量最快地达到关节目标速度，也就是说如果你给了一个模长很大的速度值，V-REP会以你给定的力矩驱动关节，力矩的方向跟目标速度相同。于是，我们不难得到如下代码：

% Send torque to vrep
        for j=1:7
            if tau(j)<0
                set_vel = -99999;
                set_tau= -tau(j);
            else
                set_vel = 99999;
                set_tau = tau(j);
            end
            vrep.simxSetJointTargetVelocity(id,joint_handles(j),set_vel,vrep.simx_opmode_oneshot);
            vrep.simxSetJointForce(id,joint_handles(j),set_tau,vrep.simx_opmode_oneshot);
        end

5. 数据处理

这里我们就简单地对比一下前馈和反馈力矩，期望和实际关节位置，期望和实际关节速度。如下图所示：

6. 运行

为了防止意外，我们现在把V-REP和MATLAB都关掉(关机重启最好)，然后先打开V-REP加载场景文件，接着打开MATLAB，运行adddir.m，最后进入iiwa14_computed_torque_control文件夹，运行iiwa14_computer_torque_control.m，在V-REP中就可以看到如下仿真动画：

----------------------------------------------------

本例程源文件链接(切记只可用于学习，科研等非商业用途)

-----------------------------------------------------

做人呐，最重要的就是开心喽。能帮到别人是最大的快乐，希望本菜鸡能对小白们有所帮助，希望大佬们的批评能帮助本菜鸡成长！