10 from std_msgs.msg
import String,Int32,Int32MultiArray,Float64,Float64MultiArray,MultiArrayLayout,MultiArrayDimension
12 sys.dont_write_bytecode =
True 16 sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__),
"../../../../common/imp")) )
31 ROBOT_ID = id; ROBOT_MODEL= model
39 item = Float64MultiArray()
46 def movej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
47 nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 0):
48 return srv_move_joint(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveMode, nBlendingType, 0)
49 def amovej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
50 nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 1):
51 return srv_move_joint(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveMode, nBlendingType, 1)
54 def movel(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
55 nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 0):
56 return srv_move_line(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, 0)
57 def amovel(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
58 nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 1):
59 return srv_move_line(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, 1)
62 def movejx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0,
63 fBlendingRadius = 0.0, nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSolSpace = 0, nSyncType = 0):
64 return srv_move_jointx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, nSolSpace, 0)
65 def amovejx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0,
66 fBlendingRadius = 0.0, nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSolSpace = 0, nSyncType = 0):
67 return srv_move_jointx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, nSolSpace, 1)
70 def movec(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
71 nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, fAngle1 = 0.0, fAngle2 = 0.0, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 0):
74 return srv_move_circle(_circle_pos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, fAngle1, fAngle2, nBlendingType, 0)
75 def amovec(fTargetPos, fTargetVel, fTargetAcc, fTargetTime = 0.0, fBlendingRadius = 0.0,
76 nMoveReference = MOVE_REFERENCE_BASE, nMoveMode = MOVE_MODE_ABSOLUTE, fAngle1 = 0.0, fAngle2 = 0.0, nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, nSyncType = 0):
79 return srv_move_circle(_circle_pos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, fAngle1, fAngle2, nBlendingType, 1)
82 def movesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
83 nMoveMode = MOVE_MODE_ABSOLUTE, nSyncType = 0):
86 return srv_move_spline_joint(_spline_joint_pos, len(_spline_joint_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, 0)
87 def amovesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
88 nMoveMode = MOVE_MODE_ABSOLUTE, nSyncType = 0):
90 return srv_move_spline_joint(_spline_joint_pos, len(_spline_joint_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, 1)
93 def movesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
94 nMoveMode = MOVE_MODE_ABSOLUTE, nMoveReference = MOVE_REFERENCE_BASE, nVelOpt = SPLINE_VELOCITY_OPTION_DEFAULT, nSyncType = 0):
96 return srv_move_spline_task(_spline_task_pos, len(_spline_task_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, nMoveReference, nVelOpt, 0)
97 def amovesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
98 nMoveMode = MOVE_MODE_ABSOLUTE, nMoveReference = MOVE_REFERENCE_BASE, nVelOpt = SPLINE_VELOCITY_OPTION_DEFAULT, nSyncType = 0):
100 return srv_move_spline_task(_spline_task_pos, len(_spline_task_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, nMoveReference, nVelOpt, 1)
103 def moveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
104 nMoveMode = MOVE_MODE_ABSOLUTE, nMoveReference = MOVE_REFERENCE_BASE, nSyncType = 0):
106 return srv_move_blending(_moveb_pos, len(_moveb_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, nMoveReference, 0)
107 def amoveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime = 0.0,
108 nMoveMode = MOVE_MODE_ABSOLUTE, nMoveReference = MOVE_REFERENCE_BASE, nSyncType = 0):
110 return srv_move_blending(_moveb_pos, len(_moveb_pos), fTargetVel, fTargetAcc, fTargetTime, nMoveMode, nMoveReference, 1)
113 def move_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime = 0.0,
114 nMoveReference = MOVE_REFERENCE_TOOL, nSyncType = 0):
115 return srv_move_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime, nMoveReference, 0)
116 def amove_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime = 0.0,
117 nMoveReference = MOVE_REFERENCE_TOOL, nSyncType = 0):
118 return srv_move_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime, nMoveReference, 1)
122 nRepeat = 1, nMoveReference = MOVE_REFERENCE_TOOL, nSyncType = 0):
123 return srv_move_periodic(fAmplitude, fPeriodic, fAccelTime, nRepeat, nMoveReference, 0)
125 nRepeat = 1, nMoveReference = MOVE_REFERENCE_TOOL, nSyncType = 0):
126 return srv_move_periodic(fAmplitude, fPeriodic, fAccelTime, nRepeat, nMoveReference, 1)
134 print "shutdown time!" 135 print "shutdown time!" 136 print "shutdown time!" 138 pub_stop.publish(stop_mode=1)
141 if __name__ ==
"__main__":
143 my_robot_id =
"dsr01" 144 my_robot_model =
"m1013" 147 rospy.init_node(
'dsr_service_motion_basic_py')
148 rospy.on_shutdown(shutdown)
151 pub_stop = rospy.Publisher(
'/'+ROBOT_ID +ROBOT_MODEL+
'/stop', RobotStop, queue_size=10)
154 rospy.wait_for_service(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_joint')
156 srv_move_joint = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_joint', MoveJoint)
157 srv_move_jointx = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_jointx', MoveJointx)
159 srv_move_line = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_line', MoveLine)
160 srv_move_circle = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_circle', MoveCircle)
161 srv_move_spline_joint = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_spline_joint', MoveSplineJoint)
162 srv_move_spline_task = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_spline_task', MoveSplineTask)
164 srv_move_blending = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_blending', MoveBlending)
165 srv_move_spiral = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_spiral', MoveSpiral)
166 srv_move_periodic = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_periodic', MovePeriodic)
167 srv_move_wait = rospy.ServiceProxy(
'/'+ROBOT_ID +ROBOT_MODEL+
'/motion/move_wait', MoveWait)
170 velx =[250.0, 80.625]
171 accx =[1000.0, 322.5]
173 j1 = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0]
175 sj1 =[[10.00, 0.00, 0.00, 0.00, 10.00, 20.00], [15.00, 0.00, -10.00, 0.00, 10.00, 20.00]]
177 x1 = [0.0, 0.0, -100.0, 0.0, 0.0, 0.0]
178 x2 = [545,100,514,0,-180,0]
179 cx1 = [[544.00, 100.00, 500.00, 0.00, -180.00, 0.00], [543.00, 106.00, 479.00, 7.00, -180.00, 7.00]]
180 sx1 = [[10.00, -10.00, 20.00, 0.00, 10.00, 0.00], [15.00, 10.00, -10.00, 0.00, 10.00, 0.00]]
182 bx1 = [564.00, 200.00, 690.00, 0.00, 180.00, 0.00] + [0, 0, 0, 0, 0, 0]
183 bx_type1= [0]; bx_radius1=[40.0]
185 bx2 = [564.00, 100.00, 590.00, 0.00, 180.00, 0.00] + [564.00, 150.00, 590.00, 0.00, 180.00, 0.00]
186 bx_type2= [1]; bx_radius2=[20.0]
188 amp = [10.00, 0.00, 20.00, 0.00, 0.50, 0.00]
189 period = [1.00, 0.00, 1.50, 0.00, 0.00, 0.00]
192 while not rospy.is_shutdown():
196 movel(x1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_RELATIVE)
198 movejx(x2, 60, 30, 2, 0, MOVE_REFERENCE_BASE, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE, 2)
200 movec(cx1, velx, accx)
202 movesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE)
204 movesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE)
207 bx_type1= [0]; bx_radius1=[40.0]
208 bx_type2= [1]; bx_radius2=[20.0]
210 seg1 = bx1 + bx_type1 + bx_radius1
211 seg2 = bx2 + bx_type2 + bx_radius2
216 moveb(posb, posCnt, velx, accx)
218 move_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL)
223 amovej(j1, 60, 30, 0, 0, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE, 1)
226 amovel(x1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_RELATIVE, BLENDING_SPEED_TYPE_DUPLICATE, 1)
229 amovejx(x2, 60, 30, 2, MOVE_MODE_ABSOLUTE,MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE, 2, 1)
232 amovec(cx1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE, 1)
235 amovesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE, 1)
238 amovesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE, MOVE_REFERENCE_BASE, SPLINE_VELOCITY_OPTION_DEFAULT, 1)
241 amoveb(posb, 2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 1)
244 amove_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL, 1)
252 time= 0.0; mode=0; ref=0; radius=0.0; blendType=0; syncType=0 253 vel=30; acc=30; sol=0 254 angle1=0.0; angle2=0.0 257 p1=[0,0,0,0,0,0] #joint 258 p2=[0.0, 0.0, 90.0, 0.0, 90.0, 0.0] #joint 259 x1=[400, 500, 800.0, 0.0, 180.0, 0.0] #task 260 x2=[400, 500, 500.0, 0.0, 180.0, 0.0] #task 264 c1 = [559,434.5,651.5,0,180,0] 265 c2 = [559,434.5,251.5,0,180,0] 266 CirclePos = _ros_listToFloat64MultiArray([c1, c2]) 269 q1 =[10, -10, 20, -30, 10, 20] 270 q2 =[25, 0, 10, -50, 20, 40] 271 q3 =[50, 50, 50, 50, 50, 50] 272 q4 =[30, 10, 30, -20, 10, 60] 273 q5 =[20, 20, 40, 20, 0, 90] 274 SplinePosj = _ros_listToFloat64MultiArray([q0,q1,q2,q3,q4,q5]) 276 x1 = [600, 600, 600, 0, 175, 0] 277 x2 = [600, 750, 600, 0, 175, 0] 278 x3 = [150, 600, 450, 0, 175, 0] 279 x4 = [-300, 300, 300, 0, 175, 0] 280 x5 = [-200, 700, 500, 0, 175, 0] 281 x6 = [600, 600, 400, 0, 175, 0] 282 SplinePosx = _ros_listToFloat64MultiArray([x1, x2, x3, x4, x5, x6]) 291 period=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 296 bx11 = [370.1, 670.1, 650.1, 0.1, 180.1, 0.1]; bx12 = [370.1, 670.1, 650.1, 0.1, 180.1, 0.1]; bx_type1= [0]; bx_radius1=[21.0] 297 bx21 = [370.2, 670.2, 400.2, 0.2, 180.2, 0.2]; bx22 = [370.2, 545.2, 400.2, 0.2, 180.2, 0.2]; bx_type2= [1]; bx_radius2=[22.0] 298 bx31 = [370.3, 595.3, 400.3, 0.3, 180.3, 0.3]; bx32 = [370.3, 595.3, 400.3, 0.3, 180.3, 0.3]; bx_type3= [0]; bx_radius3=[23.0] 300 seg1 = bx11 + bx12 + bx_type1 + bx_radius1 301 seg2 = bx21 + bx22 + bx_type2 + bx_radius2 302 seg3 = bx31 + bx32 + bx_type3 + bx_radius3 304 mb_seg = _ros_listToFloat64MultiArray([seg1, seg2, seg3]) 307 while not rospy.is_shutdown(): 308 move_joint(p2, vel, acc, time, mode, radius, blendType, syncType) 309 move_jointx(x1, sol, vel, acc, time, mode, ref, radius, blendType, syncType) 310 move_line(x2, velx, accx, time, mode, ref, radius, blendType, syncType) 311 move_circle(CirclePos, velx, accx, time, mode, ref, angle1, angle2, radius, blendType, syncType) 312 move_spline_joint(SplinePosj, len(SplinePosj), vel, acc, time, mode, syncType) 313 move_spline_task(SplinePosx, len(SplinePosx), velx, accx, time, mode, ref, opt, syncType) 314 move_spiral(revolution, maxRadius, maxLength, velx, accx, time, taskAxis, ref, syncType) 315 move_periodic(amp, period, atime, repeat, ref, syncType) 316 move_blending(mb_seg, posCnt, velx, accx, time, mode, ref, syncType)
def movej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=0)
def amovel(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveReference=MOVE_REFERENCE_BASE, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=1)
def _ros_listToFloat64MultiArray(list_src)
def amove_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveReference=MOVE_REFERENCE_TOOL, nSyncType=0)
def movesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nSyncType=0)
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)
def amovec(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)
def move_spiral(nTaskAxis, fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveReference=MOVE_REFERENCE_TOOL, nSyncType=0)
def amovej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime=0.0, fBlendingRadius=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, nSyncType=1)
def amovesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nSyncType=0)
def amove_periodic(fAmplitude, fPeriodic, fAccelTime=0.0, nRepeat=1, nMoveReference=MOVE_REFERENCE_TOOL, nSyncType=0)
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)
def movesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nVelOpt=SPLINE_VELOCITY_OPTION_DEFAULT, nSyncType=0)
def move_periodic(fAmplitude, fPeriodic, fAccelTime=0.0, nRepeat=1, nMoveReference=MOVE_REFERENCE_TOOL, nSyncType=0)
def amoveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nSyncType=0)
def moveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nSyncType=0)
def amovesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime=0.0, nMoveMode=MOVE_MODE_ABSOLUTE, nMoveReference=MOVE_REFERENCE_BASE, nVelOpt=SPLINE_VELOCITY_OPTION_DEFAULT, nSyncType=0)
def amovejx(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)
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)