V-rep学习笔记：Reflexxes Motion Library 2

 　　VREP中的simRMLMoveToPosition函数可以将静态物体按照设定的运动规律移动到指定的目标位置/姿态。If your object is dynamically enabled, it will not work (since in that case the position of the object is dictated by the physics engine). In that case, make sure to uncheck the Body is dynamic checkbox.

　　下面在原点处创建一个Dummy，为其添加threaded child script(The simRMLMoveToPosition function can only be called from child scripts running in a thread (since this is a blocking operation) )，并使用Graph记录其位置-速度-加速度信息。

local h=simGetObjectAssociatedWithScript(sim_handle_self)

local currentVel={0,0,0,0}
local currentAccel={0,0,0,0}
local maxVel={0.2,0.2,0.2,0.2}
local maxAccel={0.1,0.1,0.1,0.1}
local maxJerk={0.1,0.1,0.1,0.1}
local targetPos={1,0,0}
local targetVel={0,0,0,0}

simRMLMoveToPosition(h,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos,nil,targetVel)
simWait(1)
simStopSimulation()

　　Graph的Data stream type中没有加速度的选项，但是注意下面有个Data transformation(determines how the selected data stream is transformed)选项框可以对原始数据进行微分、积分、缩放、平移等变换。

　　Raw uses the original data stream, Derivative uses the time derivative of the data stream (e.g. if the raw data stream is the position of an object, then data will be its velocity), Integral uses the time integral of the data stream (e.g. if the raw data is the velocity of an object, then data will be its position) and Cumulative uses the cumulative value of the data stream (e.g. if the raw data is the surface cut by a mill, then data will be the total surface cut by that mill during the simulation). Additionally, the data can be scaled, shifted, and a moving average period specified.

 　　选择记录Dummy的X方向绝对速度，然后在Data transformations选项框中选择Derivative对数据进行微分，得到X方向加速度：

　　下图记录了Dummy沿X轴运动1m时的位置、速度、加速度随时间的变化曲线。可以看出在开始的加速阶段，加速度在加加速度（Jerk）限制下以梯形曲线达到最大值0.1m/s²，此后进行一段匀加速直线运动，速度曲线为直线段，位置曲线为二次曲线；达到最大速度0.2m/s时进行匀速直线运动，此时加速度为0，位置曲线为直线段。

　　另外需要注意的是simRMLMoveToPosition是一个阻塞模式（blocking operation）的函数，前一个函数执行完后才会继续执行下一个。下面连续两行代码让dummy先按规律运动到(1,0,0)处然后再返回远点：

simRMLMoveToPosition(h,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos,nil,targetVel)
simRMLMoveToPosition(h,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,{0,0,0},nil,targetVel)

 　　使用simRMLMoveToPosition并配合逆运动学可以实现机器人的路径控制：控制Target的目标位置/姿态和运动规律，VREP的运动学逆解解算器会自动让Tip跟随Target运动。下面在场景中拖入ABB IRB4600机器人模型，创建4个路径点pos1~pos4，使机器人按照指定的运动规律依次经过这4个路径点。

-- This is a threaded script!

getTargetPosVectorFromObjectPose=function(objectHandle)
    local p=simGetObjectPosition(objectHandle,targetBase)
    local o=simGetObjectQuaternion(objectHandle,targetBase)
    return p,o
end


threadFunction=function()
    while simGetSimulationState()~=sim_simulation_advancing_abouttostop do

        -- target is the object that the robot's tooltip will try to follow via IK, which means that
        -- if you change the position/orientation of target, then the robot's tooltip will try to follow
        -- target is used so that all position and orientation values are always relative to the robot base
        -- (e.g. so that if you move the robot to another position, you will not have to rewrite this code!)

        -- Go to pos1:
        targetP,targetO=getTargetPosVectorFromObjectPose(pos1_handle)
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Go to pos2:
        targetP,targetO=getTargetPosVectorFromObjectPose(pos2_handle)
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Go to pos3:
        targetP,targetO=getTargetPosVectorFromObjectPose(pos3_handle)
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Go to pos4:
        targetP,targetO=getTargetPosVectorFromObjectPose(pos4_handle)
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
    end
end

-- Initialization:
simSetThreadSwitchTiming(2) 

target=simGetObjectHandle('IRB4600_IkTarget')
targetBase=simGetObjectHandle('IRB4600')

pos1_handle=simGetObjectHandle('pos1')
pos2_handle=simGetObjectHandle('pos2')
pos3_handle=simGetObjectHandle('pos3')
pos4_handle=simGetObjectHandle('pos4')


-- Set-up some of the RML vectors:
maxVel={0.4,0.4,0.4,1.8}
maxAccel={0.3,0.3,0.3,0.9}
maxJerk={0.2,0.2,0.2,0.8}


-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
    simAddStatusbarMessage('Lua runtime error: '..err)
end

参考：

Reflexxes Motion Libraries  Manual and Documentation

V-REP: Reflexxes Motion Library Demonstration

V-rep学习笔记：Reflexxes Motion Library 1