W16 exam

google drive連結:https://drive.google.com/drive/folders/1yGLWRymmOfIucmTXto5U4iZMExDvs5NC?usp=sharing

1.Onshape 零組件繪製

圖檔連結https://cad.onshape.com/documents/ba69fd6cd01409e870faec2f/w/2f154f1a806882e4b16bc221/e/2648d28f71480c3b30ffdeff

2.建立 CoppeliaSim 4.1.0 MTB robot 場景

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

方向鍵控制手臂，3:吸取 4:吸盤向上 5:放下

function sysCall_init()
    Revolute_joint=sim.getObjectHandle('Revolute_joint')
    Revolute_joint1=sim.getObjectHandle('Revolute_joint1')
    Prismatic_joint=sim.getObjectHandle('Prismatic_joint')
    Revolute_joint0=sim.getObjectHandle('Revolute_joint0')
    sim.setJointTargetPosition(Revolute_joint,0)
    sim.setJointTargetPosition(Revolute_joint1,0)
    sim.setJointTargetPosition(Revolute_joint0,0)
    sim.setJointTargetPosition(Prismatic_joint,0)
    deg1=0
    deg2=0
    deg3=0
end

function sysCall_actuation()
    message,auxiliaryData=sim.getSimulatorMessage()
    while message~=-1 do
        if (message==sim.message_keypress) then
            if (auxiliaryData[1]==2009) then
            deg1=deg1+1
            deg3=deg2-deg1
            sim.setJointTargetPosition(Revolute_joint,deg1*math.pi/180)
            end
            if (auxiliaryData[1]==2010) then
            deg1=deg1-1
            deg3=deg2-deg1
            sim.setJointTargetPosition(Revolute_joint,deg1*math.pi/180)
            end
            if (auxiliaryData[1]==2007) then
            deg2=deg2+1
            deg3=deg1-deg2
            sim.setJointTargetPosition(Revolute_joint1,deg2*math.pi/180)
            end
            if (auxiliaryData[1]==2008) then
            deg2=deg2-1
            deg3=deg1-deg2
            sim.setJointTargetPosition(Revolute_joint1,deg2*math.pi/180)
            end
            if (auxiliaryData[1]==51) then
                sim.setJointTargetPosition(Prismatic_joint,-0.055)
                sim.setIntegerSignal("pad_switch",1)
            end
            if(auxiliaryData[1]==52) then
                sim.setJointTargetPosition(Prismatic_joint,0)
            end
            if(auxiliaryData[1]==53) then
                sim.setIntegerSignal("pad_switch",0)
            end
            sim.setJointTargetPosition(Revolute_joint0,deg3*math.pi/180)
        end
    message,auxiliaryData=sim.getSimulatorMessage()
    end
end

4.逆向運動學函式

function moving(x,y)  
    a=0.467
    b=0.401
    c=math.pow(math.pow(x,2)+math.pow(y,2),0.5)
    s=(a+b+c)*0.5
    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(deg3)
    sim.setJointTargetPosition(Revolute_joint,deg1)
    sim.setJointTargetPosition(Revolute_joint1,-deg2)
    sim.setJointTargetPosition(Revolute_joint0,deg3)
    
end

function pick_and_place(x,y)
    sim.setIntegerSignal("pad_switch",0)
    sim.wait(t)
    moving(0,0.868)
    sim.wait(t)
    moving(x,y)
    sim.wait(t)
    sim.setIntegerSignal("pad_switch",1)
    sim.setJointTargetPosition(Prismatic_joint,-0.055)
    sim.wait(t)
    sim.setJointTargetPosition(Prismatic_joint,0)
    sim.wait(t)
end

function sysCall_threadmain()
    t=5
    Revolute_joint=sim.getObjectHandle('Revolute_joint')
    Revolute_joint1=sim.getObjectHandle('Revolute_joint1')
    Prismatic_joint=sim.getObjectHandle('Prismatic_joint')
    Revolute_joint0=sim.getObjectHandle('Revolute_joint0')
    sim.setJointTargetPosition(Revolute_joint,0)
    sim.setJointTargetPosition(Revolute_joint1,0)
    sim.setJointTargetPosition(Revolute_joint0,0)
    sim.setIntegerSignal("pad_switch",1)
    sim.setJointTargetPosition(Prismatic_joint,-0.055)
    sim.wait(t)
    sim.setJointTargetPosition(Prismatic_joint,0)
    sim.wait(t)
    while sim.getSimulationState()~=sim.simulation_advancing_abouttostopre do
        x=0.2
        y=0.7
        moving(x,y)
        sim.wait(t)
        pick_and_place(x,y)
        x=-0.3
        y=-0.55
        moving(x,y)
        sim.wait(t)
        pick_and_place(x,y)
    end
end

5.Python remote API 逆向運動學函式

import sim as vrep
import math
import random
import time
import math

def moving(x,y):
    a=0.467
    b=0.401
    c=math.pow(math.pow(x,2)+math.pow(y,2),0.5)
    s=(a+b+c)*0.5
    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,Revolute_joint,deg1,opmode)
    vrep.simxSetJointTargetPosition(clientID,Revolute_joint1,(-deg2),opmode)
    vrep.simxSetJointTargetPosition(clientID,Revolute_joint0,deg3,opmode)

def pick_and_place(x,y):
    vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
    time.sleep(tt)
    moving(0,0.868)
    time.sleep(tt)
    moving(x,y)
    time.sleep(tt)
    vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
    vrep.simxSetJointTargetPosition(clientID,Prismatic_joint,-0.055,opmode)
    time.sleep(tt)
    vrep.simxSetJointTargetPosition(clientID,Prismatic_joint,0,opmode)
    time.sleep(tt)



print ('Start')
 
vrep.simxFinish(-1)
 
clientID = vrep.simxStart('192.168.0.9', 19997, True, True, 5000, 5)
  
if clientID != -1:
    print ('Connected to remote API server')
      
    res = vrep.simxAddStatusbarMessage(
        clientID, "40823232",
        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,Revolute_joint=vrep.simxGetObjectHandle(clientID,"Revolute_joint",opmode)
    ret,Revolute_joint1=vrep.simxGetObjectHandle(clientID,"Revolute_joint1",opmode)
    ret,Revolute_joint0=vrep.simxGetObjectHandle(clientID,"Revolute_joint0",opmode)
    ret,Prismatic_joint=vrep.simxGetObjectHandle(clientID,"Prismatic_joint",opmode)
    tt=0.5
 

    while True :
        vrep.simxSetJointTargetPosition(clientID,Revolute_joint,0,opmode)
        vrep.simxSetJointTargetPosition(clientID,Revolute_joint1,0,opmode)
        vrep.simxSetJointTargetPosition(clientID,Revolute_joint0,0,opmode)
        vrep.simxSetJointTargetPosition(clientID,Prismatic_joint,-0.055,opmode)
        vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
        time.sleep(tt)
        vrep.simxSetJointTargetPosition(clientID,Prismatic_joint,0,opmode)
        time.sleep(tt)
        while True:
            x=0.2
            y=0.7
            moving(x,y)
            time.sleep(tt)
            pick_and_place(x,y)
            x=-0.3
            y=-0.55
            moving(x,y)
            time.sleep(tt)
            pick_and_place(x,y)

W15 << Previous Next >> W17