0526更新 <<
Previous Next >> 尋找控制代碼
舊版本
預計更新內容:將簡化程式碼
圖檔:
W13 Pick and Place.ttt
5/21 拍攝
程式碼參考
主程式
function signal_on()
signal_1(true)
signal_2(true)
signal_3(true)
signal_4(true)
signal_5(true)
signal_6(true)
end
function signal_1(enable)
modelBase1=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName1=sim.getObjectName(modelBase1)
if enable then
sim.setIntegerSignal(modelName1 .."call_1",1)
else
sim.setIntegerSignal(modelName1 .."call_1",0)
end
end
function signal_2(enable)
modelBase2=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName2=sim.getObjectName(modelBase2)
if enable then
sim.setIntegerSignal(modelName2 .."call_2",1)
else
sim.setIntegerSignal(modelName2 .."call_2",0)
end
end
function signal_3(enable)
modelBase3=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName3=sim.getObjectName(modelBase3)
if enable then
sim.setIntegerSignal(modelName3 .."call_3",1)
else
sim.setIntegerSignal(modelName3 .."call_3",0)
end
end
function signal_4(enable)
modelBase4=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName4=sim.getObjectName(modelBase4)
if enable then
sim.setIntegerSignal(modelName4 .."call_4",1)
else
sim.setIntegerSignal(modelName4 .."call_4",0)
end
end
function signal_5(enable)
modelBase5=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName5=sim.getObjectName(modelBase5)
if enable then
sim.setIntegerSignal(modelName5 .."call_5",1)
else
sim.setIntegerSignal(modelName5 .."call_5",0)
end
end
function signal_6(enable)
modelBase6=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName6=sim.getObjectName(modelBase6)
if enable then
sim.setIntegerSignal(modelName6 .."call_6",1)
else
sim.setIntegerSignal(modelName6 .."call_6",0)
end
end
function sysCall_threadmain()
on=true
joint01=sim.getObjectHandle('X')
joint02=sim.getObjectHandle('Y')
joint03=sim.getObjectHandle('Z')
jointr=sim.getObjectHandle('Rotate')
secc=sim.getScriptHandle('suctionPad')
dx=0
dy=0
dz=0
dr=0
dt=0.01
tt=1
long_t=1.5
long_long_t=3
aty=0
setx=0.5
setpx=-0.5
sety=-0.16
setpy=-0.16
pick_h1=0.0645
pick_h2=0.134
pick_h3=0.2025
pick_h4=0.2768
fall_h4=0.26
fall_h3=0.18
fall_h2=0.12
fall_h1=0.05
fall_h4_up=0.12
fall_h3_up=0.05
while (on==true) do
signal_1(true)
sim.setJointTargetPosition(joint01,setx) --pick_1
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_2 h2-h2-
sim.setJointTargetPosition(joint02,sety-0.05)
sim.wait(tt)
signal_2(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_3
sim.setJointTargetPosition(joint02,sety+0.05)
sim.wait(tt)
signal_3(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_4
sim.setJointTargetPosition(joint02,sety+0.05)
sim.wait(tt)
signal_4(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_5
sim.setJointTargetPosition(joint02,sety-0.05)
sim.wait(tt)
signal_5(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.1) --pick_6 h3-h3
sim.setJointTargetPosition(joint02,sety+0.1)
sim.wait(tt)
signal_6(true)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 1
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 2
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 3
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 4
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 5
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 6
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx+0.1) --pick_7
sim.setJointTargetPosition(joint02,sety)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.1) --pick_8
sim.setJointTargetPosition(joint02,sety-0.1)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_9
sim.setJointTargetPosition(joint02,sety-0.1)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_10
sim.setJointTargetPosition(joint02,sety)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_11
sim.setJointTargetPosition(joint02,sety+0.1)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.1) --pick_12
sim.setJointTargetPosition(joint02,sety+0.1)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 7
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 8
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-0.12)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 9
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 10
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 11
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 12
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx-0.1) --pick_13
sim.setJointTargetPosition(joint02,sety)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.1) --pick_14
sim.setJointTargetPosition(joint02,sety-0.1)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_15 --h4-h4
sim.setJointTargetPosition(joint02,sety+0.15)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_16
sim.setJointTargetPosition(joint02,sety+0.05)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_17
sim.setJointTargetPosition(joint02,sety-0.05)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_18
sim.setJointTargetPosition(joint02,sety-0.15)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 13
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 14
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 15
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 16
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 17 h3--h3
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 18
sim.setJointTargetPosition(joint02,setpy-0)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx-0.05) --pick_19
sim.setJointTargetPosition(joint02,sety-0.15)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_20
sim.setJointTargetPosition(joint02,sety-0.05)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_21
sim.setJointTargetPosition(joint02,sety+0.05)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_22
sim.setJointTargetPosition(joint02,sety+0.15)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_23
sim.setJointTargetPosition(joint02,sety+0.15)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_24
sim.setJointTargetPosition(joint02,sety+0.05)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 19
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 20
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 21
sim.setJointTargetPosition(joint02,setpy)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 22
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 23
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 24
sim.setJointTargetPosition(joint02,setpy-0)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx+0.05) --pick_25
sim.setJointTargetPosition(joint02,sety-0.05)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_26
sim.setJointTargetPosition(joint02,sety-0.15)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_27
sim.setJointTargetPosition(joint02,sety-0.15)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_28
sim.setJointTargetPosition(joint02,sety-0.05)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_29
sim.setJointTargetPosition(joint02,sety+0.05)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_30
sim.setJointTargetPosition(joint02,sety+0.15)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 25
sim.wait(long_long_t)
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 26
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 27
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 28
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 29
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 30
sim.setJointTargetPosition(joint02,setpy)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h1)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
----------------------------------------------
sim.setJointTargetPosition(joint01,0)
sim.setJointTargetPosition(joint02,0)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
-----------------------------------------------------------
-----------------------------------------------------------
pick_h1=0.07
pick_h2=0.138
pick_h3=0.2085
pick_h4=0.28
setx=-0.51
setpx=0.49
signal_1(true)
sim.setJointTargetPosition(joint01,setx) --pick_1
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_2 h2-h2-
sim.setJointTargetPosition(joint02,sety-0.05)
sim.wait(tt)
signal_2(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_3
sim.setJointTargetPosition(joint02,sety+0.05)
sim.wait(tt)
signal_3(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_4
sim.setJointTargetPosition(joint02,sety+0.05)
sim.wait(tt)
signal_4(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_5
sim.setJointTargetPosition(joint02,sety-0.05)
sim.wait(tt)
signal_5(true)
sim.setJointTargetPosition(joint03,aty-pick_h2)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.1) --pick_6 h3-h3
sim.setJointTargetPosition(joint02,sety+0.1)
sim.wait(tt)
signal_6(true)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 1
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 2
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 3
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.15)--fall 4
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 5
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 6
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx+0.1) --pick_7
sim.setJointTargetPosition(joint02,sety)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.1) --pick_8
sim.setJointTargetPosition(joint02,sety-0.1)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_9
sim.setJointTargetPosition(joint02,sety-0.1)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_10
sim.setJointTargetPosition(joint02,sety)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx) --pick_11
sim.setJointTargetPosition(joint02,sety+0.1)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.1) --pick_12
sim.setJointTargetPosition(joint02,sety+0.1)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 7
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 8
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-0.12)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 9
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 10
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 11
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 12
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx-0.1) --pick_13
sim.setJointTargetPosition(joint02,sety)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.1) --pick_14
sim.setJointTargetPosition(joint02,sety-0.1)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h3)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_15 --h4-h4
sim.setJointTargetPosition(joint02,sety+0.15)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_16
sim.setJointTargetPosition(joint02,sety+0.05)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_17
sim.setJointTargetPosition(joint02,sety-0.05)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.15) --pick_18
sim.setJointTargetPosition(joint02,sety-0.15)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 13
sim.wait(long_long_t)
sim.setJointTargetPosition(joint02,setpy-0.15)
sim.wait(long_long_t)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 14
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 15
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.15)--fall 16
sim.setJointTargetPosition(joint02,setpy+0.15)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h4_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 17 h3--h3
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 18
sim.setJointTargetPosition(joint02,setpy-0)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx-0.05) --pick_19
sim.setJointTargetPosition(joint02,sety-0.15)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_20
sim.setJointTargetPosition(joint02,sety-0.05)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_21
sim.setJointTargetPosition(joint02,sety+0.05)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx-0.05) --pick_22
sim.setJointTargetPosition(joint02,sety+0.15)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_23
sim.setJointTargetPosition(joint02,sety+0.15)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_24
sim.setJointTargetPosition(joint02,sety+0.05)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx+0.1)--fall 19
sim.wait(tt)
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 20
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 21
sim.setJointTargetPosition(joint02,setpy)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 22
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 23
sim.setJointTargetPosition(joint02,setpy-0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 24
sim.setJointTargetPosition(joint02,setpy-0)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx)
sim.setJointTargetPosition(joint02,sety)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setx+0.05) --pick_25
sim.setJointTargetPosition(joint02,sety-0.05)
signal_1(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.05) --pick_26
sim.setJointTargetPosition(joint02,sety-0.15)
signal_2(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_27
sim.setJointTargetPosition(joint02,sety-0.15)
signal_3(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_28
sim.setJointTargetPosition(joint02,sety-0.05)
signal_4(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_29
sim.setJointTargetPosition(joint02,sety+0.05)
signal_5(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setx+0.15) --pick_30
sim.setJointTargetPosition(joint02,sety+0.15)
signal_6(true)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-pick_h4)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(long_long_t)
-----------------------------------------------------------
sim.setJointTargetPosition(joint01,setpx-0.1)--fall 25
sim.wait(long_long_t)
sim.setJointTargetPosition(joint02,setpy+0.1)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3)
sim.wait(tt)
signal_1(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h3_up)
sim.wait(tt)
sim.setJointTargetPosition(jointr,60*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 26
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_2(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,120*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx-0.05)--fall 27
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_3(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,180*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 28
sim.setJointTargetPosition(joint02,setpy-0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_4(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,240*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx+0.05)--fall 29
sim.setJointTargetPosition(joint02,setpy+0.05)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h2)
sim.wait(tt)
signal_5(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
sim.setJointTargetPosition(jointr,300*math.pi/180)
sim.wait(tt)
sim.setJointTargetPosition(joint01,setpx)--fall 30
sim.setJointTargetPosition(joint02,setpy)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty-fall_h1)
sim.wait(tt)
signal_6(false)
sim.wait(tt)
sim.setJointTargetPosition(joint03,aty)
sim.wait(tt)
----------------------------------------------
sim.setJointTargetPosition(joint01,0)
sim.setJointTargetPosition(joint02,0)
setx=0.5
setpx=0.5
sim.wait(tt)
sim.setJointTargetPosition(jointr,0*math.pi/180)
sim.wait(tt)
-----------------------------------------------------------
end
end
function sysCall_cleanup()
-- Put some clean-up code here
end
-- See the user manual or the available code snippets for additional callback functions and details
吸盤程式
function sysCall_init()
modelBase1=sim.getObjectAssociatedWithScript(sim.handle_self)
robotBase1=modelBase1
while true do
robotBase1=sim.getObjectParent(robotBase1)
if robotBase1==-1 then
robotName1='Base'
break
end
robotName1=sim.getObjectName(robotBase1)
suffix,suffixlessName=sim.getNameSuffix(robotName1)
if suffixlessName=='Base' then
break
end
end
sa=sim.getObjectHandle('suctionPadSensor')
la=sim.getObjectHandle('suctionPadLoopClosureDummy1')
l2a=sim.getObjectHandle('suctionPadLoopClosureDummy2')
ba=sim.getObjectHandle('suctionPad')
suctionPadLink=sim.getObjectHandle('suctionPadLink')
local gripperBase=sim.getObjectAssociatedWithScript(sim.handle_self)
infiniteStrength=sim.getScriptSimulationParameter(sim.handle_self,'infiniteStrength')
maxPullForce=sim.getScriptSimulationParameter(sim.handle_self,'maxPullForce')
maxShearForce=sim.getScriptSimulationParameter(sim.handle_self,'maxShearForce')
maxPeelTorque=sim.getScriptSimulationParameter(sim.handle_self,'maxPeelTorque')
sim.setLinkDummy(la,-1)
sim.setObjectParent(la,ba,true)
ma=sim.getObjectMatrix(l2a,-1)
sim.setObjectMatrix(la,-1,ma)
end
function sysCall_cleanup()
sim.setLinkDummy(la,-1)
sim.setObjectParent(la,ba,true)
ma=sim.getObjectMatrix(l2a,-1)
sim.setObjectMatrix(la,-1,ma)
end
function sysCall_sensing()
parent=sim.getObjectParent(la)
local sig=sim.getIntegerSignal(robotName1 .."call_1")
if (not sig) or (sig==0) then
if (parent~=ba) then
sim.setLinkDummy(la,-1)
sim.setObjectParent(la,ba,true)
ma=sim.getObjectMatrix(l2a,-1)
sim.setObjectMatrix(la,-1,ma)
end
else
if (parent==ba) then
-- Here we want to detect a respondable shape, and then connect to it with a force sensor (via a loop closure dummy dummy link)
-- However most respondable shapes are set to "non-detectable", so "sim.readProximitySensor" or similar will not work.
-- But "sim.checkProximitySensor" or similar will work (they don't check the "detectable" flags), but we have to go through all shape objects!
index=0
while true do
shape=sim.getObjects(index,sim.object_shape_type)
if (shape==-1) then
break
end
if (shape~=ba) and (sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)~=0) and (sim.checkProximitySensor(sa,shape)==1) then
-- Ok, we found a respondable shape that was detected
-- We connect to that shape:
-- Make sure the two dummies are initially coincident:
sim.setObjectParent(la,ba,true)
ma=sim.getObjectMatrix(l2a,-1)
sim.setObjectMatrix(la,-1,ma)
-- Do the connection:
sim.setObjectParent(la,shape,true)
sim.setLinkDummy(la,l2a)
break
end
index=index+1
end
else
-- Here we have an object attached
if (infiniteStrength==false) then
-- We might have to conditionally beak it apart!
result,force,torque=sim.readForceSensor(suctionPadLink) -- Here we read the median value out of 5 values (check the force sensor prop. dialog)
if (result>0) then
breakIt=false
if (force[3]>maxPullForce) then breakIt=true end
sf=math.sqrt(force[1]*force[1]+force[2]*force[2])
if (sf>maxShearForce) then breakIt=true end
if (torque[1]>maxPeelTorque) then breakIt=true end
if (torque[2]>maxPeelTorque) then breakIt=true end
if (breakIt) then
-- We break the link:
sim.setLinkDummy(la,-1)
sim.setObjectParent(la,ba,true)
ma=sim.getObjectMatrix(l2a,-1)
sim.setObjectMatrix(la,-1,ma)
end
end
end
end
end
end
0526更新 <<
Previous Next >> 尋找控制代碼