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 import rospy
5 import moveit_commander
6 import geometry_msgs.msg
7 import rosnode
8 import actionlib
9 from tf.transformations import quaternion_from_euler
10 from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
11 from moveit_msgs.msg import Constraints, OrientationConstraint
12 
13 def main():
14  rospy.init_node("sciurus17_pick_and_place_controller")
15  robot = moveit_commander.RobotCommander()
16 
17  eef_constraints = Constraints()
18  l_eef_oc = OrientationConstraint()
19  r_eef_oc = OrientationConstraint()
20 
21  # 両腕グループの制御
22  arm = moveit_commander.MoveGroupCommander("two_arm_group")
23  # 速度と加速度をゆっくりにする
24  arm.set_max_velocity_scaling_factor(0.2)
25  arm.set_max_acceleration_scaling_factor(0.2)
26  # 右ハンド初期化
27  r_gripper = actionlib.SimpleActionClient("/sciurus17/controller1/right_hand_controller/gripper_cmd", GripperCommandAction)
28  r_gripper.wait_for_server()
29  r_gripper_goal = GripperCommandGoal()
30  r_gripper_goal.command.max_effort = 2.0
31  # 左ハンド初期化
32  l_gripper = actionlib.SimpleActionClient("/sciurus17/controller2/left_hand_controller/gripper_cmd", GripperCommandAction)
33  l_gripper.wait_for_server()
34  l_gripper_goal = GripperCommandGoal()
35  l_gripper_goal.command.max_effort = 2.0
36 
37  rospy.sleep(1.0)
38 
39  # グループリスト表示
40  print("Group names:")
41  print(robot.get_group_names())
42 
43  # ステータス表示
44  print("Current state:")
45  print(robot.get_current_state())
46 
47  # 右アーム初期ポーズを表示
48  r_arm_initial_pose = arm.get_current_pose("r_link7").pose
49  print("Right arm initial pose:")
50  print(r_arm_initial_pose)
51  # 左アーム初期ポーズを表示
52  l_arm_initial_pose = arm.get_current_pose("l_link7").pose
53  print("Left arm initial pose:")
54  print(l_arm_initial_pose)
55 
56  # 何かを掴んでいた時のためにハンドを開く
57  # 右手を開く
58  r_gripper_goal.command.position = 0.9
59  r_gripper.send_goal(r_gripper_goal)
60  r_gripper.wait_for_result(rospy.Duration(1.0))
61  # 左手を開く
62  l_gripper_goal.command.position = -0.9
63  l_gripper.send_goal(l_gripper_goal)
64  l_gripper.wait_for_result(rospy.Duration(1.0))
65 
66  # SRDFに定義されている"home"の姿勢にする
67  arm.set_named_target("two_arm_init_pose")
68  arm.go()
69  # 右手を閉じる
70  r_gripper_goal.command.position = 0.0
71  r_gripper.send_goal(r_gripper_goal)
72  r_gripper.wait_for_result(rospy.Duration(1.0))
73  # 左手を閉じる
74  l_gripper_goal.command.position = 0.0
75  l_gripper.send_goal(l_gripper_goal)
76  l_gripper.wait_for_result(rospy.Duration(1.0))
77 
78  # 掴む準備をする
79  # 右腕
80  r_target_pose = geometry_msgs.msg.Pose()
81  r_target_pose.position.x = 0.18
82  r_target_pose.position.y = -0.25
83  r_target_pose.position.z = 0.2
84  q = quaternion_from_euler(3.14/2.0, 0.0, 0.0) # まずは手を下に向ける
85  r_target_pose.orientation.x = q[0]
86  r_target_pose.orientation.y = q[1]
87  r_target_pose.orientation.z = q[2]
88  r_target_pose.orientation.w = q[3]
89  # 左腕
90  l_target_pose = geometry_msgs.msg.Pose()
91  l_target_pose.position.x = 0.18
92  l_target_pose.position.y = 0.25
93  l_target_pose.position.z = 0.2
94  q = quaternion_from_euler(-3.14/2.0, 0.0, 0.0) # まずは手を下に向ける
95  l_target_pose.orientation.x = q[0]
96  l_target_pose.orientation.y = q[1]
97  l_target_pose.orientation.z = q[2]
98  l_target_pose.orientation.w = q[3]
99  # 目標ポーズ設定
100  arm.set_pose_target(r_target_pose,"r_link7")
101  arm.set_pose_target(l_target_pose,"l_link7")
102  arm.go() # 実行
103 
104  # ハンドを開く
105  # 右手を開く
106  r_gripper_goal.command.position = 0.7
107  r_gripper.send_goal(r_gripper_goal)
108  r_gripper.wait_for_result(rospy.Duration(1.0))
109  # 左手を開く
110  l_gripper_goal.command.position = -0.7
111  l_gripper.send_goal(l_gripper_goal)
112  l_gripper.wait_for_result(rospy.Duration(1.0))
113 
114  # 手を内向きにする
115  # 右腕
116  r_target_pose = geometry_msgs.msg.Pose()
117  r_target_pose.position.x = 0.18
118  r_target_pose.position.y = -0.25
119  r_target_pose.position.z = 0.08
120  q = quaternion_from_euler(3.14, 0.0, 0.0) # 手を内向きにする
121  r_target_pose.orientation.x = q[0]
122  r_target_pose.orientation.y = q[1]
123  r_target_pose.orientation.z = q[2]
124  r_target_pose.orientation.w = q[3]
125  # 左腕
126  l_target_pose = geometry_msgs.msg.Pose()
127  l_target_pose.position.x = 0.18
128  l_target_pose.position.y = 0.25
129  l_target_pose.position.z = 0.08
130  q = quaternion_from_euler(-3.14, 0.0, 0.0) # 手を内向きにする
131  l_target_pose.orientation.x = q[0]
132  l_target_pose.orientation.y = q[1]
133  l_target_pose.orientation.z = q[2]
134  l_target_pose.orientation.w = q[3]
135  # 目標ポーズ設定
136  arm.set_pose_target(r_target_pose,"r_link7")
137  arm.set_pose_target(l_target_pose,"l_link7")
138  arm.go() # 実行
139 
140  # ハンドを開く
141  # 右手を開く
142  r_gripper_goal.command.position = 1.0
143  r_gripper.send_goal(r_gripper_goal)
144  r_gripper.wait_for_result(rospy.Duration(1.0))
145  # 左手を開く
146  l_gripper_goal.command.position = -1.0
147  l_gripper.send_goal(l_gripper_goal)
148  l_gripper.wait_for_result(rospy.Duration(1.0))
149 
150  # 掴みに行く準備
151  # 右腕
152  r_target_pose = geometry_msgs.msg.Pose()
153  r_target_pose.position.x = 0.18
154  r_target_pose.position.y = -0.10
155  r_target_pose.position.z = 0.08
156  q = quaternion_from_euler(3.14, 0.0, 0.0)
157  r_target_pose.orientation.x = q[0]
158  r_target_pose.orientation.y = q[1]
159  r_target_pose.orientation.z = q[2]
160  r_target_pose.orientation.w = q[3]
161  # 左腕
162  l_target_pose = geometry_msgs.msg.Pose()
163  l_target_pose.position.x = 0.18
164  l_target_pose.position.y = 0.10
165  l_target_pose.position.z = 0.08
166  q = quaternion_from_euler(-3.14, 0.0, 0.0)
167  l_target_pose.orientation.x = q[0]
168  l_target_pose.orientation.y = q[1]
169  l_target_pose.orientation.z = q[2]
170  l_target_pose.orientation.w = q[3]
171  # 軌道制約の追加
172  # 右手を平行に寄せる
173  r_eef_pose = arm.get_current_pose("r_link7").pose
174  r_eef_oc.header.stamp = rospy.Time.now()
175  r_eef_oc.header.frame_id = "/base_link"
176  r_eef_oc.link_name ="r_link7"
177  r_eef_oc.orientation = r_eef_pose.orientation
178  r_eef_oc.absolute_x_axis_tolerance = 1.5708
179  r_eef_oc.absolute_y_axis_tolerance = 0.4
180  r_eef_oc.absolute_z_axis_tolerance = 0.7
181  r_eef_oc.weight = 1
182  # 左手を平行に寄せる
183  l_eef_pose = arm.get_current_pose("l_link7").pose
184  l_eef_oc.header.stamp = rospy.Time.now()
185  l_eef_oc.header.frame_id = "/base_link"
186  l_eef_oc.link_name ="l_link7"
187  l_eef_oc.orientation = l_eef_pose.orientation
188  l_eef_oc.absolute_x_axis_tolerance = 1.5708
189  l_eef_oc.absolute_y_axis_tolerance = 0.4
190  l_eef_oc.absolute_z_axis_tolerance = 0.7
191  l_eef_oc.weight = 1
192  eef_constraints.orientation_constraints.append(r_eef_oc)
193  eef_constraints.orientation_constraints.append(l_eef_oc)
194  arm.set_path_constraints(eef_constraints)
195  # 目標ポーズ設定
196  arm.set_pose_target(r_target_pose,"r_link7")
197  arm.set_pose_target(l_target_pose,"l_link7")
198  arm.go() # 実行
199 
200  # 掴みに行く
201  # 右腕
202  r_target_pose = geometry_msgs.msg.Pose()
203  r_target_pose.position.x = 0.18
204  r_target_pose.position.y = -0.08
205  r_target_pose.position.z = 0.08
206  q = quaternion_from_euler(3.14, 0.0, 0.0)
207  r_target_pose.orientation.x = q[0]
208  r_target_pose.orientation.y = q[1]
209  r_target_pose.orientation.z = q[2]
210  r_target_pose.orientation.w = q[3]
211  # 左腕
212  l_target_pose = geometry_msgs.msg.Pose()
213  l_target_pose.position.x = 0.18
214  l_target_pose.position.y = 0.08
215  l_target_pose.position.z = 0.08
216  q = quaternion_from_euler(-3.14, 0.0, 0.0)
217  l_target_pose.orientation.x = q[0]
218  l_target_pose.orientation.y = q[1]
219  l_target_pose.orientation.z = q[2]
220  l_target_pose.orientation.w = q[3]
221  # 目標ポーズ設定
222  arm.set_pose_target(r_target_pose,"r_link7")
223  arm.set_pose_target(l_target_pose,"l_link7")
224  arm.go() # 実行
225 
226  # ハンドを閉じる
227  # 右手を閉じる
228  r_gripper_goal.command.position = 0.4
229  r_gripper.send_goal(r_gripper_goal)
230  r_gripper.wait_for_result(rospy.Duration(1.0))
231  # 左手を閉じる
232  l_gripper_goal.command.position = -0.4
233  l_gripper.send_goal(l_gripper_goal)
234  l_gripper.wait_for_result(rospy.Duration(1.0))
235 
236  # 前へ押し出す
237  # 右腕
238  r_target_pose = geometry_msgs.msg.Pose()
239  r_target_pose.position.x = 0.35
240  r_target_pose.position.y = -0.09
241  r_target_pose.position.z = 0.08
242  q = quaternion_from_euler(3.14, 0.0, 0.0)
243  r_target_pose.orientation.x = q[0]
244  r_target_pose.orientation.y = q[1]
245  r_target_pose.orientation.z = q[2]
246  r_target_pose.orientation.w = q[3]
247  # 左腕
248  l_target_pose = geometry_msgs.msg.Pose()
249  l_target_pose.position.x = 0.35
250  l_target_pose.position.y = 0.09
251  l_target_pose.position.z = 0.08
252  q = quaternion_from_euler(-3.14, 0.0, 0.0)
253  l_target_pose.orientation.x = q[0]
254  l_target_pose.orientation.y = q[1]
255  l_target_pose.orientation.z = q[2]
256  l_target_pose.orientation.w = q[3]
257  # 目標ポーズ設定
258  arm.set_pose_target(r_target_pose,"r_link7")
259  arm.set_pose_target(l_target_pose,"l_link7")
260  arm.go() # 実行
261 
262  # ハンドを開く
263  # 右手を開く
264  r_gripper_goal.command.position = 1.5
265  r_gripper.send_goal(r_gripper_goal)
266  r_gripper.wait_for_result(rospy.Duration(1.0))
267  # 左手を開く
268  l_gripper_goal.command.position = -1.5
269  l_gripper.send_goal(l_gripper_goal)
270  l_gripper.wait_for_result(rospy.Duration(1.0))
271 
272  # 軌道制約の解除
273  arm.set_path_constraints(None)
274 
275  # 1秒待つ
276  rospy.sleep(1.0)
277 
278  # 離す
279  # 右腕
280  r_target_pose = geometry_msgs.msg.Pose()
281  r_target_pose.position.x = 0.28
282  r_target_pose.position.y = -0.20
283  r_target_pose.position.z = 0.12
284  q = quaternion_from_euler(3.14/2.0, 0.0, 0.0) # 手を下向きにして逃がす
285  r_target_pose.orientation.x = q[0]
286  r_target_pose.orientation.y = q[1]
287  r_target_pose.orientation.z = q[2]
288  r_target_pose.orientation.w = q[3]
289  # 左腕
290  l_target_pose = geometry_msgs.msg.Pose()
291  l_target_pose.position.x = 0.28
292  l_target_pose.position.y = 0.20
293  l_target_pose.position.z = 0.12
294  q = quaternion_from_euler(-3.14/2.0, 0.0, 0.0) # 手を下向きにして逃がす
295  l_target_pose.orientation.x = q[0]
296  l_target_pose.orientation.y = q[1]
297  l_target_pose.orientation.z = q[2]
298  l_target_pose.orientation.w = q[3]
299  # 目標ポーズ設定
300  arm.set_pose_target(r_target_pose,"r_link7")
301  arm.set_pose_target(l_target_pose,"l_link7")
302  arm.go() # 実行
303 
304  # ハンドを閉じる
305  # 右手を閉じる
306  r_gripper_goal.command.position = 0.4
307  r_gripper.send_goal(r_gripper_goal)
308  r_gripper.wait_for_result(rospy.Duration(1.0))
309  # 左手を閉じる
310  l_gripper_goal.command.position = -0.4
311  l_gripper.send_goal(l_gripper_goal)
312  l_gripper.wait_for_result(rospy.Duration(1.0))
313 
314  # 腕を持ち上げてターゲットから離れる
315  # 右腕
316  r_target_pose = geometry_msgs.msg.Pose()
317  r_target_pose.position.x = 0.32
318  r_target_pose.position.y = -0.25
319  r_target_pose.position.z = 0.30
320  q = quaternion_from_euler(3.14/2.0, -3.14/2.0, 0.0)
321  r_target_pose.orientation.x = q[0]
322  r_target_pose.orientation.y = q[1]
323  r_target_pose.orientation.z = q[2]
324  r_target_pose.orientation.w = q[3]
325  # 左腕
326  l_target_pose = geometry_msgs.msg.Pose()
327  l_target_pose.position.x = 0.32
328  l_target_pose.position.y = 0.25
329  l_target_pose.position.z = 0.30
330  q = quaternion_from_euler(-3.14/2.0, -3.14/2.0, 0.0)
331  l_target_pose.orientation.x = q[0]
332  l_target_pose.orientation.y = q[1]
333  l_target_pose.orientation.z = q[2]
334  l_target_pose.orientation.w = q[3]
335  # 目標ポーズ設定
336  arm.set_pose_target(r_target_pose,"r_link7")
337  arm.set_pose_target(l_target_pose,"l_link7")
338  arm.go() # 実行
339 
340  # SRDFに定義されている"home"の姿勢にする
341  arm.set_named_target("two_arm_init_pose")
342  arm.go()
343 
344  print("done")
345 
346 
347 if __name__ == '__main__':
348 
349  try:
350  if not rospy.is_shutdown():
351  main()
352  except rospy.ROSInterruptException:
353  pass
pick_and_place_two_arm_demo.main
def main()
Definition: pick_and_place_two_arm_demo.py:13

sciurus17_examples
Author(s): Daisuke Sato , Hiroyuki Nomura
autogenerated on Sun Oct 2 2022 02:21:39