W5 <<
Previous W16
(1.)新增onshape繪製之MTB_ROBOT
日期:2021/06/11
共享連結:
https://cad.onshape.com/documents/499dacb4d6975aaaa3b94b07/w/c9adb62fa78569c2a9310092/e/bb6760cfa02f9b391a093811
影片操作:
(2.)建立coppeliaSim 4.1.0 MTB_robot 場景
日期:2021/06/12
Google Drive:40823251_MTB_robot載點.zip
leo修改檔:mtb_w16_leo.zip
影片操作:
程式修改重點說明:
1.請參考鍵盤鍵碼對照表修改指令:
http://web.tnu.edu.tw/me/study/moodle/tutor/vb6/tutor/r03/index.htm
2.按R鍵時軸1會+5角度，按L鍵時軸1會-5角度。
按數字1鍵時軸2會+5角度，按數字2鍵時軸2會-5角度。
if (auxiliaryData[1]==108) 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]==114) 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]==49) then --l left1 turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation2 = rotation2 + 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if 1
if (auxiliaryData[1]==50) then --r left2 turn in degree
-- if key l pressed axis1 angle substract 5 degrees
rotation2 = rotation2 - 5*deg
sim.setJointPosition(axis2, rotation2)
end -- if 2
(3.)手臂末端加入suction pad 吸盤
註解:
1.由於此圖檔同取分項目2_ttt圖檔加入吸盤故沿用。
2.影片中軸旋轉流暢，錄影時會出現lag情況。
Google Drive:40823251_W16_取分項目3.zip
影片操作:
新增方體及吸盤程式:
BaseFrame=sim.getObjectHandle("BaseFrame")
suctionPad=sim.getObjectHandle('suctionPad')
新增suctionpad_w14.lua-----按P吸住方體，按Q放下方體。
(4.)逆向運算學函式 日期:2021/06/13
Google Drive:40823251_W16_取分項目4_方法一and方法二.zip
方法一
影片操作:
程式說明:
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.06) 吸盤向下0.06
sim.wait(2) 停頓2秒
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
moving(0.2,0.7) 要求點一
sim.wait(2)
sim.setIntegerSignal("pad_switch",0)
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.wait(2)
sim.setJointTargetPosition(jointz,-0.06)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
moving(-0.3,-0.55) 要求點二
sim.wait(5)
sim.setIntegerSignal("pad_switch",0)
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.wait(2)
sim.setJointTargetPosition(jointz,-0.06)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
副程式運算面積及角度
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)
吸盤程式(由同學40823214提供給與學習)
function sysCall_init()
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()
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)
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
方法二 更新時間:2021/06/14
影片操作:
說明:
利用w15影片中的算式去求到Q1、Q2並帶入程式。
本次操作利用知道a1和a2將值帶入程式中，並將運算程式寫入lua內自動求得Q1、Q2 。
程式:
function sysCall_threadmain() 此處為老師w15運算公式。
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
deg = 180/math.pi
a1 = 0.47 此為圖檔第一段力臂尺寸。
a2 = 0.4 此為第二段力臂尺寸。
angle=math.pi/180
function ik(x, y)
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
theta = ik(0.7, 0.2)
end
joint01=sim.getObjectHandle('joint1') 一樣設定軸。
joint02=sim.getObjectHandle('joint2')
joint03=sim.getObjectHandle('joint3')
jointz=sim.getObjectHandle('jointz')
suctionPad=sim.getObjectHandle('suctionPad')
while(true)
do
sim.setJointTargetPosition(jointz,-0.06)
sim.wait(2)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
啟動吸盤程式。
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
theta = ik(0.7, 0.2) 執行點x.y相反故位置互換。
sim.wait(2)
sim.setJointTargetPosition(joint01,-theta[1]*angle)
sim.setJointTargetPosition(joint02,theta[2]*angle)
sim.wait(2)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(4)
sim.setJointTargetPosition(jointz,-0.06)
sim.wait(2)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
theta = ik(-0.55, -0.3)
sim.wait(2)
sim.setJointTargetPosition(joint01,-theta[1]*angle)
sim.setJointTargetPosition(joint02,theta[2]*angle)
sim.wait(2)
sim.setJointTargetPosition(jointz,-0.06)
sim.wait(2)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
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
(5.)Python remote API逆向運算學函式
Google Drive:40823251_W16_取分項目五.zip
影片操作:
說明:
取分項目五可沿用取分項目四之coppeliaSim完成-將其主程式刪除。
程式:
import sim as vrep
import math
import random
import time
import math
def 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 :
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,joint01,deg1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode)
vrep.simxSetJointTargetPosition(clientID,joint03,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, "This is teach by 40823251 ",
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,joint01=vrep.simxGetObjectHandle(clientID,"joint1",opmode)
ret,joint02=vrep.simxGetObjectHandle(clientID,"joint2",opmode)
ret,joint03=vrep.simxGetObjectHandle(clientID,"joint3",opmode)
ret,jointz=vrep.simxGetObjectHandle(clientID,"jointz",opmode)
vrep.simxSetJointTargetPosition(clientID,joint01,0,opmode)
vrep.simxSetJointTargetPosition(clientID,joint02,0,opmode)
vrep.simxSetJointTargetPosition(clientID,joint03,0,opmode)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
vrep.simxSetJointTargetPosition(clientID,jointz,-0.06,opmode)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)
while True:
moving(0.2,0.7)
time.sleep(2)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(2)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
vrep.simxSetJointTargetPosition(clientID,jointz,-0.06,opmode)
time.sleep(2)
vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)
moving(-0.3,-0.55)
time.sleep(2)
vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
time.sleep(2)
vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
vrep.simxSetJointTargetPosition(clientID,jointz,-0.06,opmode)
time.sleep(2)
vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)
後方為驅動手臂移動及軸程式。同取分四。
感謝40823214_林厚宇同學利用6月13日晚上8點開直播從繪製onshape圖檔到api的詳細教導。
讓我們班上的能都能理解且跟上進度。
2021/06/12/20:00 直播檔
W5 <<
Previous