42 from moveit_python
import *
46 from moveit_msgs.msg
import MoveItErrorCodes, PlaceLocation
48 joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint",
"upperarm_roll_joint",
49 "elbow_flex_joint",
"forearm_roll_joint",
"wrist_flex_joint",
"wrist_roll_joint"]
50 ready_pose = [-1.393150, -0.103543, 0, -1.608378, -0.458660, -0.1, -2.611218]
53 result = interface.moveToJointPosition(joint_names, ready_pose)
54 if result.error_code.val != 1:
56 rospy.logerr(
"Move arm to ready position failed, trying again...")
57 result = interface.moveToJointPosition(joint_names, ready_pose, 0.02)
59 if __name__==
"__main__":
60 parser = argparse.ArgumentParser(description=
"Simple demo of pick and place")
61 parser.add_argument(
"--objects", help=
"Just do object perception", action=
"store_true")
62 parser.add_argument(
"--all", help=
"Just do object perception, but insert all objects", action=
"store_true")
63 parser.add_argument(
"--once", help=
"Run once.", action=
"store_true")
64 parser.add_argument(
"--ready", help=
"Move the arm to the ready position.", action=
"store_true")
65 parser.add_argument(
"--plan", help=
"Only do planning, no execution", action=
"store_true")
66 parser.add_argument(
"--x", help=
"Recommended x offset, how far out an object should roughly be.", type=float, default=0.5)
67 args, unknown = parser.parse_known_args()
69 rospy.init_node(
"pick_and_place_demo")
70 move_group = MoveGroupInterface(
"arm",
"base_link", plan_only = args.plan)
77 scene = PlanningSceneInterface(
"base_link")
78 pickplace = PickPlaceInterface(
"arm",
"gripper", plan_only = args.plan, verbose =
True)
80 rospy.loginfo(
"Connecting to basic_grasping_perception/find_objects...")
82 find_objects.wait_for_server()
83 rospy.loginfo(
"...connected")
85 while not rospy.is_shutdown():
86 goal = FindGraspableObjectsGoal()
87 goal.plan_grasps =
True 88 find_objects.send_goal(goal)
89 find_objects.wait_for_result(rospy.Duration(5.0))
90 find_result = find_objects.get_result()
92 rospy.loginfo(
"Found %d objects" % len(find_result.objects))
95 for name
in scene.getKnownCollisionObjects():
96 scene.removeCollisionObject(name,
False)
97 for name
in scene.getKnownAttachedObjects():
98 scene.removeAttachedObject(name,
False)
101 scene._colors = dict()
105 the_object_dist = 0.35
107 for obj
in find_result.objects:
109 scene.addSolidPrimitive(
"object%d"%count,
110 obj.object.primitives[0],
111 obj.object.primitive_poses[0],
114 if len(obj.grasps) < 1:
117 dx = obj.object.primitive_poses[0].position.x - args.x
118 dy = obj.object.primitive_poses[0].position.y
119 d = math.sqrt((dx * dx) + (dy * dy))
120 if d < the_object_dist:
124 if the_object ==
None:
125 rospy.logerr(
"Nothing to grasp! try again...")
129 for obj
in find_result.support_surfaces:
131 height = obj.primitive_poses[0].position.z
132 obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
134 obj.primitives[0].dimensions[2] + height]
135 obj.primitive_poses[0].position.z += -height/2.0
138 scene.addSolidPrimitive(obj.name,
140 obj.primitive_poses[0],
143 obj_name =
"object%d"%the_object
149 scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0)
150 scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0)
161 grasps = find_result.objects[the_object].grasps
162 support_surface = find_result.objects[the_object].object.support_surface
165 rospy.loginfo(
"Beginning to pick.")
166 success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
173 l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
174 l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
176 l.place_pose.pose.position.y = -l.place_pose.pose.position.y
178 l.post_place_posture = pick_result.grasp.pre_grasp_posture
179 l.pre_place_approach = pick_result.grasp.pre_grasp_approach
180 l.post_place_retreat = pick_result.grasp.post_grasp_retreat
181 places.append(copy.deepcopy(l))
183 l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
184 places.append(copy.deepcopy(l))
185 l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
186 places.append(copy.deepcopy(l))
187 l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
188 places.append(copy.deepcopy(l))
191 rospy.loginfo(
"Beginning to place.")
192 while not rospy.is_shutdown():
194 success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
200 rospy.loginfo(
"Ready...")
def move_to_ready(interface)