--高效传送带模型脚本
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
【coppeliasim】高效传送带
最新推荐文章于 2022-08-15 13:28:01 发布