CoppeliaSim笔记（2）：运动学功能(kinematics functionality)

注意：CoppeliaSim的运动学功能是比较新的功能，在CoppeliaSim V4.1.0才出现。 我还拿低版本去跑新出的逆运动学教程，心累。。。另外，在新版本中，tool模块里DH参数提取器已经没有了，看来bug还没有修好。

CoppeliaSim的运动学功能包括以下两部分：

1. 逆向运动学模块（IK mode）
2. 前向运动学模块（FK mode）

该运动学功能是基于Coppelia kinematics routines 实现的，这是一个基于C++的函数库，可以单独使用在其它应用中。关于该函数库的API接口文档，可以参考官方文档Coppelia Kinematics Routines, API 。

在一般情况下，我们不会直接使用这个C++的函数库求解决运动学问题，而是借助kinematics plugin实现的。kinematics plugin是一个运动学插件，它对Coppelia kinematics routines提供的接口进一步进行了封装，提供了一些带有simIK前缀的API供用户使用，参考文档为simIK API。下面简单介绍如何使用这个运动学插件来解决运动学任务。

1.kinematics plugin

运动学任务可以简单的进行设置，如下面这个例子。

-- set-up:
function sysCall_init()
    simBase = sim.getObjectHandle('base')  -- 获得基句柄
    simTip = sim.getObjectHandle('tip')  -- 获得末端句柄
    simTarget = sim.getObjectHandle('target')  -- 获得目标句柄
    ikEnv = simIK.createEnvironment()
    ikGroup = simIK.createIkGroup(ikEnv)
    local ikElement = simIK.addIkElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,desiredConstraints)
end

-- IK calculation, and application to the scene:
function sysCall_actuation()
    simIK.applyIkEnvironmentToScene(ikEnv,ikGroup)
end

以上例子中，addIkElementFromScene在给定"base"、“tip”、"target"以及一些约束的情况下，会生成一个IK元件（相当于一个计算模块）。另一方面，applyIkEnvironmentToScene会使用这个IK元件求解运动学问题，并将结果应用到场景中的对应关节中。

当然，为了可以完全的控制，我们也可以自己手动创建一个运动学环境，如下这个例子所示。

-- set-up:
function sysCall_init()
    simJoints = {sim.getObjectHandle('j1'),sim.getObjectHandle('j2')}  -- 获得关节句柄
    ikJoints = {}  -- 保存IK环境下的关节
    ikBase = simIK.createDummy(ikEnv) -- create a dummy in the IK environemnt
    -- set that dummy into the same pose as its CoppeliaSim counterpart:
    simIK.setObjectMatrix(ikEnv,ikBase,-1,sim.getObjectMatrix(simBase,-1)) 
    local parent = ikBase
    for i=1,#simJoints,1 do -- loop through all joints
        -- 在IK环境下创建一个关节:
        ikJoints[i] = simIK.createJoint(ikEnv,simIK.jointtype_revolute)
        -- 将该关节设置为IK模式: 
        simIK.setJointMode(ikEnv,ikJoints[i],simIK.jointmode_ik) 
        -- 获得sim环境下相同关节的角度变化范围(min, max)
        local cyclic,interv = sim.getJointInterval(simJoints[i])
        -- 将IK环境的同名关节设置为相同的变化范围
        simIK.setJointInterval(ikEnv,ikJoints[i],cyclic,interv)
        -- 将IK环境的同名关节设置为相同的位置
        simIK.setJointPosition(ikEnv,ikJoints[i],sim.getJointPosition(simJoints[i]))
        -- 将IK环境的同名关节设置为相同的位姿(相同的变换矩阵)
        simIK.setObjectMatrix(ikEnv,ikJoints[i],-1,sim.getObjectMatrix(simJoints[i],-1)) 
        simIK.setObjectParent(ikEnv,ikJoints[i],parent) -- set its corresponding parent
        parent = ikJoints[i]
    end
    ikTip = simIK.createDummy(ikEnv) -- create the tip dummy in the IK environment
    -- 在IK环境设置一个与sim环境下的具有相同位姿的dummy-tip
    simIK.setObjectMatrix(ikEnv,ikTip,-1,sim.getObjectMatrix(simTip,-1)) 
    simIK.setObjectParent(ikEnv,ikTip,parent) -- attach it to the kinematic chain
    ikTarget = simIK.createDummy(ikEnv) -- create the target dummy in the IK environment
    -- 在IK环境设置一个与sim环境下的具有相同位姿的dummy-object
    simIK.setObjectMatrix(ikEnv,ikTarget,-1,sim.getObjectMatrix(simTip,-1)) 
    simIK.setLinkedDummy(ikEnv,ikTip,ikTarget) -- link the two dummies
    ikGroup = simIK.createIkGroup(ikEnv) -- create an IK group
    -- 设置求逆解的求解方法
    simIK.setIkGroupCalculation(ikEnv,ikGroup,simIK.method_pseudo_inverse,0,3)
    -- add an IK element to that IK group: 
    local ikElement = simIK.addIkElement(ikEnv,ikGroup,ikTip)
    -- specify the base of that IK element: 
    simIK.setIkElementBase(ikEnv,ikGroup,ikElement,ikBase)
    -- specify the constraints of that IK element: 
    simIK.setIkElementConstraints(ikEnv,ikGroup,ikElement,simIK.constraint_x+simIK.constraint_y) 
end

-- IK calculation, and application to the scene:
function sysCall_actuation()
    -- reflect the pose of the target dummy in the IK environment:
    simIK.setObjectMatrix(ikEnv,ikTarget,ikBase,sim.getObjectMatrix(simTarget,simBase)) 
    simIK.handleIkGroup(ikEnv,ikGroup) -- solve
    -- apply the calculated joint values:
    sim.setJointPosition(simJoints[1],simIK.getJointPosition(ikEnv,ikJoints[1])) 
    sim.setJointPosition(simJoints[2],simIK.getJointPosition(ikEnv,ikJoints[2]))
end

注：仿真环境与IK环境是相互隔离的，即两者的关节或其它目标并不是同一个。

Lua函数的参考文档为regular API reference。

2.Basics on IK groups and IK elements

Basics on IK groups and IK elements (coppeliarobotics.com)

CoppeliaSim的运动学模块在求解运动学任务时，需要先设置IK groups 和IK elements。一个IK任务必须定义一个IK group，而一个IK group则至少需要一个IK element。

• IK group：一个IK group包含一个或多个IK elements。为了求解一个简单的运动链的运动学问题，必须要有一个IK element。IK group会定义总体的求解特性（比如，求解器，迭代次数等）。
• IK element：一个IK element表征一个简单的运动学链，其中至少有一个关节处于IK模式。运动学链必须明确三部分内容，分别为tip （即末端执行器，通常为链的最后一个部件），base（即表示基坐标系的部件，通常为链的第一个部件）， target（tip将要移动的位姿）。
  • base
  • several links：任何object（除了处于IK模式的关节）
  • several joints：处于IK模式的关节，否则它们就会被视为link
  • tip
  • target

下图就是由两个IK element描述的运动学链（kinematic chains）。其中注意右侧的运动学链，它第一个关节被忽略了。

下图是两个以相同方式创建的运动学链，分别创建了两个IK group，每一个都有一个IK element描述。这种情况下，两者的求解顺序并不重要。

下面这种情况，第二个运动学链的target在第一个运动学链上，此时两者的求解顺序就变得重要了，第一组应该放在第一位。

下面这种情况下，两个运动学链虽然相互连接，但是并没有共享任何关节。下图中，Base2是一个共享连杆。求解IK element2并不会移动Base2，但是求解IK element1则会有可能移动Base2。因此，IK group1的求解顺序应该放在第一个。

下面这种情况下，两个运动学链共享了相同的关节base1&base2。在这种情况下，以上采用的Sequential solving已经无法正常工作了，转而使用Simultaneous solving。为了使用这种模式，必须将这两个IK elements放入同一个IK group中。

3.Solving IK and FK for any type of mechanism

Solving IK and FK for any type of mechanism (coppeliarobotics.com)

在使用IK Groups 和 IK Elements时，必须要牢记以下6点：

1. 在设置运动学链时，必须明确提供base 和tip；
2. 必须明确target；
3. 若多个运动学链共享相同的关节，则必须将它们放入到同一个IK group中；
4. 对各个IK group进行排序以获得所需的行为；
5. 核验运动学链中的关节均设置了正确的特性；
6. 核验每一个IK element不会出现过约束。若这种情况无法避免，则需要选择阻尼求解法求解问题。

最后一点特别重要。需要了解，启用了所有约束的运动链的tip将会在x、y、z方向上跟随其关联target，同时尝试与target保持相同的方向。但是只有当运动链只有有6个非冗余自由度时，此功能才能正常工作。tip应该始终受到适当约束，即约束不应该超过机构中的自由度。

但是有时候我们无法正确指定tip的约束，在这种情况下，IK group的计算方法应该设置为阻尼方法（如阻尼最小二乘法），并选择适合的阻尼系数。当目标不可达（无法到达或接近奇异点）时，也应该选择阻尼方法求解。 但是，阻尼方法随虽然可以保证更稳定的计算，但是阻尼始终会降低计算速度（需要更多的迭代次数）。

启用Alpha-Beta约束，将使得tip的z轴方向与target的z轴方向向匹配，同时在禁用Gamma约束时保持围绕z轴的自由旋转。启用Alpha Beta约束和Gamma约束时，tip将尝试采用与target完全相同的方向。

在Basics on IK groups and IK elements一节中，已经介绍如何解决简单运动学链的IK问题。解决简单运动链的FK问题非常简单（只需将所需的关节值应用于链中的所有关节即可获得tip的位置和方向），故不再说明。

但是，解决闭合机构的IK和FK问题则不会那么简单，接下来就会介绍这个问题。

暂时没有涉及到这部分内容，故暂略。。。