dance_m1013.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # ##
4 # @brief [py example simple] motion basic test for doosan robot
5 # @author Kab Kyoum Kim (kabkyoum.kim@doosan.com)
6 
7 import rospy
8 import os
9 import threading, time
10 import sys
11 sys.dont_write_bytecode = True
12 sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__),"../../../../common/imp")) ) # get import pass : DSR_ROBOT.py
13 
14 # for single robot
15 ROBOT_ID = "dsr01"
16 ROBOT_MODEL = "m1013"
17 import DR_init
18 DR_init.__dsr__id = ROBOT_ID
19 DR_init.__dsr__model = ROBOT_MODEL
20 from DSR_ROBOT import *
21 
22 def shutdown():
23  print "shutdown time!"
24  print "shutdown time!"
25  print "shutdown time!"
26 
27  pub_stop.publish(stop_mode=STOP_TYPE_QUICK)
28  return 0
29 
30 def msgRobotState_cb(msg):
31  msgRobotState_cb.count += 1
32 
33  if (0==(msgRobotState_cb.count % 100)):
34  rospy.loginfo("________ ROBOT STATUS ________")
35  print(" robot_state : %d" % (msg.robot_state))
36  print(" robot_state_str : %s" % (msg.robot_state_str))
37  print(" current_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posj[0],msg.current_posj[1],msg.current_posj[2],msg.current_posj[3],msg.current_posj[4],msg.current_posj[5]))
38  #print(" current_posx : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posx[0],msg.current_posx[1],msg.current_posx[2],msg.current_posx[3],msg.current_posx[4],msg.current_posx[5]))
39 
40  #print(" io_control_box : %d" % (msg.io_control_box))
41  ##print(" io_modbus : %d" % (msg.io_modbus))
42  ##print(" error : %d" % (msg.error))
43  #print(" access_control : %d" % (msg.access_control))
44  #print(" homming_completed : %d" % (msg.homming_completed))
45  #print(" tp_initialized : %d" % (msg.tp_initialized))
46  print(" speed : %d" % (msg.speed))
47  #print(" mastering_need : %d" % (msg.mastering_need))
48  #print(" drl_stopped : %d" % (msg.drl_stopped))
49  #print(" disconnected : %d" % (msg.disconnected))
50 msgRobotState_cb.count = 0
51 
52 def thread_subscriber():
53  rospy.Subscriber('/'+ROBOT_ID +ROBOT_MODEL+'/state', RobotState, msgRobotState_cb)
54  rospy.spin()
55  #rospy.spinner(2)
56 
57 if __name__ == "__main__":
58  rospy.init_node('dsr_simple_test_py')
59  rospy.on_shutdown(shutdown)
60 
61  t1 = threading.Thread(target=thread_subscriber)
62  t1.daemon = True
63  t1.start()
64 
65  pub_stop = rospy.Publisher('/'+ROBOT_ID +ROBOT_MODEL+'/stop', RobotStop, queue_size=10)
66 
67  set_velx(30,20) # set global task speed: 30(mm/sec), 20(deg/sec)
68  set_accx(60,40) # set global task accel: 60(mm/sec2), 40(deg/sec2)
69 
70  JReady = [0, -20, 110, 0, 60, 0]
71 
72  TCP_POS = [0, 0, 0, 0, 0, 0]
73  J00 = [-180, 0, -145, 0, -35, 0]
74 
75  J01r = [-180.0, 71.4, -145.0, 0.0, -9.7, 0.0]
76  J02r = [-180.0, 67.7, -144.0, 0.0, 76.3, 0.0]
77  J03r = [-180.0, 0.0, 0.0, 0.0, 0.0, 0.0]
78 
79  J04r = [-90.0, 0.0, 0.0, 0.0, 0.0, 0.0]
80  J04r1 = [-90.0, 30.0, -60.0, 0.0, 30.0, -0.0]
81  J04r2 = [-90.0, -45.0, 90.0, 0.0, -45.0, -0.0]
82  J04r3 = [-90.0, 60.0, -120.0, 0.0, 60.0, -0.0]
83  J04r4 = [-90.0, 0.0, -0.0, 0.0, 0.0, -0.0]
84 
85  J05r = [-144.0, -4.0, -84.8, -90.9, 54.0, -1.1]
86  J07r = [-152.4, 12.4, -78.6, 18.7, -68.3, -37.7]
87  J08r = [-90.0, 30.0, -120.0, -90.0, -90.0, 0.0]
88 
89  JEnd = [0.0, -12.6, 101.1, 0.0, 91.5, -0.0]
90 
91  dREL1 = [0, 0, 350, 0, 0, 0]
92  dREL2 = [0, 0, -350, 0, 0, 0]
93 
94  velx = [0, 0]
95  accx = [0, 0]
96 
97  vel_spi = [400, 400]
98  acc_spi = [150, 150]
99 
100  J1 = [81.2, 20.8, 127.8, 162.5, 56.1, -37.1]
101  X0 = [-88.7, 799.0, 182.3, 95.7, 93.7, 133.9]
102  X1 = [304.2, 871.8, 141.5, 99.5, 84.9, 133.4]
103  X2 = [437.1, 876.9, 362.1, 99.6, 84.0, 132.1]
104  X3 = [-57.9, 782.4, 478.4, 99.6, 84.0, 132.1]
105 
106  amp = [0, 0, 0, 30, 30, 0]
107  period = [0, 0, 0, 3, 6, 0]
108 
109  x01 = [423.6, 334.5, 651.2, 84.7, -180.0, 84.7]
110  x02 = [423.6, 34.5, 951.2, 68.2, -180.0, 68.2]
111  x03 = [423.6, -265.5, 651.2, 76.1, -180.0, 76.1]
112  x04 = [423.6, 34.5, 351.2, 81.3, -180.0, 81.3]
113  x0204c = [x02, x04]
114 
115  while not rospy.is_shutdown():
116  movej(JReady, v=20, a=20)
117 
118  movej(J1, v=0, a=0, t=3)
119  movel(X3, velx, accx, t=2.5)
120 
121  for i in range(0, 1):
122  movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
123  movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
124  movel(X0, velx, accx, t=2.5)
125  movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
126  movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
127  movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
128 
129 
130  movej(J00, v=60, a=60, t=6)
131 
132  movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
133  movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
134  movej(J03r, v=0, a=0, t=2)
135 
136  movej(J04r, v=0, a=0, t=1.5)
137  movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
138  movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
139  movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
140  movej(J04r4, v=0, a=0, t=2)
141 
142  movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
143  movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
144  movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
145 
146  movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
147  movej(J08r, v=60, a=60, t=2)
148 
149  movej(JEnd, v=60, a=60, t=4)
150 
151  move_periodic(amp, period, 0, 1, ref=DR_TOOL)
152  move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
153 
154  movel(x01, velx, accx, t=2)
155  movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
156  movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
157  movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
158  movel(x01, velx, accx, t=2)
159 
160  movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
161 
162  print 'good bye!'
dsr_service_motion_basic.movej
def movej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=0)
Definition: dsr_service_motion_basic.py:47
dsr_basic_test.move_spiral
move_spiral
Definition: dsr_basic_test.py:67
dance_m1013.shutdown
def shutdown()
Definition: dance_m1013.py:22
dance_m1013.msgRobotState_cb
def msgRobotState_cb(msg)
Definition: dance_m1013.py:30
dsr_service_motion_basic.movel
def movel(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveReference=MOVE_REFERENCE_BASE, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=0)
Definition: dsr_service_motion_basic.py:55
dance_m1013.thread_subscriber
def thread_subscriber()
Definition: dance_m1013.py:52
dsr_basic_test.move_periodic
move_periodic
Definition: dsr_basic_test.py:68
dsr_service_motion_basic.movec
def movec(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveReference=MOVE_REFERENCE_BASE, nMoveMode=MOVE_MODE_ABSOLUTE, fAngle1=0.0, fAngle2=0.0, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=0)
Definition: dsr_service_motion_basic.py:71

py
Author(s):
autogenerated on Sat May 18 2019 02:32:55