def make_places(self, init_pose, allowed_touch_objects, pre, post, set_rpy=0, roll_vals=[0], pitch_vals=[0], yaw_vals=[0]): place = PlaceLocation() place.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN) place.pre_place_approach = self.make_gripper_translation( pre[0], pre[1], pre[2]) place.post_place_retreat = self.make_gripper_translation( post[0], post[1], post[2]) place.place_pose = init_pose x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] places = [] for yaw in yaw_vals: for pitch in pitch_vals: for y in y_vals: for x in x_vals: place.place_pose.pose.position.x = init_pose.pose.position.x + x place.place_pose.pose.position.y = init_pose.pose.position.y + y q = quaternion_from_euler(0, pitch, yaw) if set_rpy: place.place_pose.pose.orientation.x = q[0] place.place_pose.pose.orientation.y = q[1] place.place_pose.pose.orientation.z = q[2] place.place_pose.pose.orientation.w = q[3] place.id = str(len(places)) place.allowed_touch_objects = allowed_touch_objects places.append(deepcopy(place)) return places
p.place_pose.pose.orientation.x = 0 p.place_pose.pose.orientation.y = 0 p.place_pose.pose.orientation.z = 0 p.place_pose.pose.orientation.w = 1 p.pre_place_approach.direction.vector.z = 1 p.pre_place_approach.direction.header.frame_id = 'endeff' p.pre_place_approach.min_distance = 0.06 p.pre_place_approach.desired_distance = 0.12 p.post_place_retreat.direction.vector.z = -1 p.post_place_retreat.direction.header.frame_id = 'endeff' p.post_place_retreat.min_distance = 0.05 p.post_place_retreat.desired_distance = 0.06 p.allowed_touch_objects = ["part"] # append the grasp to the list of grasps for i in range(3): scene.remove_attached_object("endeff", "part") rospy.sleep(2) scene.add_box("part", pos, (0.02, 0.04, 0.02)) rospy.sleep(2) result = -1 attempt = 0 while result < 0: result = arm.pick("part", grasps) print "Attempt:", attempt print "Final Result: ", result