【动画】机器人&传动带拾放物体

场景

视频演示

部分代码

机器人脚本：

--LBR_iiwa_14_R820 机器人脚本


-- 路径上的速度和加速度
nominalVel = 0.25
nominalAcc = 0.5


-- 获取对象和脚本句柄
target = sim.getObjectHandle("Target")   --tcp目标句柄
connector = sim.getObjectHandle("Connector")  --连接器dummy句柄
belt1_script = sim.getScriptHandle("ConveyorBelt")  --传送带1脚本


--初始化变量：被其他脚本设置值。
pickupDummy = -1   --要拾拣的对象，也就是被传感器检测到的对象
releasePath = -1  --释放路径句柄


function sysCall_threadmain()
    while true do
  -- 暂停脚本，直到信号应用于整型信号“objectAvailable”
        sim.waitForSignal("objectAvailable")  ----绿色盒子被传感器检测到 
  -- 获取当前拾拣路径
        path = sim.getObjectHandle("pickupPath")
  -- 跟随 拾拣路径
        sim.followPath(target,path,3,0,nominalVel,nominalAcc)
  --等待一秒钟以模拟连接过程
        sim.wait(1)
  -- 将连接器连接到 pickupDummy
        sim.setLinkDummy(connector,pickupDummy)
    -- 设置链接类型
        sim.setObjectInt32Param(connector,sim.dummyintparam_link_type,sim.dummy_linktype_dynamics_loop_closure)
  -- 反向跟随 捡起路径
        sim.followPath(target,path,3,1,-nominalVel,-nominalAcc)
  -- 跟随 释放路径
        sim.followPath(target,releasePath,3,0,nominalVel,nominalAcc)
  -- 等待 0.25 秒
        sim.wait(0.25)
  -- 从连接器断开pickupDummy
        sim.setLinkDummy(connector,-1)
  -- 反向跟随 释放路径 到“idle”位置
        sim.followPath(target,releasePath,3,1,-nominalVel,-nominalAcc)
    end
end

传动带1方法：

--移除表中第一个对象，调用传送带2脚本中函数addObject 为表添加对象
function removeFirstObject()
    --通过从表中删除来获取句柄
    local box = table.remove(boxList,1)  --从列表中移除第一个box
    local boxDummy = table.remove(boxDummyList,1)  --移除第一个boxdummy
    table.remove(boolList,1) --移除第一个bool变量值


    --为belt2 表添加对象
    sim.callScriptFunction("addObject",belt2script,{box,boxDummy})


    -- 返回句柄
    return {box,boxDummy}
end

--创建路径：路径名，起始位姿，终止位姿
function createPath(name,startPoint,startOrient,endPoint,endOrient)
    --创建路径对象
    local path = sim.createPath(1)  --创建一条空的新路径


    -- 创建缓冲区变量
    local buffer = {startPoint[1],startPoint[2],startPoint[3],startOrient[1],startOrient[2],startOrient[3], 1,0,0,0,0,
                    endPoint[1],endPoint[2],endPoint[3],endOrient[1],endOrient[2],endOrient[3],             1,0,0,0,0}


    -- 插入 2 个控制点（起点和终点）
    sim.insertPathCtrlPoints(path,0,0,2,buffer)  --向新路径插入控制点 2个 在buffer中


    -- 重命名对象
    sim.setObjectName(path,name) --设置路径名
    -- 返回路径句柄
    return path
end

--插入盒子
function insertBox()
    -- 生成随机数
    local rand1 = math.random()
    local rand2 = math.random()
    local rand3 = math.random()


    -- 在位置和方向上产生   随机干扰
    local dx = (2*rand1-1)*0.1
    local dy = (2*rand2-1)*0.1
    local dphi = (2*rand3-1)*0.5
    local disturbedCoordinates = {0,0,0}  --扰动坐标
    disturbedCoordinates[1] = insertCoordinate[1]+dx     
    disturbedCoordinates[2] = insertCoordinate[2]+dy  
    disturbedCoordinates[3] = insertCoordinate[3]    --扰动后 高度 不变  


    -- 复制粘贴 box 和 boxDummy
    local insertedObjects = sim.copyPasteObjects({box,boxDummy},0) --复制box和boxdummy的组合,索引1开始


    --更新上一插入的盒子时间
    T_last_inserted = sim.getSimulationTime()  --获取仿真时间


    --移动和旋转
--    sim.setObjectPosition(insertedObjects[1],-1,disturbedCoordinates)
    sim.setObjectPosition(insertedObjects[1],-1,insertCoordinate)  --设置插入盒子的位置 
    sim.setObjectOrientation(insertedObjects[1],-1,{0,0,dphi})  --旋转
  
    --存入  box和 boxdummy 的表
    table.insert(boxList,insertedObjects[1])
    table.insert(boxDummyList,insertedObjects[2]) 


    --判断对象是好是坏，设置对象颜色
    local decision = math.random()     --随机数，百分比，用于决定盒子是否good
    if decision <= goodPercentage then
  -- 对象good，分配good颜色
        sim.setShapeColor(insertedObjects[1],nil,sim.colorcomponent_ambient_diffuse,goodColor)
        table.insert(boolList,true)  --bool变量表：存放box 序列的 好坏。
    else
  --  红色：bad 对象
        sim.setShapeColor(insertedObjects[1],nil,sim.colorcomponent_ambient_diffuse,{1,0,0})
        table.insert(boolList,false)
    end
end

笔记：

sim.followPath

Lua语法：

numberdeltaTimeLeft =sim.followPath(number objectHandle,number pathHandle,numberpositionAndOrOrientation,number relativeDistanceOnPath,number velocity,numberaccel)

Lua参数：

objectHandle：要移动的对象的句柄

pathHandle：路径对象的句柄

positionAndOrOrientation：1到3之间的值（1：只修改位置，2：只修改方向，3：修改位置和方向）。可以为 nil，在这种情况下应用 3。

relativeDistanceOnPath：一个介于 0 和 1 之间的值，其中 0 是路径的开始，1 是路径的结束。确保您选择了适当的路径长度计算方法（请参阅路径位置计算方法部分）。

velocity：运动标称速度。

accel：加速/减速。

deltaTimeLeft：如果follow路径所需的时间不是仿真时间步长的倍数，则 deltatimeLeft 是当前仿真时间剩余的执行时间。deltaTimeLeft也在内部以线程为基础存储，并在后续阻塞命令中用作补偿或校正因子。如果发生错误，deltaTimeLeft 为零。

sim.setLinkDummy

定义（或中断）一个dummy-dummy链接对。用于动态创建动态循环闭合约束（等等）。另请参阅 sim.getLinkDummy。

Lua语法：

sim.getLinkDummy(int dummyHandle,intlinkedDummyHandle)

dummyHandle：dummy-dummy链接对中第一个 dummy 的句柄。

linkedDummyHandle：dummy-dummy链接对中第二个 dummy 的句柄。设置为 -1 以取消链接第一个虚拟对象。

sim.copyPasteObjects

复制和粘贴对象及其所有相关脚本。 重复对象的返回顺序可能与提交的原始对象不同。 另请参见 sim.removeObject 和 sim.removeModel。

C语法：

simInt simCopyPasteObjects(simInt*objectHandles,simInt objectCount,simInt options)

Lua语法：

table[]copiedObjectHandles=sim.copyPasteObjects(table[] objectHandles,int options)

objectHandles：包含要复制和粘贴的对象句柄的数组。 同一个数组将接收复制的对象句柄。

objectCount：objectHandles数组中包含的句柄数。

options：位编码。 如果设置了bit0（即 1），则将复制整个模型。 在这种情况下，所有指定的对象都应标记为模型库。

返回值：

如果操作不成功，则返回 -1，否则返回objectHandles 数组中的句柄数。