WEEK17 << Previous Next >> 心得

w16

(1) Onshape 繪製出機械手臂零件

圖檔連結: 請點我

(2) 建立 CoppeliaSim 4.1.0 MTB robot 場景

檔案下載: 請點我

function sysCall_init()
    -- do some initialization here
    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
    a2=0.8
    x=0.2
    y=0.7
end

function sysCall_actuation()
    -- put your actuation code here
    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
                message, auxiliaryData=sim.getSimulatorMessage()
                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
            end
        end  
end

(3) 手臂末端加入 components-gripper-suction pad 吸盤

這是主程式

function sysCall_init()
    -- do some initialization here
    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
    a2=0.8
    x=0.2
    y=0.7
end

function sysCall_actuation()
    -- put your actuation code here
    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

(4) 逆向運動學函式

檔案下載

這是主程式

function sysCall_threadmain()
    -- do some initialization here
    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
    a1=0.8
    a2=0.8
    x=0.7
    y=0.2
    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.8
-- link 2 length
a2 = 0.8
-- 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)
    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

這是吸盤程式

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 逆向運動學函式

檔案下載

# File created by Thibaut Royer, Epitech school
# thibaut1.royer@epitech.eu
# It intends to be an example program for the "Two wheels, one arm" educative project.
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('127.0.0.1', 19999,, True, True, 5000, 5)

if clientID != -1:
    print ('Connected to remote API server')
    
    res = vrep.simxAddStatusbarMessage(
        clientID, "40823111",
        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')

WEEK17 << Previous Next >> 心得