W15 <<
Previous Next >> W17
W16
取分項目1:onshape繪製
影片
取分項目2:用 Leo Editor 以 require 導入 Lua 程式運作
程式碼(lua)
function sysCall_init()
axis1=sim.getObjectHandle('MTB_axis1')
axis2=sim.getObjectHandle('MTB_axis2')
axis3=sim.getObjectHandle('MTB_axis3')
axis4=sim.getObjectHandle('MTB_axis4')
mtb3=sim.getObjectHandle('MTB_link3Respondable')
BaseFrame=sim.getObjectHandle("BaseFrame")
block =sim.getObjectHandle("block")
rotation1 = 0
rotation2 = 0
distance3 = 0
deg = math.pi/180
end
function sysCall_actuation()
-- 吸盤往下降時, 直接貼在方塊頂部的校正值, 單位 m
-- 向下較正值為 4.2 mm
calibration = 0.0042
message, auxiliaryData=sim.getSimulatorMessage()
while message ~= -1 do
key=auxiliaryData[1]
sim.addStatusbarMessage('使用者按下 key:'..key)
if (message==sim.message_keypress) then
if (auxiliaryData[1]==97) then --r right turn in degree
-- if key r pressed axis1 angle adds 5 degrees
rotation1 = rotation1 + 5*deg
sim.setJointPosition(axis1, rotation1)
end -- if r
if (auxiliaryData[1]==115) then --l left turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation1 = rotation1 - 5*deg
sim.setJointPosition(axis1, rotation1)
end -- if l
if (auxiliaryData[1]==122) then --r right turn in degree
-- if key r pressed axis1 angle adds 5 degrees
rotation2 = rotation2 + 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if r
if (auxiliaryData[1]==120) then --l left turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation2 = rotation2 - 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if l
end -- if
message, auxiliaryData=sim.getSimulatorMessage()
end -- while
end -- function
function sysCall_sensing()
--[[
-- Read Proximity sensor (0= nothing detected, 1 = object detected)
local res = sim.readProximitySensor(proximity)
-- Check if possible to insert an new box
if (sim.getSimulationTime()-T_last_inserted > T_insert) and not hasStopped then
insertBox()
end
-- If proximity sensor detects an object, stop the belt, stop inserting objects
if res == 1 and not hasStopped then
if boolList[1] then
sim.setScriptSimulationParameter(sim.handle_self,"conveyorBeltVelocity",0)
deltaTime = sim.getSimulationTime()-T_last_inserted
hasStopped = true
else
local box = table.remove(boxList,1)
local boxDummy = table.remove(boxDummyList,1)
table.remove(boolList,1)
sim.removeObject(box)
sim.removeObject(boxDummy)
end
end
-- If proximity sensor detects nothing and belt has stopped, start belt, continue inserting
if res == 0 and hasStopped then
sim.setScriptSimulationParameter(sim.handle_self,"conveyorBeltVelocity",beltSpeed)
hasStopped = false
T_last_inserted = sim.getSimulationTime()-deltaTime
end
]]--
end
function sysCall_cleanup()
end
-- Convert a lua table into a lua syntactically correct string
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
影片
取分項目3:末端接上 force sensor 後接上標準 suction pad
程式碼(lua)
function sysCall_init()
axis1=sim.getObjectHandle('MTB_axis1')
axis2=sim.getObjectHandle('MTB_axis2')
axis3=sim.getObjectHandle('MTB_axis3')
axis4=sim.getObjectHandle('MTB_axis4')
mtb3=sim.getObjectHandle('MTB_link3Respondable')
suctionPad=sim.getObjectHandle('suctionPad')
BaseFrame=sim.getObjectHandle("BaseFrame")
block =sim.getObjectHandle("block")
rotation1 = 0
rotation2 = 0
distance3 = 0
deg = math.pi/180
end
function sysCall_actuation()
-- 吸盤往下降時, 直接貼在方塊頂部的校正值, 單位 m
-- 向下較正值為 4.2 mm
calibration = 0.0042
message, auxiliaryData=sim.getSimulatorMessage()
while message ~= -1 do
key=auxiliaryData[1]
sim.addStatusbarMessage('使用者按下 key:'..key)
if (message==sim.message_keypress) then
if (auxiliaryData[1]==97) then --r right turn in degree
-- if key r pressed axis1 angle adds 5 degrees
rotation1 = rotation1 + 5*deg
sim.setJointPosition(axis1, rotation1)
end -- if r
if (auxiliaryData[1]==115) then --l left turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation1 = rotation1 - 5*deg
sim.setJointPosition(axis1, rotation1)
end -- if l
if (auxiliaryData[1]==122) then --r right turn in degree
-- if key r pressed axis1 angle adds 5 degrees
rotation2 = rotation2 + 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if r
if (auxiliaryData[1]==120) then --l left turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation2 = rotation2 - 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if l
if (auxiliaryData[1]==112) then --p activate the suction pad
-- if key p pressed activate the suction mode
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
end -- if p
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]==100) then --d suction pad down
-- if key d pressed axis3 will down 0.01 m plus calibration
distance3 = distance3 + 0.01 + calibration
sim.setJointPosition(axis3, distance3)
end -- if d
if (auxiliaryData[1]==117) then --u suction pad up
-- if key u pressed axis3 will up 0.01 m minus calibration
-- 吸盤往下升 1 公分加上校正值
distance3 = distance3 - 0.01 - calibration
sim.setJointPosition(axis3, distance3)
end -- if u
end -- if
message, auxiliaryData=sim.getSimulatorMessage()
end -- while
end -- function
function sysCall_sensing()
--[[
-- Read Proximity sensor (0= nothing detected, 1 = object detected)
local res = sim.readProximitySensor(proximity)
-- Check if possible to insert an new box
if (sim.getSimulationTime()-T_last_inserted > T_insert) and not hasStopped then
insertBox()
end
-- If proximity sensor detects an object, stop the belt, stop inserting objects
if res == 1 and not hasStopped then
if boolList[1] then
sim.setScriptSimulationParameter(sim.handle_self,"conveyorBeltVelocity",0)
deltaTime = sim.getSimulationTime()-T_last_inserted
hasStopped = true
else
local box = table.remove(boxList,1)
local boxDummy = table.remove(boxDummyList,1)
table.remove(boolList,1)
sim.removeObject(box)
sim.removeObject(boxDummy)
end
end
-- If proximity sensor detects nothing and belt has stopped, start belt, continue inserting
if res == 0 and hasStopped then
sim.setScriptSimulationParameter(sim.handle_self,"conveyorBeltVelocity",beltSpeed)
hasStopped = false
T_last_inserted = sim.getSimulationTime()-deltaTime
end
]]--
end
function sysCall_cleanup()
end
-- Convert a lua table into a lua syntactically correct string
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
影片
取分項目4:以程式指定方塊取放之兩個位置 - (0.2, 0.7, 0.05) 與 (-0.3, -0.55, 0.05)
程式碼(lua)
function move(x,y)
a=0.4
b=0.4
c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
--c=a+b
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
--
print(deg1)
sim.setJointTargetPosition(joint,deg1)
sim.setJointTargetPosition(joint1,-deg2)
sim.setJointTargetPosition(joint2,deg3)
end
function sysCall_threadmain()
t=3
joint=sim.getObjectHandle('joint')
joint1=sim.getObjectHandle('joint1')
joint2=sim.getObjectHandle('joint2')
joint0=sim.getObjectHandle('joint0')
--joint setting
sim.setJointTargetPosition(joint,0)
sim.setJointTargetPosition(joint1,0)
sim.setJointTargetPosition(joint2,0)
sim.setJointTargetPosition(joint0,0)
--joint position
sim.wait(t)
sim.wait(t)
sim.setIntegerSignal("pad_switch",1)
--suctionPad setting
sim.setJointTargetPosition(joint0,-0.04)
--suctionPad down
sim.wait(t)
--wait 2 seconds
sim.setJointTargetPosition(joint0,0)
--suctionPad up
sim.wait(t)
while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
move(0.2,0.7)
--move to (0.2,0.7)
sim.wait(t)
sim.setIntegerSignal("pad_switch",0)
sim.wait(t)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(joint0,-0.04)
sim.wait(t)
sim.setJointTargetPosition(joint0,0)
sim.wait(t)
--Suck and release
move(-0.3,-0.55)
--move to (-0.3,0.55)
sim.wait(t)
sim.setIntegerSignal("pad_switch",0)
sim.wait(t)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(joint0,-0.04)
sim.wait(t)
sim.setJointTargetPosition(joint0,0)
sim.wait(t)
--Suck and release
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
影片
取分項目5:用 Python remote API 程式重現以迴圈方式執行 W15 兩個指定位置之方塊取放
程式碼(api)
import sim as vrep
import math
import random
import time
import math
def move(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 :
deg1_base=deg1_base+math.pi
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
vrep.simxSetJointTargetPosition(clientID,joint,deg1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint1,-deg2,opmode)
vrep.simxSetJointTargetPosition(clientID,joint2,deg3,opmode)
print ('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print ('Connected to remote API server')
res = vrep.simxAddStatusbarMessage(
clientID, "W16",
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
STREAMING = vrep.simx_opmode_streaming
vrep.simxStartSimulation(clientID, opmode)
ret,joint=vrep.simxGetObjectHandle(clientID,"joint",opmode)
ret,joint1=vrep.simxGetObjectHandle(clientID,"joint1",opmode)
ret,joint2=vrep.simxGetObjectHandle(clientID,"joint2",opmode)
ret,joint0=vrep.simxGetObjectHandle(clientID,"joint0",opmode)
vrep.simxSetJointTargetPosition(clientID,joint,0,opmode)
vrep.simxSetJointTargetPosition(clientID,joint1,0,opmode)
vrep.simxSetJointTargetPosition(clientID,joint2,0,opmode)
time.sleep(1)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint0,-0.045,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,joint0,0,opmode)
while True:
move(0.2,0.7)
time.sleep(1)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(1)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,joint0,-0.045,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,joint0,0,opmode)
move(-0.3,-0.55)
time.sleep(1)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(1)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,joint0,-0.045,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,joint0,0,opmode)
影片
W15 <<
Previous Next >> W17