pick_and_place_two_arm_demo.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Copyright 2018 RT Corporation
5 #
6 # Licensed under the Apache License, Version 2.0 (the "License");
7 # you may not use this file except in compliance with the License.
8 # You may obtain a copy of the License at
9 #
10 # http://www.apache.org/licenses/LICENSE-2.0
11 #
12 # Unless required by applicable law or agreed to in writing, software
13 # distributed under the License is distributed on an "AS IS" BASIS,
14 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 # See the License for the specific language governing permissions and
16 # limitations under the License.
17 
18 import rospy
19 import moveit_commander
20 import geometry_msgs.msg
21 import rosnode
22 import actionlib
23 from tf.transformations import quaternion_from_euler
24 from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
25 from moveit_msgs.msg import Constraints, OrientationConstraint
26 
27 def main():
28  rospy.init_node("sciurus17_pick_and_place_controller")
29  robot = moveit_commander.RobotCommander()
30 
31  eef_constraints = Constraints()
32  l_eef_oc = OrientationConstraint()
33  r_eef_oc = OrientationConstraint()
34 
35  # 両腕グループの制御
36  # Sets the controls both arms in MoveGroup
37  arm = moveit_commander.MoveGroupCommander("two_arm_group")
38  # 速度と加速度をゆっくりにする
39  # Lowers the velocity and acceleration
40  arm.set_max_velocity_scaling_factor(0.2)
41  arm.set_max_acceleration_scaling_factor(0.2)
42  # 右ハンド初期化
43  # Initializes the right hand
44  r_gripper = actionlib.SimpleActionClient("/sciurus17/controller1/right_hand_controller/gripper_cmd", GripperCommandAction)
45  r_gripper.wait_for_server()
46  r_gripper_goal = GripperCommandGoal()
47  r_gripper_goal.command.max_effort = 2.0
48  # 左ハンド初期化
49  # Initializes the left hand
50  l_gripper = actionlib.SimpleActionClient("/sciurus17/controller2/left_hand_controller/gripper_cmd", GripperCommandAction)
51  l_gripper.wait_for_server()
52  l_gripper_goal = GripperCommandGoal()
53  l_gripper_goal.command.max_effort = 2.0
54 
55  rospy.sleep(1.0)
56 
57  # グループリスト表示
58  # Displays the group names
59  print("Group names:")
60  print(robot.get_group_names())
61 
62  # ステータス表示
63  # Displays the current status
64  print("Current state:")
65  print(robot.get_current_state())
66 
67  # 右アーム初期ポーズを表示
68  # Displays the initial pose of the right arm
69  r_arm_initial_pose = arm.get_current_pose("r_link7").pose
70  print("Right arm initial pose:")
71  print(r_arm_initial_pose)
72  # 左アーム初期ポーズを表示
73  # Displays the initial pose of the left arm
74  l_arm_initial_pose = arm.get_current_pose("l_link7").pose
75  print("Left arm initial pose:")
76  print(l_arm_initial_pose)
77 
78  # 何かを掴んでいた時のためにハンドを開く
79  # Opens the hand in case it's holding on to something
80  # 右手を開く
81  # Opens the right hand
82  r_gripper_goal.command.position = 0.9
83  r_gripper.send_goal(r_gripper_goal)
84  r_gripper.wait_for_result(rospy.Duration(1.0))
85  # 左手を開く
86  # Opens the left hand
87  l_gripper_goal.command.position = -0.9
88  l_gripper.send_goal(l_gripper_goal)
89  l_gripper.wait_for_result(rospy.Duration(1.0))
90 
91  # SRDFに定義されている"home"の姿勢にする
92  # Moves to the pose declared as "home" in the SRDF
93  arm.set_named_target("two_arm_init_pose")
94  arm.go()
95  # 右手を閉じる
96  # Closes the right hand
97  r_gripper_goal.command.position = 0.0
98  r_gripper.send_goal(r_gripper_goal)
99  r_gripper.wait_for_result(rospy.Duration(1.0))
100  # 左手を閉じる
101  # Closes the leftt hand
102  l_gripper_goal.command.position = 0.0
103  l_gripper.send_goal(l_gripper_goal)
104  l_gripper.wait_for_result(rospy.Duration(1.0))
105 
106  # 掴む準備をする
107  # Prepares for grasping
108  # 右腕
109  # Right arm
110  r_target_pose = geometry_msgs.msg.Pose()
111  r_target_pose.position.x = 0.18
112  r_target_pose.position.y = -0.25
113  r_target_pose.position.z = 0.2
114  # Points the hand down
115  q = quaternion_from_euler(3.14/2.0, 0.0, 0.0) # まずは手を下に向ける
116  r_target_pose.orientation.x = q[0]
117  r_target_pose.orientation.y = q[1]
118  r_target_pose.orientation.z = q[2]
119  r_target_pose.orientation.w = q[3]
120  # 左腕
121  # Left arm
122  l_target_pose = geometry_msgs.msg.Pose()
123  l_target_pose.position.x = 0.18
124  l_target_pose.position.y = 0.25
125  l_target_pose.position.z = 0.2
126  # Points the hand down
127  q = quaternion_from_euler(-3.14/2.0, 0.0, 0.0) # まずは手を下に向ける
128  l_target_pose.orientation.x = q[0]
129  l_target_pose.orientation.y = q[1]
130  l_target_pose.orientation.z = q[2]
131  l_target_pose.orientation.w = q[3]
132  # 目標ポーズ設定
133  arm.set_pose_target(r_target_pose,"r_link7")
134  arm.set_pose_target(l_target_pose,"l_link7")
135  arm.go() # 実行
136 
137  # ハンドを開く
138  # Opens the hand
139  # 右手を開く
140  # Opens the right hand
141  r_gripper_goal.command.position = 0.7
142  r_gripper.send_goal(r_gripper_goal)
143  r_gripper.wait_for_result(rospy.Duration(1.0))
144  # 左手を開く
145  # Opens the leftt hand
146  l_gripper_goal.command.position = -0.7
147  l_gripper.send_goal(l_gripper_goal)
148  l_gripper.wait_for_result(rospy.Duration(1.0))
149 
150  # 手を内向きにする
151  # Turns the hands inward
152  # 右腕
153  # Right arm
154  r_target_pose = geometry_msgs.msg.Pose()
155  r_target_pose.position.x = 0.18
156  r_target_pose.position.y = -0.25
157  r_target_pose.position.z = 0.08
158  # Turnsthe hand inward
159  q = quaternion_from_euler(3.14, 0.0, 0.0) # 手を内向きにする
160  r_target_pose.orientation.x = q[0]
161  r_target_pose.orientation.y = q[1]
162  r_target_pose.orientation.z = q[2]
163  r_target_pose.orientation.w = q[3]
164  # 左腕
165  # Left arm
166  l_target_pose = geometry_msgs.msg.Pose()
167  l_target_pose.position.x = 0.18
168  l_target_pose.position.y = 0.25
169  l_target_pose.position.z = 0.08
170  # Turns the hand inward
171  q = quaternion_from_euler(-3.14, 0.0, 0.0) # 手を内向きにする
172  l_target_pose.orientation.x = q[0]
173  l_target_pose.orientation.y = q[1]
174  l_target_pose.orientation.z = q[2]
175  l_target_pose.orientation.w = q[3]
176  # 目標ポーズ設定
177  arm.set_pose_target(r_target_pose,"r_link7")
178  arm.set_pose_target(l_target_pose,"l_link7")
179  arm.go() # 実行
180 
181  # ハンドを開く
182  # Opens the hand
183  # 右手を開く
184  # Opens the right hand
185  r_gripper_goal.command.position = 1.0
186  r_gripper.send_goal(r_gripper_goal)
187  r_gripper.wait_for_result(rospy.Duration(1.0))
188  # 左手を開く
189  # Opens the left hand
190  l_gripper_goal.command.position = -1.0
191  l_gripper.send_goal(l_gripper_goal)
192  l_gripper.wait_for_result(rospy.Duration(1.0))
193 
194  # 掴みに行く準備
195  # Prepares to grasp
196  # 右腕
197  # Right arm
198  r_target_pose = geometry_msgs.msg.Pose()
199  r_target_pose.position.x = 0.18
200  r_target_pose.position.y = -0.10
201  r_target_pose.position.z = 0.08
202  q = quaternion_from_euler(3.14, 0.0, 0.0)
203  r_target_pose.orientation.x = q[0]
204  r_target_pose.orientation.y = q[1]
205  r_target_pose.orientation.z = q[2]
206  r_target_pose.orientation.w = q[3]
207  # 左腕
208  # Left arm
209  l_target_pose = geometry_msgs.msg.Pose()
210  l_target_pose.position.x = 0.18
211  l_target_pose.position.y = 0.10
212  l_target_pose.position.z = 0.08
213  q = quaternion_from_euler(-3.14, 0.0, 0.0)
214  l_target_pose.orientation.x = q[0]
215  l_target_pose.orientation.y = q[1]
216  l_target_pose.orientation.z = q[2]
217  l_target_pose.orientation.w = q[3]
218  # 軌道制約の追加
219  # Adds a path constraint
220  # 右手を平行に寄せる
221  # Moves the right hand towards the center
222  r_eef_pose = arm.get_current_pose("r_link7").pose
223  r_eef_oc.header.stamp = rospy.Time.now()
224  r_eef_oc.header.frame_id = "/base_link"
225  r_eef_oc.link_name ="r_link7"
226  r_eef_oc.orientation = r_eef_pose.orientation
227  r_eef_oc.absolute_x_axis_tolerance = 1.5708
228  r_eef_oc.absolute_y_axis_tolerance = 0.4
229  r_eef_oc.absolute_z_axis_tolerance = 0.7
230  r_eef_oc.weight = 1
231  # 左手を平行に寄せる
232  # Moves the left hand towards the center
233  l_eef_pose = arm.get_current_pose("l_link7").pose
234  l_eef_oc.header.stamp = rospy.Time.now()
235  l_eef_oc.header.frame_id = "/base_link"
236  l_eef_oc.link_name ="l_link7"
237  l_eef_oc.orientation = l_eef_pose.orientation
238  l_eef_oc.absolute_x_axis_tolerance = 1.5708
239  l_eef_oc.absolute_y_axis_tolerance = 0.4
240  l_eef_oc.absolute_z_axis_tolerance = 0.7
241  l_eef_oc.weight = 1
242  eef_constraints.orientation_constraints.append(r_eef_oc)
243  eef_constraints.orientation_constraints.append(l_eef_oc)
244  arm.set_path_constraints(eef_constraints)
245  # 目標ポーズ設定
246  # Sets the target pose
247  arm.set_pose_target(r_target_pose,"r_link7")
248  arm.set_pose_target(l_target_pose,"l_link7")
249  arm.go() # 実行 | Execute
250 
251  # 掴みに行く
252  # Grasp!
253  # 右腕
254  # Right arm
255  r_target_pose = geometry_msgs.msg.Pose()
256  r_target_pose.position.x = 0.18
257  r_target_pose.position.y = -0.08
258  r_target_pose.position.z = 0.08
259  q = quaternion_from_euler(3.14, 0.0, 0.0)
260  r_target_pose.orientation.x = q[0]
261  r_target_pose.orientation.y = q[1]
262  r_target_pose.orientation.z = q[2]
263  r_target_pose.orientation.w = q[3]
264  # 左腕
265  # Left arm
266  l_target_pose = geometry_msgs.msg.Pose()
267  l_target_pose.position.x = 0.18
268  l_target_pose.position.y = 0.08
269  l_target_pose.position.z = 0.08
270  q = quaternion_from_euler(-3.14, 0.0, 0.0)
271  l_target_pose.orientation.x = q[0]
272  l_target_pose.orientation.y = q[1]
273  l_target_pose.orientation.z = q[2]
274  l_target_pose.orientation.w = q[3]
275  # 目標ポーズ設定
276  # Sets the target pose
277  arm.set_pose_target(r_target_pose,"r_link7")
278  arm.set_pose_target(l_target_pose,"l_link7")
279  arm.go() # 実行 | Execute
280 
281  # ハンドを閉じる
282  # Closes the hand
283  # 右手を閉じる
284  # Closes the right hand
285  r_gripper_goal.command.position = 0.4
286  r_gripper.send_goal(r_gripper_goal)
287  r_gripper.wait_for_result(rospy.Duration(1.0))
288  # 左手を閉じる
289  # Closes the left hand
290  l_gripper_goal.command.position = -0.4
291  l_gripper.send_goal(l_gripper_goal)
292  l_gripper.wait_for_result(rospy.Duration(1.0))
293 
294  # 前へ押し出す
295  # Push forward
296  # 右腕
297  # Right arm
298  r_target_pose = geometry_msgs.msg.Pose()
299  r_target_pose.position.x = 0.35
300  r_target_pose.position.y = -0.09
301  r_target_pose.position.z = 0.08
302  q = quaternion_from_euler(3.14, 0.0, 0.0)
303  r_target_pose.orientation.x = q[0]
304  r_target_pose.orientation.y = q[1]
305  r_target_pose.orientation.z = q[2]
306  r_target_pose.orientation.w = q[3]
307  # 左腕
308  # Left arm
309  l_target_pose = geometry_msgs.msg.Pose()
310  l_target_pose.position.x = 0.35
311  l_target_pose.position.y = 0.09
312  l_target_pose.position.z = 0.08
313  q = quaternion_from_euler(-3.14, 0.0, 0.0)
314  l_target_pose.orientation.x = q[0]
315  l_target_pose.orientation.y = q[1]
316  l_target_pose.orientation.z = q[2]
317  l_target_pose.orientation.w = q[3]
318  # 目標ポーズ設定
319  # Sets the target pose
320  arm.set_pose_target(r_target_pose,"r_link7")
321  arm.set_pose_target(l_target_pose,"l_link7")
322  arm.go() # 実行 | Execute
323 
324  # ハンドを開く
325  # Opens the hand
326  # 右手を開く
327  # Opens the right hand
328  r_gripper_goal.command.position = 1.5
329  r_gripper.send_goal(r_gripper_goal)
330  r_gripper.wait_for_result(rospy.Duration(1.0))
331  # 左手を開く
332  # Opens the left hand
333  l_gripper_goal.command.position = -1.5
334  l_gripper.send_goal(l_gripper_goal)
335  l_gripper.wait_for_result(rospy.Duration(1.0))
336 
337  # 軌道制約の解除
338  # Resets the path constraints
339  arm.set_path_constraints(None)
340 
341  # 1秒待つ
342  # Waits one second
343  rospy.sleep(1.0)
344 
345  # 離す
346  # Release
347  # 右腕
348  # Right arm
349  r_target_pose = geometry_msgs.msg.Pose()
350  r_target_pose.position.x = 0.28
351  r_target_pose.position.y = -0.20
352  r_target_pose.position.z = 0.12
353  # Points the hand down
354  q = quaternion_from_euler(3.14/2.0, 0.0, 0.0) # 手を下向きにして逃がす
355  r_target_pose.orientation.x = q[0]
356  r_target_pose.orientation.y = q[1]
357  r_target_pose.orientation.z = q[2]
358  r_target_pose.orientation.w = q[3]
359  # 左腕
360  # Left arm
361  l_target_pose = geometry_msgs.msg.Pose()
362  l_target_pose.position.x = 0.28
363  l_target_pose.position.y = 0.20
364  l_target_pose.position.z = 0.12
365  # Points the hand down
366  q = quaternion_from_euler(-3.14/2.0, 0.0, 0.0) # 手を下向きにして逃がす
367  l_target_pose.orientation.x = q[0]
368  l_target_pose.orientation.y = q[1]
369  l_target_pose.orientation.z = q[2]
370  l_target_pose.orientation.w = q[3]
371  # 目標ポーズ設定
372  # Sets the target pose
373  arm.set_pose_target(r_target_pose,"r_link7")
374  arm.set_pose_target(l_target_pose,"l_link7")
375  arm.go() # 実行 | Execute
376 
377  # ハンドを閉じる
378  # Closes the hand
379  # 右手を閉じる
380  # Closes the right hand
381  r_gripper_goal.command.position = 0.4
382  r_gripper.send_goal(r_gripper_goal)
383  r_gripper.wait_for_result(rospy.Duration(1.0))
384  # 左手を閉じる
385  # Closes the left hand
386  l_gripper_goal.command.position = -0.4
387  l_gripper.send_goal(l_gripper_goal)
388  l_gripper.wait_for_result(rospy.Duration(1.0))
389 
390  # 腕を持ち上げてターゲットから離れる
391  # Raises the arm to move away from the target
392  # 右腕
393  # Right arm
394  r_target_pose = geometry_msgs.msg.Pose()
395  r_target_pose.position.x = 0.32
396  r_target_pose.position.y = -0.25
397  r_target_pose.position.z = 0.30
398  q = quaternion_from_euler(3.14/2.0, -3.14/2.0, 0.0)
399  r_target_pose.orientation.x = q[0]
400  r_target_pose.orientation.y = q[1]
401  r_target_pose.orientation.z = q[2]
402  r_target_pose.orientation.w = q[3]
403  # 左腕
404  # Left arm
405  l_target_pose = geometry_msgs.msg.Pose()
406  l_target_pose.position.x = 0.32
407  l_target_pose.position.y = 0.25
408  l_target_pose.position.z = 0.30
409  q = quaternion_from_euler(-3.14/2.0, -3.14/2.0, 0.0)
410  l_target_pose.orientation.x = q[0]
411  l_target_pose.orientation.y = q[1]
412  l_target_pose.orientation.z = q[2]
413  l_target_pose.orientation.w = q[3]
414  # 目標ポーズ設定
415  # Sets the target pose
416  arm.set_pose_target(r_target_pose,"r_link7")
417  arm.set_pose_target(l_target_pose,"l_link7")
418  arm.go() # 実行 | Execute
419 
420  # SRDFに定義されている"home"の姿勢にする
421  # Moves to the pose declared as "home" in the SRDF
422  arm.set_named_target("two_arm_init_pose")
423  arm.go()
424 
425  print("done")
426 
427 
428 if __name__ == '__main__':
429 
430  try:
431  if not rospy.is_shutdown():
432  main()
433  except rospy.ROSInterruptException:
434  pass
actionlib::SimpleActionClient
pick_and_place_two_arm_demo.main
def main()
Definition: pick_and_place_two_arm_demo.py:27

sciurus17_examples
Author(s): Daisuke Sato , Hiroyuki Nomura
autogenerated on Fri Aug 2 2024 08:37:20