w15 <<
Previous w16
繪製過程:
joint type使用Passive mode，參見:網址
程式:
function sysCall_init()
-- 設定個軸
joint_hanld1=sim.getObjectHandle('joint1')
joint_hanld2=sim.getObjectHandle('joint2')
joint_hanld3=sim.getObjectHandle('joint3')
joint_hanld4=sim.getObjectHandle('joint4')
suctionPad=sim.getObjectHandle('suctionPad')
-- 賦值
angle=math.pi/180
angle1=0
angle2=0
d=0
a1=0.8 --手臂長度1
a2=0.8 --手臂長度2
end
function sysCall_actuation()
-- 鍵盤控制代碼
message, auxiliaryData=sim.getSimulatorMessage()
while message ~= -1 do
key=auxiliaryData[1]
sim.addStatusbarMessage('????? key:'..key)
if (message==sim.message_keypress) then
if (auxiliaryData[1]==100) then --d
-- if key a pressed joint1 left
angle1=angle1+angle
sim.setJointPosition(joint_hanld1,angle1)
end -- if d
if (auxiliaryData[1]==97) then --a
-- if key d pressed joint1 right
angle1=angle1-angle
sim.setJointPosition(joint_hanld1,angle1)
end -- if a
if (auxiliaryData[1]==101) then --e
-- if key e pressed joint2 left
angle2=angle2+angle
sim.setJointPosition(joint_hanld2,angle2)
end -- if d
if (auxiliaryData[1]==113) then --q
-- if key q pressed joint2 right
angle2=angle2-angle
sim.setJointPosition(joint_hanld2,angle2)
end -- if a
if (auxiliaryData[1]==56) then --8
-- if key 8 pressed suctionpad up and active=ture
d=d-0.05
sim.setJointPosition(joint_hanld3,d)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
end -- if 8
if (auxiliaryData[1]==53) then --5
-- if key 8 pressed suctionpad down
sim.setJointPosition(joint_hanld3,0.084)
end -- if 5
if (auxiliaryData[1]==115) then --s
-- if key s pressed active=false
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
end -- if 5
message, auxiliaryData=sim.getSimulatorMessage()
end
end
end
吸盤程式:
function sysCall_init()
s=sim.getObjectHandle('suctionPadSensor')
l=sim.getObjectHandle('suctionPadLoopClosureDummy1')
l2=sim.getObjectHandle('suctionPadLoopClosureDummy2')
b=sim.getObjectHandle('suctionPad')
suctionPadLink=sim.getObjectHandle('suctionPadLink')
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(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
function sysCall_cleanup()
--[[
sim.setLinkDummy(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
]]--
end
function sysCall_sensing()
parent=sim.getObjectParent(l)
if (sim.getScriptSimulationParameter(sim.handle_self,'active')==false) then
if (parent~=b) then
sim.setLinkDummy(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
else
if (parent==b) 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~=b) and (sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)~=0) and (sim.checkProximitySensor(s,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(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
-- Do the connection:
sim.setObjectParent(l,shape,true)
sim.setLinkDummy(l,l2)
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(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
end
end
end
end
if (sim.getSimulationState()==sim.simulation_advancing_lastbeforestop) then
sim.setLinkDummy(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
end
檔案下載:
MTB_force mode.zip
在 non-threaded child script 模式下不能使用sim.wait，所以使用threaded child script
如果要在non-threaded child script 模式下用延遲函數，可以參考:網址
程式:
function sysCall_threadmain()
-- 設定個軸
joint_hanld1=sim.getObjectHandle('joint1')
joint_hanld2=sim.getObjectHandle('joint2')
joint_hanld3=sim.getObjectHandle('joint3')
suctionPad=sim.getObjectHandle('suctionPad') --吸盤
--賦值
angle=180/math.pi
angle1=math.pi/180
d=0
x=0.7 --(m)
y=0.2 --(m)
--逆向運動學函式
function round(x, n)
n = math.pow(10, n or 0)
x = x * n
if x >= 0 then x = math.floor(x + 0.5) else x = math.ceil(x - 0.5) end
return x / n
end
-- radian to degree
deg = 180/math.pi
-- link 1 length
a1 = 0.8 --手臂長度1(m)
-- link 2 length
a2 = 0.8 --手臂長度2(m)
-- derivated based upon https://www.youtube.com/watch?v=IKOGwoJ2HLk&t=311s
function ik(x, y)
-- (x, y) need to be located inside the circle with radius a1+a2
if (x^2 + y^2) <= (a1+ a2)^2 then
q2 = -math.acos((x^2+y^2-a1^2-a2^2)/(2*a1*a2))
q1 = math.atan2(y, x) + math.atan2((a2*math.sin(q2)), (a1+a2*math.cos(q2)))
return {round(q1*deg, 4), round(q2*deg, 4)}
else
print("Over range!")
end
end
theta = ik(0.7, 0.2)
print(theta[1], theta[2])
sim.setJointTargetPosition(joint_hanld1,-theta[1]*angle1)
sim.setJointTargetPosition(joint_hanld2,theta[2]*angle1)
sim.wait(3)
--(0.2,0.7,0) 和 (-0.3,-0.55,0)來回取放
while(true)
do
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,0.06)
sim.wait(3)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,-0.06)
sim.wait(3)
theta = ik(-0.55, -0.3)
sim.wait(3)
sim.setJointTargetPosition(joint_hanld1,-theta[1]*angle1)
sim.setJointTargetPosition(joint_hanld2,theta[2]*angle1)
sim.wait(10)
sim.setJointTargetPosition(joint_hanld3,0.06)
sim.wait(3)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,-0.06)
sim.wait(3)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,0.06)
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,-0.06)
sim.wait(3)
theta = ik(0.7, 0.2)
sim.setJointTargetPosition(joint_hanld1,-theta[1]*angle1)
sim.setJointTargetPosition(joint_hanld2,theta[2]*angle1)
sim.wait(10)
sim.setJointTargetPosition(joint_hanld3,0.06)
sim.wait(3)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(3)
sim.setJointTargetPosition(joint_hanld3,-0.06)
end
end
檔案下載:
MTB_remote api python.zip
程式:
import sim as vrep
import math
import random
import time
import keyboard
print ('Start')
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('192.168.1.116', 19997, True, True, 5000, 5)
if clientID != -1:
print ('Connected to remote API server')
res = vrep.simxAddStatusbarMessage(
clientID, "40823246",
vrep.simx_opmode_oneshot)
if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
print("Could not add a message to the status bar.")
opmode = vrep.simx_opmode_oneshot_wait
angle1=math.pi/180
# radian to degree
deg = 180/math.pi
# link 1 length
a1 = 0.8
# link 2 length
a2 = 0.8
# derivated based up https://www.youtube.com/watch?v=IKOGwoJ2HLk&t=311s
def ik(x, y):
# (x, y) need to be located inside the circle with radius a1+a2
if (x**2 + y**2) <= (a1+ a2)**2:
q2 = -math.acos((x**2+y**2-a1**2-a2**2)/(2*a1*a2))
q1 = math.atan2(y, x) + math.atan2((a2*math.sin(q2)), (a1+a2*math.cos(q2)))
# The decimal point of number is rounded to the 4th place
return [round(q1*deg, 4), round(q2*deg, 4)]
else:
print("Over range!")
# end the script execution
theta = ik(0.7, 0.2)
print(theta[0], theta[1])
vrep.simxStartSimulation(clientID, opmode)
ret,joint_hanld1=vrep.simxGetObjectHandle(clientID,"joint1",opmode)
ret,joint_hanld2=vrep.simxGetObjectHandle(clientID,"joint2",opmode)
ret,joint_hanld3=vrep.simxGetObjectHandle(clientID,"joint3",opmode)
ret,suctionPad=vrep.simxGetObjectHandle(clientID,"suctionPad",opmode)
vrep.simxSetJointTargetPosition(clientID,joint_hanld1,-theta[0]*angle1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint_hanld2,theta[1]*angle1,opmode)
time.sleep(0.5)
while True:
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,0.06,opmode)
time.sleep(0.5)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,-0.06,opmode)
time.sleep(0.5)
theta = ik(-0.55, -0.3)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld1,-theta[0]*angle1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint_hanld2,theta[1]*angle1,opmode)
time.sleep(2)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,0.06,opmode)
time.sleep(0.5)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,-0.06,opmode)
time.sleep(0.5)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,0.06,opmode)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,-0.06,opmode)
time.sleep(0.5)
theta = ik(0.7, 0.2)
vrep.simxSetJointTargetPosition(clientID,joint_hanld1,-theta[0]*angle1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint_hanld2,theta[1]*angle1,opmode)
time.sleep(2)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,0.06,opmode)
time.sleep(0.5)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID,joint_hanld3,-0.06,opmode)
end
else:
print ('Failed connecting to remote API server')
print ('End')
因為在原本吸盤程式是藉由更改"active" 的值，ture or false，來決定吸盤是否生效，但我在python remote api中 找不到可以更改"active"的功能，所以使用40823214的吸盤程式，只要更改"pad_switch"的值，0 or 1，即可決定吸盤是否生效。
找到最類似的功能:simxSetBooleanParameter
吸盤程式(使用40823214的程式):
function sysCall_init()
--this is teach by 40823214
objectHandle=sim.getObjectHandle('suctionPad')
sim.setUserParameter(objectHandle,'@enable','')
modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)
robotBase=modelBase
while true do
robotBase=sim.getObjectParent(robotBase)
if robotBase==-1 then
robotName='Dobot'
break
end
robotName=sim.getObjectName(robotBase)
suffix,suffixlessName=sim.getNameSuffix(robotName)
if suffixlessName=='Dobot' then
break
end
end
s=sim.getObjectHandle('suctionPadSensor')
l=sim.getObjectHandle('suctionPadLoopClosureDummy1')
l2=sim.getObjectHandle('suctionPadLoopClosureDummy2')
b=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(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
function sysCall_cleanup()
--this is teach by 40823214
sim.setLinkDummy(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
function sysCall_sensing()
parent=sim.getObjectParent(l)
--this is teach by 40823214
local sig=sim.getIntegerSignal("pad_switch")
if (not sig) or (sig==0) then
if (parent~=b) then
sim.setLinkDummy(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
else
if (parent==b) then
index=0
while true do
shape=sim.getObjects(index,sim.object_shape_type)
if (shape==-1) then
break
end
local res,val=sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)
if (shape~=b) and (val~=0) and (sim.checkProximitySensor(s,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(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
-- Do the connection:
sim.setObjectParent(l,shape,true)
sim.setLinkDummy(l,l2)
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(l,-1)
sim.setObjectParent(l,b,true)
m=sim.getObjectMatrix(l2,-1)
sim.setObjectMatrix(l,-1,m)
end
end
end
end
end
end
--by 40823214
w15 <<
Previous