手臂末端加入 components-gripper-suction pad 吸盤 <<
Previous Next >> Python remote API 逆向運動學函式
逆向運動學函式
使用老師的逆運動學函式求出終點位置的位移，使MTB_robot在coppeliasim裡可以順利達到老師要求的位置
MTB_force mode.7z
第一版:
第二版:
Mtb_robot_keyboard.lua:
function sysCall_init()
-- do some initialization here
axis1=sim.getObjectHandle('MTB_axis1')
axis2=sim.getObjectHandle('MTB_axis2')
axis3=sim.getObjectHandle('MTB_axis3')
suctionPad=sim.getObjectHandle('suctionPad')
rotation1 = 0
rotation2 = 0
distance3 = 0
deg = math.pi/180
function ik(x, y)
theta = ik(0.2, 0.7)
theta = ik(-0.3, -0.55)
-- (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)}
end
print(theta[1], theta[2])
end
end
function sysCall_actuation()
calibration = 0.004
message,auxiliaryData=sim.getSimulatorMessage()
if (message==sim.message_keypress) then
print(auxiliaryData[1])--key
if (auxiliaryData[1]==string.byte(' ')) then
end
--Display keyboard key code
if (auxiliaryData[1]==97) then --a
rotation1 = rotation1 + 5*deg
sim.setJointPosition(axis1, rotation1)
end
-- Press the "a" key to turn axis1 5 degrees counterclockwise
if (auxiliaryData[1]==100) then --d
rotation1 = rotation1 - 5*deg
sim.setJointPosition(axis1, rotation1)
end
-- Press the "d" key to turn axis1 5 degrees clockwise
if (auxiliaryData[1]==119) then --w
rotation2 = rotation2 + 5*deg
sim.setJointPosition(axis2, rotation2)
end
-- Press the "w" key to turn axis2 5 degrees counterclockwise
if (auxiliaryData[1]==115) then --s
rotation2 = rotation2 - 5*deg
sim.setJointPosition(axis2, rotation2)
end
-- Press the "s" key to turn axis2 5 degrees clockwise
if (auxiliaryData[1]==101) then --e
distance3 = distance3 + calibration
sim.setJointPosition(axis3, distance3)
end
-- Press the "e" key to move down 0.01
if (auxiliaryData[1]==113) then --q
distance3 = distance3 - calibration
sim.setJointPosition(axis3, distance3)
end
-- Press the "q" key to move up 0.01
if (auxiliaryData[1]==122) then --z
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
end -- Press "z" key to activate the suction pad
if (auxiliaryData[1]==120) then --x
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
end -- Press "x" key to deactivate the suction pad
if (auxiliaryData[1]==49) then --1
V1=43.9242*deg
V2=66.007*deg
sim.setJointPosition(axis1, V1)
sim.setJointPosition(axis2, V2)
print(0.2,0.7)
end
-- Press "1" to go to (0.2, 0.7)
if (auxiliaryData[1]==50) then --2
V1=-158.2529*deg
V2=87.7894*deg
sim.setJointPosition(axis1, V1)
sim.setJointPosition(axis2, V2)
print(-0.3,-0.55)
end
-- Press "2" to go to (-0.3,- 0.55)
end
end
function sysCall_sensing()
-- put your sensing code here
end
function sysCall_cleanup()
-- do some clean-up here
end
Mtb_robot_loop.lua:
function sysCall_threadmain()
-- do some initialization here
axis1=sim.getObjectHandle('MTB_axis1')
axis2=sim.getObjectHandle('MTB_axis2')
axis3=sim.getObjectHandle('MTB_axis3')
suctionPad=sim.getObjectHandle('suctionPad')
angle=180/math.pi
angle1=math.pi/180
a1=0.467
a2=0.4
x=0.2
y=0.7
print(a)
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.467
-- link 2 length
a2 = 0.4
-- 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.3, 0.3)
sim.setJointPosition(axis1,theta[1]*angle1*0)
sim.setJointPosition(axis2,theta[2]*angle1*0)
sim.wait(5)
while(true)
do
sim.wait(5)
sim.setJointPosition(axis3, -0.03)
sim.wait(5)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
sim.wait(5)
sim.setJointPosition(axis3, 0.01)
theta = ik(0.2, 0.7)
sim.wait(5)
sim.setJointPosition(axis3, 0.01)
sim.setJointPosition(axis1,theta[1]*angle1)
sim.setJointPosition(axis2,theta[2]*angle1)
print(x,y)
print(theta[1],theta[2])
sim.wait(10)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(5)
sim.setJointPosition(axis3,0.01)
sim.wait(5)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','ture')
sim.wait(5)
sim.setJointPosition(axis3,-0.03)
sim.wait(5)
sim.setJointPosition(axis3, 0.01)
sim.wait(10)
theta = ik(-0.3, -0.55)
sim.setJointPosition(axis3, 0.01)
sim.setJointPosition(axis1,theta[1]*angle1)
sim.setJointPosition(axis2,theta[2]*angle1)
print(x,y)
print(theta[1],theta[2])
sim.wait(5)
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
sim.wait(5)
sim.setJointPosition(axis3,0.01)
end
end
手臂末端加入 components-gripper-suction pad 吸盤 <<
Previous Next >> Python remote API 逆向運動學函式