dsr_simple_test.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(" actual_mode : %d" % (msg.actual_mode))
38  print(" actual_space : %d" % (msg.actual_space))
39  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]))
40  print(" current_velj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_velj[0],msg.current_velj[1],msg.current_velj[2],msg.current_velj[3],msg.current_velj[4],msg.current_velj[5]))
41  print(" joint_abs : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.joint_abs[0],msg.joint_abs[1],msg.joint_abs[2],msg.joint_abs[3],msg.joint_abs[4],msg.joint_abs[5]))
42  print(" joint_err : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.joint_err[0],msg.joint_err[1],msg.joint_err[2],msg.joint_err[3],msg.joint_err[4],msg.joint_err[5]))
43  print(" target_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.target_posj[0],msg.target_posj[1],msg.target_posj[2],msg.target_posj[3],msg.target_posj[4],msg.target_posj[5]))
44  print(" target_velj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.target_velj[0],msg.target_velj[1],msg.target_velj[2],msg.target_velj[3],msg.target_velj[4],msg.target_velj[5]))
45  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]))
46  print(" current_velx : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_velx[0],msg.current_velx[1],msg.current_velx[2],msg.current_velx[3],msg.current_velx[4],msg.current_velx[5]))
47  print(" task_err : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.task_err[0],msg.task_err[1],msg.task_err[2],msg.task_err[3],msg.task_err[4],msg.task_err[5]))
48  print(" solution_space : %d" % (msg.solution_space))
49  sys.stdout.write(" rotation_matrix : ")
50  for i in range(0 , 3):
51  sys.stdout.write( "dim : [%d]"% i)
52  sys.stdout.write(" [ ")
53  for j in range(0 , 3):
54  sys.stdout.write("%d " % msg.rotation_matrix[i].data[j])
55  sys.stdout.write("] ")
56  print ##end line
57  print(" dynamic_tor : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.dynamic_tor[0],msg.dynamic_tor[1],msg.dynamic_tor[2],msg.dynamic_tor[3],msg.dynamic_tor[4],msg.dynamic_tor[5]))
58  print(" actual_jts : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_jts[0],msg.actual_jts[1],msg.actual_jts[2],msg.actual_jts[3],msg.actual_jts[4],msg.actual_jts[5]))
59  print(" actual_ejt : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_ejt[0],msg.actual_ejt[1],msg.actual_ejt[2],msg.actual_ejt[3],msg.actual_ejt[4],msg.actual_ejt[5]))
60  print(" actual_ett : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_ett[0],msg.actual_ett[1],msg.actual_ett[2],msg.actual_ett[3],msg.actual_ett[4],msg.actual_ett[5]))
61  print(" sync_time : %7.3f" % (msg.sync_time))
62  print(" actual_bk : %d %d %d %d %d %d" % (msg.actual_bk[0],msg.actual_bk[1],msg.actual_bk[2],msg.actual_bk[3],msg.actual_bk[4],msg.actual_bk[5]))
63  print(" actual_bt : %d %d %d %d %d " % (msg.actual_bt[0],msg.actual_bt[1],msg.actual_bt[2],msg.actual_bt[3],msg.actual_bt[4]))
64  print(" actual_mc : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_mc[0],msg.actual_mc[1],msg.actual_mc[2],msg.actual_mc[3],msg.actual_mc[4],msg.actual_mc[5]))
65  print(" actual_mt : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.actual_mt[0],msg.actual_mt[1],msg.actual_mt[2],msg.actual_mt[3],msg.actual_mt[4],msg.actual_mt[5]))
66 
67  #print digital i/o
68  sys.stdout.write(" ctrlbox_digital_input : ")
69  for i in range(0 , 16):
70  sys.stdout.write("%d " % msg.ctrlbox_digital_input[i])
71  print ##end line
72  sys.stdout.write(" ctrlbox_digital_output: ")
73  for i in range(0 , 16):
74  sys.stdout.write("%d " % msg.ctrlbox_digital_output[i])
75  print
76  sys.stdout.write(" flange_digital_input : ")
77  for i in range(0 , 6):
78  sys.stdout.write("%d " % msg.flange_digital_input[i])
79  print
80  sys.stdout.write(" flange_digital_output : ")
81  for i in range(0 , 6):
82  sys.stdout.write("%d " % msg.flange_digital_output[i])
83  print
84  #print modbus i/o
85  sys.stdout.write(" modbus_state : " )
86  if len(msg.modbus_state) > 0:
87  for i in range(0 , len(msg.modbus_state)):
88  sys.stdout.write("[" + msg.modbus_state[i].modbus_symbol)
89  sys.stdout.write(", %d] " % msg.modbus_state[i].modbus_value)
90  print
91 
92  print(" access_control : %d" % (msg.access_control))
93  print(" homming_completed : %d" % (msg.homming_completed))
94  print(" tp_initialized : %d" % (msg.tp_initialized))
95  print(" mastering_need : %d" % (msg.mastering_need))
96  print(" drl_stopped : %d" % (msg.drl_stopped))
97  print(" disconnected : %d" % (msg.disconnected))
98 msgRobotState_cb.count = 0
99 
100 def thread_subscriber():
101  rospy.Subscriber('/'+ROBOT_ID +ROBOT_MODEL+'/state', RobotState, msgRobotState_cb)
102  rospy.spin()
103  #rospy.spinner(2)
104 
105 if __name__ == "__main__":
106  rospy.init_node('dsr_simple_test_py')
107  rospy.on_shutdown(shutdown)
108 
109  t1 = threading.Thread(target=thread_subscriber)
110  t1.daemon = True
111  t1.start()
112 
113  pub_stop = rospy.Publisher('/'+ROBOT_ID +ROBOT_MODEL+'/stop', RobotStop, queue_size=10)
114 
115  set_velx(30,20) # set global task speed: 30(mm/sec), 20(deg/sec)
116  set_accx(60,40) # set global task accel: 60(mm/sec2), 40(deg/sec2)
117 
118  velx=[50, 50]
119  accx=[100, 100]
120 
121  p1= posj(0,0,0,0,0,0) #joint
122  p2= posj(0.0, 0.0, 90.0, 0.0, 90.0, 0.0) #joint
123 
124  x1= posx(400, 500, 800.0, 0.0, 180.0, 0.0) #task
125  x2= posx(400, 500, 500.0, 0.0, 180.0, 0.0) #task
126 
127  c1 = posx(559,434.5,651.5,0,180,0)
128  c2 = posx(559,434.5,251.5,0,180,0)
129 
130 
131  q0 = posj(0,0,0,0,0,0)
132  q1 = posj(10, -10, 20, -30, 10, 20)
133  q2 = posj(25, 0, 10, -50, 20, 40)
134  q3 = posj(50, 50, 50, 50, 50, 50)
135  q4 = posj(30, 10, 30, -20, 10, 60)
136  q5 = posj(20, 20, 40, 20, 0, 90)
137  qlist = [q0, q1, q2, q3, q4, q5]
138 
139  x1 = posx(600, 600, 600, 0, 175, 0)
140  x2 = posx(600, 750, 600, 0, 175, 0)
141  x3 = posx(150, 600, 450, 0, 175, 0)
142  x4 = posx(-300, 300, 300, 0, 175, 0)
143  x5 = posx(-200, 700, 500, 0, 175, 0)
144  x6 = posx(600, 600, 400, 0, 175, 0)
145  xlist = [x1, x2, x3, x4, x5, x6]
146 
147 
148  X1 = posx(370, 670, 650, 0, 180, 0)
149  X1a = posx(370, 670, 400, 0, 180, 0)
150  X1a2= posx(370, 545, 400, 0, 180, 0)
151  X1b = posx(370, 595, 400, 0, 180, 0)
152  X1b2= posx(370, 670, 400, 0, 180, 0)
153  X1c = posx(370, 420, 150, 0, 180, 0)
154  X1c2= posx(370, 545, 150, 0, 180, 0)
155  X1d = posx(370, 670, 275, 0, 180, 0)
156  X1d2= posx(370, 795, 150, 0, 180, 0)
157 
158 
159  seg11 = posb(DR_LINE, X1, radius=20)
160  seg12 = posb(DR_CIRCLE, X1a, X1a2, radius=21)
161  seg14 = posb(DR_LINE, X1b2, radius=20)
162  seg15 = posb(DR_CIRCLE, X1c, X1c2, radius=22)
163  seg16 = posb(DR_CIRCLE, X1d, X1d2, radius=23)
164  b_list1 = [seg11, seg12, seg14, seg15, seg16]
165 
166  while not rospy.is_shutdown():
167  movej(p2, vel=100, acc=100)
168  movejx(x1, vel=30, acc=60, sol=0)
169  movel(x2, velx, accx)
170  movec(c1, c2, velx, accx)
171  movesj(qlist, vel=100, acc=100)
172  movesx(xlist, vel=100, acc=100)
173  move_spiral(rev=9.5,rmax=20.0,lmax=50.0,time=20.0,axis=DR_AXIS_Z,ref=DR_TOOL)
174  move_periodic(amp =[10,0,0,0,30,0], period=1.0, atime=0.2, repeat=5, ref=DR_TOOL)
175  moveb(b_list1, vel=150, acc=250, ref=DR_BASE, mod=DR_MV_MOD_ABS)
176 
177  print 'good bye!'
dsr_simple_test.thread_subscriber
def thread_subscriber()
Definition: dsr_simple_test.py:100
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
dsr_service_motion_basic.movesj
def movesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nSyncType=0)
Definition: dsr_service_motion_basic.py:83
dsr_service_motion_basic.movejx
def movejx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveReference=MOVE_REFERENCE_BASE, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSolSpace=0, nSyncType=0)
Definition: dsr_service_motion_basic.py:63
dsr_simple_test.shutdown
def shutdown()
Definition: dsr_simple_test.py:22
dsr_simple_test.msgRobotState_cb
def msgRobotState_cb(msg)
Definition: dsr_simple_test.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
dsr_service_motion_basic.movesx
def movesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nVelOpt=SPLINE_VELOCITY_OPTION_DEFAULT, nSyncType=0)
Definition: dsr_service_motion_basic.py:94
dsr_service_motion_basic.posb
list posb
Definition: dsr_service_motion_basic.py:213
dsr_service_motion_basic.moveb
def moveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nSyncType=0)
Definition: dsr_service_motion_basic.py:104
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