手臂末端加入吸盤 <<
Previous Next >> Python remote API
逆向運算學函式
程式迴圈
function moving(x,y)
a=0.4
b=0.4
c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
s=(a+b+c)/2
area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
h=area/(2*c)
deg1_base=math.atan(x/y)
if x<0 and y<0 then
deg1_base=deg1_base+math.pi
end
deg1_tri=math.asin(h/a)
deg1=deg1_base+deg1_tri
deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
deg3=deg2-deg1
sim.setJointTargetPosition(joint01,deg1)
sim.setJointTargetPosition(joint02,-deg2)
sim.setJointTargetPosition(joint03,deg3)
end
function sysCall_threadmain()
joint01=sim.getObjectHandle('joint1')
joint02=sim.getObjectHandle('joint2')
joint03=sim.getObjectHandle('joint3')
jointz=sim.getObjectHandle('jointz')
sim.setJointTargetPosition(joint01,0)
sim.setJointTargetPosition(joint02,0)
sim.setJointTargetPosition(joint03,0)
sim.setJointTargetPosition(jointz,0)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.1)
sim.wait(5)
sim.setJointTargetPosition(jointz,0)
moving(0.2,0.7)
sim.wait(5)
sim.setIntegerSignal("pad_switch",0)
sim.wait(5)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.1)
sim.wait(5)
sim.setJointTargetPosition(jointz,0)
sim.wait(5)
moving(-0.3,-0.55)
sim.wait(2)
sim.setIntegerSignal("pad_switch",0)
sim.wait(5)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.1)
sim.wait(5)
sim.setJointTargetPosition(jointz,0)
sim.wait(5)
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()
axis1=sim.getObjectHandle('joint1')
axis2=sim.getObjectHandle('joint2')
axis3=sim.getObjectHandle('jointz')
axis4=sim.getObjectHandle('joint3')
suctionPad=sim.getObjectHandle('suctionPad')
BaseFrame=sim.getObjectHandle("BaseFrame")
block =sim.getObjectHandle("box")
rotation1 = 0
rotation2 = 0
distance3 = 0
deg = math.pi/180
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]==101) then --e activate the suction pad
-- if key e pressed activate the suction mode
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
end -- if e
if (auxiliaryData[1]==069) then --E activate the suction pad
-- if key E pressed activate the suction mode
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
end -- if E
if (auxiliaryData[1]==113) then --q deactivate the suction pad
-- if key q pressed deactivate the suction mode
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
end -- if q
if (auxiliaryData[1]==081) then --Q deactivate the suction pad
-- if key Q pressed deactivate the suction mode
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
end -- if Q
if (auxiliaryData[1]==068) then --D right turn in degree
-- if key D pressed axis1 angle adds 43.793 degrees
rotation1 = rotation1 + 43.793*deg
sim.setJointPosition(axis1, rotation1)
end -- if D
if (auxiliaryData[1]==065) then --A left turn in degree
-- if key A pressed axis1 angle substract 237.252 degrees
rotation1 = rotation1 +237.252*deg
sim.setJointPosition(axis1, rotation1)
end -- if A
if (auxiliaryData[1]==100) then --d right turn in degree
-- if key d pressed axis1 angle adds 66.638 degrees
rotation2 = rotation2 + 66.638*deg
sim.setJointPosition(axis2, rotation2)
end -- if d
if (auxiliaryData[1]==097) then --a right turn in degree
-- if key a pressed axis1 angle adds 154.87 degrees
rotation2 = rotation2 - 154.87*deg
sim.setJointPosition(axis2, rotation2)
end -- if a
if (auxiliaryData[1]==115) then --s suction pad down
-- if key s pressed axis3 will down 0.08 m
distance3 = distance3 + 0.08
sim.setJointPosition(axis3, distance3)
end -- if s
if (auxiliaryData[1]==119) then --w suction pad up
-- if key w pressed axis3 will up 0.08 m
-- ????? 8 ??
distance3 = distance3 - 0.08
sim.setJointPosition(axis3, distance3)
end -- if w
if (auxiliaryData[1]==99) then --c coordinate of block
blockPosition = sim.getObjectPosition(block, BaseFrame)
sim.addStatusbarMessage("???????:"..table_to_string(blockPosition))
end --if c
end -- if
message, auxiliaryData=sim.getSimulatorMessage()
end -- while
end -- function
function sysCall_sensing()
end
function sysCall_cleanup()
end
function table_to_string(tbl)
local result = "{"
for k, v in pairs(tbl) do
-- Check the key type (ignore any numerical keys - assume its an array)
if type(k) == "string" then
result = result.."[\""..k.."\"]".."="
end
-- Check the value type
if type(v) == "table" then
result = result..table_to_string(v)
elseif type(v) == "boolean" then
result = result..tostring(v)
else
v = round(v, 4)
result = result.."\""..v.."\""
end
result = result..","
end
-- Remove leading commas from the result
if result ~= "" then
result = result:sub(1, result:len()-1)
end
return result.."}"
end
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
function insertBox()
-- Generate random numbers
local rand1 = math.random()
local rand2 = math.random()
local rand3 = math.random()
-- Generate random disturbances on position and orientation
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]
-- Copy and paste box and boxDummy
local insertedObjects = sim.copyPasteObjects({box,boxDummy},0)
-- Update last inserted box time
T_last_inserted = sim.getSimulationTime()
-- Move and rotate
sim.setObjectPosition(insertedObjects[1],-1,disturbedCoordinates)
sim.setObjectOrientation(insertedObjects[1],-1,{0,0,dphi})
-- Store handles to boxes and dummies
table.insert(boxList,insertedObjects[1])
table.insert(boxDummyList,insertedObjects[2])
-- Decide if object is good or bad
local decision = math.random()
if decision <= goodPercentage then
-- Object is good, assign goodColor
sim.setShapeColor(insertedObjects[1],nil,sim.colorcomponent_ambient_diffuse,goodColor)
table.insert(boolList,true)
else
-- Object is bad, assign random color
sim.setShapeColor(insertedObjects[1],nil,sim.colorcomponent_ambient_diffuse,{rand1,rand2,rand3})
table.insert(boolList,false)
end
end
手臂末端加入吸盤 <<
Previous Next >> Python remote API