UR5 逆向运动学

V-REP逆解概述

V-REP提供多种求逆解的模式，每种各有特色，当然你也可以穿插着使用，但同一时间智能用一种。穿插使用的方法是设定不同的范围启用不同的逆解模式，这种方式有些复杂。V-REP安装文件夹下提供了各种不同方式的示例场景，在

YOUR_INSTALL_PATHV-REP3V-REP_PRO_EDUscenesik_fk_simple_examples

下可以找到。建议学习之前先看一下每一个例子，当然不看也不影响你阅读下面的内容。

UR5的V-REP逆解

这里我们仅以UR5为例，一步一步介绍如何在V-REP中添加逆解。注意此时我们采用的是V-REP模型浏览器中自带的UR5，也不修改各个模块的名称。这里我们从零开始搭建。官方也给出了一个完整的实验环境：

YOUR_INSTALL_PATHV-REP3V-REP_PRO_EDUscenesur5WithRg2Grasping

下面我们一步一步来搭建我们的环境。

Step 1:搭建你的V-REP仿真环境。

新建一个V-REP环境，将UR5拖拽到环境中，并将RG2安装在UR5末端即可。主要不要修改每个模块的名称。如果你对这些操作不太熟悉，打开其中的 UR5plusRG2.ttt 环境。

Step 2:添加你的 IKTip。

添加一个dummy，dummy在V-REP 中是最简单的模型。然后调整其位置到（-3.7658e-1，+0e+0，+1.0013e+0），注意此时我们假设你的UR的坐标是（+0e+0，+0e+0，+1.4650e-2）。如果你调整的UR的位置，那么这个dummy的位置也需要左相应的调整。当然也没有这么死板，只要将其放在你认为的夹持器工作中心即可。然后将其重命名为 UR5_ikTip。

添加完成后，将其与UR5建立关联。选中UR5_tip，然后按住Ctrl按键再选中UR6_link7，然后[Menu bar –> Edit –> Make last selected object parent]。

Step 3:添加你的 IKTarget。

复制 UR5_tip，黏贴并重命名为 UR5_ikTarget。

Step 4:创建你的Tip-Target对。

打开UR5_tip的属性窗口，按照下图设置：

完成后你可以看到UR5_tip 和 UR5_target 由一条红线连接。配对完成。

Step 5:创建 IK Group。

现在你只是创建了两个关联的dummy，下面我们为其指定逆解组。打开Inverse kinematics dialog，添加一个新的 IK group，并将其更名为UR5：

完成后按照上图进行设置，点击 Edit IK elements。在IK Group dialog中，在左上角的下拉菜单中选择UR5_tip，然后点击 Add new IK elemtn with tip。此时IK elements 列表中会添加一项 UR5_tip。点击UR5_tip，在下方的Kinematic chain section 中，将Base设置为 UR5。在下方的Constraints section 中勾选所有复选框，完成后关闭IK Group dialog。

此处关于 Inverse kinematics dialog中各个复选框的含义请参考官方文档说明。

Step 6:完成。

好了，现状你已经基本完成逆解的配置，你可以将UR5_target dummy合并到UR5下，并将UR5_tip 和 UR5_target的 Visibility 属性调整到第11层使它们隐藏。当然这都不是必须的。

下面做一个简单的测试。注意这里的测试方法与官方教程中给出的不同，对于想深入了解V-REP逆解机制的读者可以参考官方教材进行操作，然而对于想快速入门的读者不建议这样做，因为因为太费时，而且是没必要的。测试方式如下：

打开UR5的script。

删除原有代码，输入如下代码：

enableIk=function(enable)
    if enable then
        sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))
        for i=1,#jointHandles,1 do
            sim.setJointMode(jointHandles[i],sim.jointmode_ik,1)
        end

        sim.setExplicitHandling(ikGroupHandle,0)
    else
        sim.setExplicitHandling(ikGroupHandle,1)
        for i=1,#jointHandles,1 do
            sim.setJointMode(jointHandles[i],sim.jointmode_force,0)
        end
    end
end

function sysCall_threadmain()
    -- Initialize some values:
    jointHandles={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jointHandles[i]=sim.getObjectHandle('UR5_joint'..i)
    end
    ikGroupHandle=sim.getIkGroupHandle('UR5')
    ikTip=sim.getObjectHandle('UR5_ikTip')
    ikTarget=sim.getObjectHandle('UR5_ikTarget')
    --modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)
    --modelName=sim.getObjectName(modelBase)

    -- Set-up some of the RML vectors:
    vel=180
    accel=40
    jerk=80
    currentVel={0,0,0,0,0,0,0}
    currentAccel={0,0,0,0,0,0,0}
    maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk大专栏  UR5 逆向运动学en operator">*math.pi/180}
    targetVel={0,0,0,0,0,0}

    ikMaxVel={0.4,0.4,0.4,1.8}
    ikMaxAccel={0.8,0.8,0.8,0.9}
    ikMaxJerk={0.6,0.6,0.6,0.8}

    initialConfig={0,0,0,0,0,0}
    startConfig={-70.1*math.pi/180,18.85*math.pi/180,93.18*math.pi/180,68.02*math.pi/180,109.9*math.pi/180,90*math.pi/180}

    enableIk(false)
    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.rmlMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,startConfig,targetVel)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        enableIk(true)

        pos=sim.getObjectPosition(ikTip,-1)
        quat=sim.getObjectQuaternion(ikTip,-1)
        sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{pos[1],pos[2]-0.2,pos[3]},quat,nil)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.wait(1)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{pos[1],pos[2]-0.2,pos[3]+0.2},quat,nil)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.wait(1)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{pos[1],pos[2],pos[3]+0.2},quat,nil)
    end

    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.wait(1)
    end

    enableIk(false)
    sim.rmlMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,initialConfig,targetVel)
    sim.stopSimulation()
end

运行仿真，观测UR5的动作。可以看到它的末端走了一个方形的四个顶点。

有关逆解的常用API

下面列举一些常用的有关逆解的Lua API。

sim.getObjectMatrix 获取场景中对象的变换矩阵（H）。

sim.setObjectMatrix 设置场景中对象的变换矩阵（H）。

sim.setJointMode 设置某关节控制模式。

sim.getIKGroupHandle 获取IK Group的引用。

sim.getObjectPosition 获取场景中某个对象的位置。

sim.getObjectQuaternion 获取场景中某个对象的姿态（四元数）。

sim.rmlMoveToJointPosition 关节空间运动规划。

sim.rmlMoveToPosition 笛卡尔空间运动规划。