【coppeliasim】高效传送带

--高效传送带模型脚本
function sysCall_init() 
    config={}
    
    -- Modify following to customize your conveyor:
    -----------------------------------------------
    config.size={1,0.2,0.1} --长宽高
    config.col1={0.2,0.2,0.2}--传送带颜色
    config.col2={0.5,0.5,0.5}--实体颜色
    config.initPos=0--初始位置0
    config.initVel=0.1--初始速度0.1
    config.accel=1.5--初始加速度1.5
    -----------------------------------------------
    
    -- To command the conveyor externally or from another script, simply do:
    --
    -- sim.writeCustomDataBlock(conveyorHandle,'CONVMOV',sim.packTable({vel=0.1})) -- vel. ctrl
    --
    -- or:
    --
    -- sim.writeCustomDataBlock(conveyorHandle,'CONVMOV',sim.packTable({pos=0.1})) -- pos. ctrl
    --
    -- Its current position can be read with:
    -- local data=sim.readCustomDataBlock(model,'CONVMOV')
    --
    -- if data then
    --     local currentPos=data.currentPos
    -- end

    model=sim.getObjectHandle(sim.handle_self)--传送带 模型句柄
    visible1=sim.getObjectHandle('efficientConveyor_visible1')--可见模型1句柄
    visible2=sim.getObjectHandle('efficientConveyor_visible2')--可见模型2句柄
    forwarder=sim.getObjectHandle('efficientConveyor_forwarder')--前进句柄
    
    sim.setShapeColor(visible1,'',sim.colorcomponent_ambient_diffuse,config.col1)--设置颜色
    sim.setShapeColor(visible2,'',sim.colorcomponent_ambient_diffuse,config.col2)--设置颜色 
    sim.setShapeBB(visible1,config.size)--设置形状
    sim.setShapeBB(visible2,{config.size[1]+0.005,config.size[2]+0.005,config.size[3]})--设置形状 x,y方向稍微大些
    sim.setShapeBB(forwarder,config.size)--设置形状大小
    sim.setObjectPosition(visible1,model,{0,0,-config.size[3]/2})--设置模型位置
    sim.setObjectPosition(visible2,model,{0,0,-config.size[3]/2-0.0025})--稍微靠下
    sim.setObjectPosition(forwarder,model,{0,0,-config.size[3]/2})--模型位置
    
    pos=config.initPos--初始位置
    prevPos=pos--记录上一位置
    targetVel=config.initVel--目标速度：初始速度 
    vel=0--上一速度
    accel=config.accel--加速度
    sim.writeCustomDataBlock(model,'CONVMOV',sim.packTable({currentPos=pos}))--设置传送带位置
end

function sysCall_afterSimulation()
    pos=config.initPos --仿真结束后，设置位置
    prevPos=pos--更新上一位置
    targetVel=config.initVel--设置为初始速度
    vel=0--更新上一速度
    accel=config.accel--设置为初始加速度
    sim.writeCustomDataBlock(model,'CONVMOV',sim.packTable({currentPos=pos}))--设置传送带回到初始位置
end

function sysCall_actuation() 
    local dat=sim.readCustomDataBlock(model,'CONVMOV')--读取传送带数据
    local off
    if dat then
        dat=sim.unpackTable(dat)
        if dat.offset then
            off=dat.offset--所需传送带的偏置量
        end
        if dat.vel then
            targetVel=dat.vel--传送带速度
        end
        if dat.accel then
            accel=dat.accel--传送带加速度
        end
    end
    local velErr=targetVel-vel--计算速度调节量
    local velErrAbs=math.abs(velErr)
    local velErrSign=velErrAbs>1e-4 and velErr/velErrAbs or 1--移动方向
    if velErrAbs>1e-6 then
        vel=vel+math.min(velErrAbs,accel*velErrSign*sim.getSimulationTimeStep())--更新速度
    end
    if off or vel~=0 then--偏置且速度非零
        if off then
            pos=off--设置传送带位置为偏置量
        else--无设置偏置量时通过计算得到位置
            pos=pos+vel*sim.getSimulationTimeStep()--计算传送带位置
        end
        setPos(pos)--更新传送带位置
    end
    if not dat then--数据为空
        dat={}
    end
    dat.currentPos=pos--记录传送带当前位置
    sim.writeCustomDataBlock(model,'CONVMOV',sim.packTable(dat))--写入数据块 传送带数据
end

function setPos(p)    
    -- Here we "fake" the transportation pads with a single static rectangle that we dynamically reset
    -- at each simulation pass (while not forgetting to set its initial velocity vector) :
    -- 在这里，我们使用我们动态重置的单个静态矩形“伪造”运输垫
     -- 在每次模拟过程中（同时不要忘记设置其初始速度矢量）：
    local relativeLinearVelocity={(p-prevPos)/sim.getSimulationTimeStep(),0,0}--沿着x方向相对线速度
    prevPos=pos--记录初始位置 
    
    -- Reset the dynamic rectangle from the simulation (it will be removed and added again)
	--从模拟中重置动态矩形（它将被删除并再次添加）
    sim.resetDynamicObject(forwarder)
    -- Compute the absolute velocity vector:计算绝对速度向量：
    local m=sim.getObjectMatrix(forwarder,-1)--获取前移矩形的位姿矩阵，并把位置设置为0
    m[4]=0 -- Make sure the translation component is discarded
    m[8]=0 -- Make sure the translation component is discarded
    m[12]=0 -- Make sure the translation component is discarded
    local absoluteLinearVelocity=sim.multiplyVector(m,relativeLinearVelocity)--绝对速度：  由相对线性速度变换到 前移矩形坐标系下。
    -- Now set the initial velocity of the dynamic rectangle:现在设置动态矩形的初始速度：
    sim.setObjectFloatParam(forwarder,sim.shapefloatparam_init_velocity_x,absoluteLinearVelocity[1])
    sim.setObjectFloatParam(forwarder,sim.shapefloatparam_init_velocity_y,absoluteLinearVelocity[2])
    sim.setObjectFloatParam(forwarder,sim.shapefloatparam_init_velocity_z,absoluteLinearVelocity[3])
end