stage3-ag6

  • Home
    • Site Map
    • reveal
    • blog
  • About
  • Collaborative flowchart
  • stage1
    • stage1-ag7
    • stage1-ag14
    • stage1-ag13
  • stage2
    • 階段2-ag7
    • stage2-ag12
  • stage3
    • 任務1
      • 40823125-1
      • 40823152-1
    • 任務 2
      • 40823149
      • 40823125-2
      • 40823152-2
    • w16-exam
      • 40823125
      • 40823122
      • 40823152
  • W18
    • 1.Basketball machine senser
    • 2.Basketball machine Slider
    • 3.Basketball machine by (MTB_Robot)
40823122 << Previous Next >> W18

40823152

MTB_robot onshape

onshape
stl
直接使用coppeliasim的MTB_robot的ttt檔轉成stl檔後放進onshape裡，照著原本的尺寸畫上去。
onshape
onshape教學

MTB_robot coppeliasim

W16_exam.7z

使用老師w15的鍵盤控制，使自己組裝完的MTB_robot可以鍵盤控制。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
function sysCall_init()
    -- do some initialization here
     axis1=sim.getObjectHandle('MTB_axis1')
     axis1=sim.getObjectHandle('MTB_axis1')
     axis3=sim.getObjectHandle('MTB_axis3')
     axis4=sim.getObjectHandle('MTB_axis4')    
     rotation1 = 0
     distance3 = 0
      
     deg = math.pi/180
     --sim.sJointTargetVelocity(joint,5.5)
      
      
     
      
 
end
 
function sysCall_actuation()
 calibration = 0.0042
 message,auxiliaryData=sim.getSimulatorMessage()
 if (message==sim.message_keypress) then
      print(auxiliaryData[1])--key
      if (auxiliaryData[1]==string.byte(' ')) then
      end
      if (auxiliaryData[1]==97) then--a
      rotation1 = rotation1 + 5*deg
      sim.setJointPosition(axis1, rotation1)
      end
      if (auxiliaryData[1]==100) then --d
      rotation1 = rotation1 - 5*deg
      sim.setJointPosition(axis1, rotation1)
      end
      if (auxiliaryData[1]==119) then --s
      distance3 = distance3 + 0.01 + calibration
      sim.setJointPosition(axis3, distance3)
      end
      if (auxiliaryData[1]==115) then --w
      distance3 = distance3 - 0.01 - calibration
      sim.setJointPosition(axis3, distance3)
      end
      
 end   
 
 
end
function sysCall_sensing()
    -- put your sensing code here
end
 
function sysCall_cleanup()
    -- do some clean-up here
end
 
-- See the user manual or the available code snippets for additional callback functions and details

MTB_robot組裝教學:

w16_t.7zip

參考影片:

MTB_robot add suction pad

在coppeliaSim裡的MTB_robot增加吸盤控制。(使用老師的程式)

W16_exam.7z

主程式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
function sysCall_init()
    -- do some initialization here
     axis1=sim.getObjectHandle('MTB_axis1')
     axis1=sim.getObjectHandle('MTB_axis1')
     axis3=sim.getObjectHandle('MTB_axis3')
     axis4=sim.getObjectHandle('MTB_axis4')
     suctionPad=sim.getObjectHandle('suctionPad')    
     rotation1 = 0
     distance3 = 0
      
     deg = math.pi/180
     --sim.sJointTargetVelocity(joint,5.5)
      
      
     
      
 
end
 
function sysCall_actuation()
 calibration = 0.0042
 message,auxiliaryData=sim.getSimulatorMessage()
 if (message==sim.message_keypress) then
      print(auxiliaryData[1])--key
      if (auxiliaryData[1]==string.byte(' ')) then
      end
      if (auxiliaryData[1]==97) then--a
      rotation1 = rotation1 + 5*deg
      sim.setJointPosition(axis1, rotation1)
      end
      if (auxiliaryData[1]==100) then --d
      rotation1 = rotation1 - 5*deg
      sim.setJointPosition(axis1, rotation1)
      end
      if (auxiliaryData[1]==119) then --s
      distance3 = distance3 + 0.01 + calibration
      sim.setJointPosition(axis3, distance3)
      end
      if (auxiliaryData[1]==115) then --w
      distance3 = distance3 - 0.01 - calibration
      sim.setJointPosition(axis3, distance3)
      end
      if (auxiliaryData[1]==112) then --p activate the suction pad
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','true')
      end -- if p
      if (auxiliaryData[1]==113) then --q deactivate the suction pad
sim.setScriptSimulationParameter(sim.getScriptAssociatedWithObject(suctionPad),'active','false')
      end -- if q
      
 end   
 
 
end
function sysCall_sensing()
    -- put your sensing code here
end
 
function sysCall_cleanup()
    -- do some clean-up here
end
 
-- See the user manual or the available code snippets for additional callback functions and details

吸盤程式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
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
end



教學影片:

如何在cippeliasim裡的MTB_robot安裝吸盤。

W16_t.7z

MTB_robot add IK

使用老師的逆運動學函式求出終點位置的位移，使MTB_robot在coppeliasim裡可以順利達到老師要求的位置

W16_exam.7z error版本

W16_exam.7z

loop迴圈影片:

因為迴圈做不出來，發現到46號網站的內容，找出wait問題點後的操作狀態

參考40823246

正確位置的影片(鍵盤)

錯誤位置的影片，但是有求出a1與a2。

老師的程式

修改後的程式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
-- The decimal point of number x is rounded to the nth place
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 the script execution
        os.exit()
    end
end
  
theta = ik(-0.3, -0.55)
  
print(theta[1], theta[2])

lua 編譯網站:

目前吸盤無法放下，只能跟著連桿移動

以下是參考40823246的remote api 來達成的結果。

W16_exam

python 程式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
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', 19997, True, True, 5000, 5)
  
if clientID != -1:
    print ('Connected to remote API server')
      
    res = vrep.simxAddStatusbarMessage(
        clientID, "40823152",
        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.468
    # link 2 length
    a2 = 0.4
    # 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.2, 0.7)
  
    print(theta[0], theta[1])
 
 
    ret,axis1=vrep.simxGetObjectHandle(clientID,"MTB_axis1",opmode)
    ret,axis2=vrep.simxGetObjectHandle(clientID,"MTB_axis2",opmode)
    ret,axis3=vrep.simxGetObjectHandle(clientID,"MTB_axis3",opmode)
    ret,suctionPad=vrep.simxGetObjectHandle(clientID,"suctionPad",opmode)
    vrep.simxSetJointTargetPosition(clientID,axis1,theta[0]*angle1,opmode)
    vrep.simxSetJointTargetPosition(clientID,axis2,theta[1]*angle1,opmode)
    
    time.sleep(0.5)
    while True:
            vrep.simxSetJointPosition(clientID,axis3,0.001,opmode)
            time.sleep(0.5)
            theta = ik(0.2, 0.7)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis1,theta[0]*angle1,opmode)
            vrep.simxSetJointPosition(clientID,axis2,theta[1]*angle1,opmode)
            time.sleep(2)
            vrep.simxSetIntegerSignal(clientID,"suctionPad", 0,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis3,-0.001,opmode)
            time.sleep(0.5)
            vrep.simxSetIntegerSignal(clientID,"suctionPad", 0,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis3,0.001,opmode)
            time.sleep(0.5)
            theta = ik(-0.3, -0.55)
            vrep.simxSetJointPosition(clientID,axis1,theta[0]*angle1,opmode)
            vrep.simxSetJointPosition(clientID,axis2,theta[1]*angle1,opmode)
            time.sleep(2)
            vrep.simxSetIntegerSignal(clientID,"suctionPad", 1,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis3,-0.001,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis3,0.002,opmode)
    end
          
else:
    print ('Failed connecting to remote API server')
    print ('End')

MTB_robo use require

將2到4的步驟新增require操作MTB_robot，並簡單介紹使用方法

W16_exam.7z

老師的leo


40823122 << Previous Next >> W18

Copyright © All rights reserved | This template is made with by Colorlib