def commander(): global past_reads rospy.init_node('rubik_camera', anonymous=True) rospy.Subscriber("/usb_cam/image_raw", Image, callback) pub = rospy.Publisher("cube_configuration", String, queue_size=1) # Gets a handle on the robot that MoveIt is controlling robot = RobotCommander() rospy.sleep(1) # Picks a group called "manipulator" to work with # arm = robot.get_group("manipulator") arm.set_max_velocity_scaling_factor(1.0) arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.005) go_home(arm) pick_rubik(arm, robot, "right") go_home(arm) for i in range(5): read(arm, i) go_home(arm) place_rubik(arm, robot, "right") go_home(arm) pick_rubik(arm, robot, "left") go_home(arm) for i in range(5): read(arm, i) go_home(arm) place_rubik(arm, robot, "left") go_home(arm) print(str(past_reads)) pub.publish(reads_to_cube()) rospy.sleep(1)
class PythonTimeParameterizationTest(unittest.TestCase): PLANNING_GROUP = "arm" @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def plan(self): start_pose = self.group.get_current_pose().pose goal_pose = self.group.get_current_pose().pose goal_pose.position.z -= 0.1 (plan, fraction) = self.group.compute_cartesian_path([start_pose, goal_pose], 0.005, 0.0) self.assertEqual(fraction, 1.0, "Cartesian path plan failed") return plan def time_parameterization(self, plan): ref_state = self.commander.get_current_state() retimed_plan = self.group.retime_trajectory( ref_state, plan, velocity_scaling_factor=0.1) return retimed_plan def test_plan_and_time_parameterization(self): plan = self.plan() retimed_plan = self.time_parameterization(plan)
class PythonTimeParameterizationTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def plan(self): start_pose = self.group.get_current_pose().pose goal_pose = self.group.get_current_pose().pose goal_pose.position.z -= 0.1 (plan, fraction) = self.group.compute_cartesian_path( [start_pose, goal_pose], 0.005, 0.0 ) self.assertEqual(fraction, 1.0, "Cartesian path plan failed") return plan def time_parameterization(self, plan, algorithm): ref_state = self.commander.get_current_state() retimed_plan = self.group.retime_trajectory( ref_state, plan, velocity_scaling_factor=0.1, acceleration_scaling_factor=0.1, algorithm=algorithm, ) return retimed_plan def test_plan_and_time_parameterization(self): plan = self.plan() retimed_plan = self.time_parameterization( plan, "iterative_time_parameterization" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization( plan, "iterative_spline_parameterization" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization( plan, "time_optimal_trajectory_generation" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization(plan, "") self.assertTrue( len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm" )
class PythonMoveitCommanderNsTest(unittest.TestCase): PLANNING_GROUP = "manipulator" PLANNING_NS = "test_ns/" @classmethod def setUpClass(self): self.commander = RobotCommander( "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS) self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue( np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res), ) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2, ) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting( [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) success1, plan1, time1, err1 = self.plan(current + 0.2) success2, plan2, time2, err2 = self.plan(current + 0.2) self.assertTrue(success1) self.assertTrue(success2) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again success3, plan3, time3, err3 = self.plan(current) self.assertTrue(success3) self.assertTrue(self.group.execute(plan3))
class PythonMoveitCommanderTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res)) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2,) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) success1, plan1, time1, err1 = self.plan(current + 0.2) success2, plan2, time2, err2 = self.plan(current + 0.2) self.assertTrue(success1) self.assertTrue(success2) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again success3, plan3, time3, err3 = self.plan(current) self.assertTrue(success3) self.assertTrue(self.group.execute(plan3)) def test_planning_scene_interface(self): planning_scene = PlanningSceneInterface()
class CokeCanPickAndPlace: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y + 0.02 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
class PythonMoveitCommanderTest(unittest.TestCase): PLANNING_GROUP = "manipulator" JOINT_NAMES = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", ] @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def test_enforce_bounds_empty_state(self): in_msg = RobotState() with self.assertRaises(genpy.DeserializationError): self.group.enforce_bounds(in_msg) def test_enforce_bounds(self): in_msg = RobotState() in_msg.joint_state.header.frame_id = "base_link" in_msg.joint_state.name = self.JOINT_NAMES in_msg.joint_state.position = [0] * 6 in_msg.joint_state.position[0] = 1000 out_msg = self.group.enforce_bounds(in_msg) self.assertEqual(in_msg.joint_state.position[0], 1000) self.assertLess(out_msg.joint_state.position[0], 1000) def test_get_current_state(self): expected_state = RobotState() expected_state.joint_state.header.frame_id = "base_link" expected_state.multi_dof_joint_state.header.frame_id = "base_link" expected_state.joint_state.name = self.JOINT_NAMES expected_state.joint_state.position = [0] * 6 self.assertEqual(self.group.get_current_state(), expected_state) def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue( np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res), ) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2,) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting( [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} ) self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) success1, plan1, time1, err1 = self.plan(current + 0.2) success2, plan2, time2, err2 = self.plan(current + 0.2) self.assertTrue(success1) self.assertTrue(success2) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again success3, plan3, time3, err3 = self.plan(current) self.assertTrue(success3) self.assertTrue(self.group.execute(plan3)) def test_gogogo(self): current_joints = np.asarray(self.group.get_current_joint_values()) self.group.set_joint_value_target(current_joints) self.assertTrue(self.group.go(True)) self.assertTrue(self.group.go(current_joints)) self.assertTrue(self.group.go(list(current_joints))) self.assertTrue(self.group.go(tuple(current_joints))) self.assertTrue( self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints)) ) self.group.remember_joint_values("current") self.assertTrue(self.group.go("current")) current_pose = self.group.get_current_pose() self.assertTrue(self.group.go(current_pose)) def test_planning_scene_interface(self): planning_scene = PlanningSceneInterface()
class EdPick: def __init__(self): allowed_planning_time = 0.0 ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear" ARM_GROUP_NAME = "arm_torso" GRIPPER_GROUP = "gripper" REFERENCE_FRAME = "base_link" PLANNER = "RRTConnectkConfigDefault" #PLANNER = "RRTstarkConfigDefault" MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters" result = False upright_constraints = None robot = None arm = None arm_and_linear = None database = None scene = None grasp_generator = None place_generator = None marker_publisher = None self.robot = RobotCommander() self.arm = self.robot.get_group("arm_torso") self.gripper = self.robot.get_group(GRIPPER_GROUP) self.scene = PlanningSceneInterface() self.pickup_object = RecognizedObject("part") self.table = EntityInfo() def get_object(self): rospy.wait_for_service('/ed/kinect/update') self.update_client = rospy.ServiceProxy('/ed/kinect/update', Update) self.request_update = UpdateRequest() self.request_update.area_description = "on_top_of dinner-table" self.result_update = self.update_client(self.request_update) print "Found IDs: " % self.result_update.new_ids rospy.wait_for_service('/ed/simple_query') self.query_client = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) self.query_request = SimpleQueryRequest() self.query_request.id = self.result_update.new_ids[0] self.query_response = self.query_client(self.query_request) rospy.wait_for_service('/ed/simple_query') self.ed_simple = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) simpleRequest = SimpleQueryRequest() simpleRequest.id = 'dinner-table' self.table = self.ed_simple(simpleRequest).entities[0] print self.table.pose def pick_object(self): # self.object = self.query_response.entities self.pickup_object = RecognizedObject("part") tmpPoseStamped = PoseStamped() tmpPoseStamped.header.frame_id = '/map' tmpPoseStamped.header.stamp = rospy.Time(0) tmpPoseStamped.pose = self.query_response.entities[0].pose self.pickup_obj.pose = tmpPoseStamped self.value_x = [] self.value_y = [] self.value_z = [] for j in range(0, len(self.query_response.entities[0].convex_hull)): self.value_x.append( self.query_response.entities[0].convex_hull[j].x) self.value_y.append( self.query_response.entities[0].convex_hull[j].y) self.value_z.append( self.query_response.entities[0].convex_hull[j].z) self.dim_x = max(self.value_x) - min(self.value_x) self.dim_y = max(self.value_y) - min(self.value_y) self.dim_z = max(self.value_z) - min(self.value_z) self.pickup_object.dimensions = (self.dim_x, self.dim_y, self.dim_z) self.add_to_planning_scene(self.pickup_object) self.grasps = self.generate_grasps_for_object(self.pickup_object) if not self.pickup(self.pickup_object, self.grasps) == -1: print "Executed pick action" self.attach_object_to_gripper(self.pickup_object) self.place_location.header = tmpPoseStamped.header self.place_location.pose.position.x = self.result.entityInfos[ i].pose.position.x self.place_location.pose.position.y = self.result.entityInfos[ i].pose.position.y + 0.1 self.place_location.pose.position.z = self.result.entityInfos[ i].pose.position.z self.place_location.pose.orientation.x = self.result.entityInfos[ i].pose.orientation.x self.place_location.pose.orientation.y = self.result.entityInfos[ i].pose.orientation.y self.place_location.pose.orientation.z = self.result.entityInfos[ i].pose.orientation.z self.place_location.pose.orientation.w = self.result.entityInfos[ i].pose.orientation.w myTable = MyTable(tmpPoseStamped.header, self.table.pose, self.table.convex_hull) self.place(self.recog_obj, myTable) print "Executed place action" def add_to_planning_scene(self, recognized_object): pose_stamped = copy.deepcopy(recognized_object.pose) self.scene.add_box( recognized_object.name, pose_stamped, (recognized_object.dimensions[0], recognized_object.dimensions[1], recognized_object.dimensions[2])) rospy.loginfo("Object {} added to planning scene".format( recognized_object.name)) rospy.logdebug( "Object {} added with pose \n{} \nand dimensions {}".format( recognized_object.name, pose_stamped, recognized_object.dimensions)) def attach_object_to_gripper(self, recognized_object): rospy.loginfo("Attach object to gripper") pose_stamped = copy.deepcopy(recognized_object.pose) # self.remove_attached_object(recognized_object.name) self.scene.remove_world_object(recognized_object.name) self.scene.attach_box( END_EFFECTOR_LINK_NAME, recognized_object.name, pose_stamped, (recognized_object.dimensions[0], recognized_object.dimensions[1], recognized_object.dimensions[2]), GRIPPER_LINKS_TO_IGNORE_COLLISION) def pickup(self, pickup_object, grasps): supp_surface = 'table', rospy.loginfo("Recognized object: " + str(pick_obj)) self.arm.set_support_surface_name(supp_surface) result = self.arm.pick(pick_object.name, grasps) self.print_moveit_result(result) return result def generate_grasps_for_object(self, recognized_object): self.recognized_object = recognized_object self.base_grasp_pose_stamped = copy.deepcopy( self.recognized_object.pose) self.horizontal_center_grasp_pose_stamped = self.base_grasp_pose_stamped self.temp_grasp_pose_stamped = self.base_grasp_pose_stamped self.grasp_quality = 1.0 if recognized_object.dimensions[2] < self.high_res_threshold: rospy.loginfo( "Z axis of object ({} m) is shorter than threshold {}. \ create denser grasp poses.".format( recognized_object.dimensions[2], self.high_res_threshold)) self.distance_vertical = self.distance_vertical_default / 2.0 self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default / 2.0 else: rospy.loginfo( "Z axis of object ({} m) is longer than threshold {}. \ create normal distance grasp poses.".format( recognized_object.dimensions[2], self.high_res_threshold)) self.distance_vertical = self.distance_vertical_default self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default self.number_of_poses_vertical = int( (recognized_object.dimensions[2] - float(self.padding_from_top_and_bottom)) / float(self.distance_vertical) / 2.0) rospy.loginfo("Number of vertical poses " + str(self.number_of_poses_vertical)) # Generate the central grasp pose. Based on this pose the other poses are generated. # check if this is a top grasp (x axis faces in negative z direction of base_link) or a side grasp # pose with x downward, z forward pose_stamped_compare = PoseStamped() pose_stamped_compare.header.frame_id = "base_link" pose_stamped_compare.pose.position.x = 0.0 pose_stamped_compare.pose.position.y = 0.0 pose_stamped_compare.pose.position.z = 0.0 pose_stamped_compare.pose.orientation.w = 0.7071 pose_stamped_compare.pose.orientation.x = 0.0 pose_stamped_compare.pose.orientation.y = 0.7071 pose_stamped_compare.pose.orientation.z = 0.0 angle = self.angle_between_poses(self.temp_grasp_pose_stamped, pose_stamped_compare) if angle < pi / 4: # do a pinch grasp rospy.loginfo("This grasp is a top grasp. Do a pinch grasp") self.x_offset = -recognized_object.dimensions[ 0] / 2 + self.palm_to_finger_tips_distance self.recognized_object.top_grasp = True else: rospy.loginfo("This grasp is a side grasp. Do a power grasp") self.x_offset = recognized_object.dimensions[ 0] / 2 + self.additional_offset_in_grasp_direction self.recognized_object.top_grasp = False self.shift_pose_along_the_grippers_axes(self.temp_grasp_pose_stamped, [-self.x_offset, 0, 0]) self.create_grasp(self.temp_grasp_pose_stamped) # vertical for vertical_counter in range(0, self.number_of_poses_vertical + 1): if self.recognized_object.rotational_symmetric: self.generate_horizontal_grasps(True) self.generate_vertical_grasp_upper_part(vertical_counter) self.generate_horizontal_grasps(True) self.generate_vertical_grasp_lower_part(vertical_counter) else: self.generate_horizontal_grasps(False) self.generate_vertical_grasp_upper_part(vertical_counter) self.generate_horizontal_grasps(False) self.generate_vertical_grasp_lower_part(vertical_counter) rospy.loginfo("Generated vertical pose " + str(vertical_counter)) rospy.loginfo("Number of generated grasps: " + str(len(self.grasps))) for grasp in self.grasps: # rospy.logdebug("Publishing grasp z position:" + str(grasp.grasp_pose.pose.position)) # rospy.loginfo("Grasp quality: %s" % (grasp.grasp_quality)) return self.grasps def generate_vertical_grasp_upper_part(self, vertical_counter): self.generate_vertical_grasp("upper", vertical_counter) def generate_vertical_grasp_lower_part(self, vertical_counter): self.generate_vertical_grasp("lower", vertical_counter) def generate_vertical_grasp(self, part, vertical_counter): if part == "upper": distance_vertical = self.distance_vertical elif part == "lower": distance_vertical = -self.distance_vertical else: rospy.logerror('Part %s unkown. Only use "upper" or "lower"' % (part)) self.horizontal_center_grasp_pose_stamped = copy.deepcopy( self.base_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.horizontal_center_grasp_pose_stamped marvin_manipulation.transformations_common.shift_pose_along_the_grippers_axes( self.temp_grasp_pose_stamped, [0, 0, float(distance_vertical) * float(vertical_counter)]) self.grasp_quality = 1.0 / (float(vertical_counter) + 0.001 ) #avoid devided by zero self.create_grasp(self.temp_grasp_pose_stamped) def generate_horizontal_grasps(self, rotational_symmetric): if rotational_symmetric: rospy.loginfo('Rotational symmetric') for horizontal_counter in range( -self.number_of_poses_horizontal, self.number_of_poses_horizontal + 1): self.temp_grasp_pose_stamped = copy.deepcopy( self.horizontal_center_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center( self.temp_grasp_pose_stamped, self.angle_horizontal * float(horizontal_counter), self.x_offset) self.create_grasp(self.temp_grasp_pose_stamped) else: rospy.loginfo('non symmetric') #create grasp facing in negative x direction self.temp_grasp_pose_stamped = copy.deepcopy( self.horizontal_center_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center( self.temp_grasp_pose_stamped, pi, self.x_offset) self.create_grasp(self.temp_grasp_pose_stamped) def rotate_pose_around_object_center(self, pose_stamped, angle, distance_to_object_center): self.shift_pose_along_the_grippers_axes( pose_stamped, (distance_to_object_center, 0, 0)) self.rotate_pose_around_axis_by_angle(pose_stamped, angle, (0, 0, 1)) self.shift_pose_along_the_grippers_axes( pose_stamped, (-distance_to_object_center, 0, 0)) return pose_stamped def create_grasp(self, pose_stamped): grasp = Grasp() grasp.grasp_pose = copy.deepcopy(pose_stamped) # pre grasp gripper configuration grasp.pre_grasp_posture.header.frame_id = self.gripper_frame grasp.pre_grasp_posture.header.stamp = rospy.Time.now() grasp.pre_grasp_posture.joint_names = self.finger_joint_names grasp.pre_grasp_posture.points.append( JointTrajectoryPoint(positions=[ self.gripper_open_angle_radian, self.gripper_open_angle_radian ], effort=[self.max_effort, self.max_effort])) # approach grasp.pre_grasp_approach.direction.header.frame_id = self.gripper_frame grasp.pre_grasp_approach.direction.vector.x = 1.0 grasp.pre_grasp_approach.min_distance = 0.04 grasp.pre_grasp_approach.desired_distance = 0.15 # grasp gripper configuration grasp.grasp_posture.header.frame_id = self.gripper_frame grasp.grasp_posture.header.stamp = rospy.Time.now() grasp.grasp_posture.joint_names = self.finger_joint_names grasp.grasp_posture.points.append( JointTrajectoryPoint(positions=[ self.gripper_closed_angle_radian, self.gripper_closed_angle_radian ], effort=[self.max_effort, self.max_effort])) # TODO: add time_from_start here and for gripper open # retreat grasp.post_grasp_retreat.direction.header.frame_id = "base_link" grasp.post_grasp_retreat.direction.vector.z = 1.0 grasp.post_grasp_retreat.min_distance = 0.03 grasp.post_grasp_retreat.desired_distance = 0.05 # objects allowed to touch while approaching grasp.allowed_touch_objects = [self.recognized_object.name] grasp.grasp_quality = self.grasp_quality self.grasps.append(grasp) def angle_between_poses(pose_stamped, pose_stamped_compare=None): if not pose_stamped_compare: #pose with x forward, z upward pose_stamped_compare = PoseStamped() pose_stamped_compare.header.frame_id = "base_link" pose_stamped_compare.pose.position.x = 0.0 pose_stamped_compare.pose.position.y = 0.0 pose_stamped_compare.pose.position.z = 0.0 pose_stamped_compare.pose.orientation.w = 1.0 pose_stamped_compare.pose.orientation.x = 0.0 pose_stamped_compare.pose.orientation.y = 0.0 pose_stamped_compare.pose.orientation.z = 0.0 quaternion = [ pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w ] compare_quaternion = [ pose_stamped_compare.pose.orientation.x, pose_stamped_compare.pose.orientation.y, pose_stamped_compare.pose.orientation.z, pose_stamped_compare.pose.orientation.w ] inner_product = numpy.inner(quaternion, compare_quaternion) angle_between_poses = math.degrees(math.acos(2 * inner_product**2 - 1)) print("Angle between poses: %s" % angle_between_poses) return angle_between_poses def shift_pose_along_the_grippers_axes(pose_stamped, shift_per_axis_array): ''' Shifts given pose_stamped by the amounts given in shift_per_axis_array :param PoseStamped pose_stamped: :param () shift_per_axis_array: tupel of translations e.g. (0.5, 0, 0) to shift the pose 0.5m in x direction ''' frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w), PyKDL.Vector(pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)) point = PyKDL.Vector(shift_per_axis_array[0], shift_per_axis_array[1], shift_per_axis_array[2]) point = frame * point pose_stamped.pose.position.x = point.x() pose_stamped.pose.position.y = point.y() pose_stamped.pose.position.z = point.z() def place(self, recognized_object, pose_stamped): result = self.arm.place(recognized_object.name, pose_stamped) rospy.loginfo("Place result: " + str(result)) if result == True: return result return result
class RobotMotionObserver(object): """Observes motions of the robot. The implementation is based on using the get_current_joint_values()) functionality of the RobotCommander(). :param group_name: Name of the planning group, default value is 'manipulator' """ _DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 3.0 _DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01 _DEFAULT_SLEEP_INTERVAL_SEC = 0.01 _DEFAULT_GROUP_NAME = "manipulator" def __init__(self, group_name=_DEFAULT_GROUP_NAME): self._robot_commander = RobotCommander() self._group_name = group_name @staticmethod def _detect_motion(joint_values_a, joint_values_b, tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD): """TRUE if a significant motion was detected, otherwise FALSE.""" return not numpy.allclose( joint_values_a, joint_values_b, atol=tolerance) def _get_current_joint_states(self): """Returns the current joint state values of the robot. :raises RobotCurrentStateError if given planning group does not exist. """ try: return self._robot_commander.get_group( self._group_name).get_current_joint_values() except MoveItCommanderException as e: rospy.logerr(e.message) raise RobotCurrentStateError(e.message) def is_robot_moving( self, sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC, move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD): """TRUE if the robot is currently moving, otherwise FLASE.""" start_position = self._get_current_joint_states() rospy.sleep(sleep_interval) return RobotMotionObserver._detect_motion( start_position, self._get_current_joint_states(), move_tolerance) def wait_motion_start( self, wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC, move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC): """TRUE if the motion started in the given time interval, otherwise FALSE.""" old_joint_values = self._get_current_joint_states() rospy.loginfo("Start joint values: " + str(old_joint_values)) motion_started = False start_time = rospy.get_time() rospy.loginfo("Wait until motion starts...") while True: rospy.sleep(sleep_interval) curr_joint_values = self._get_current_joint_states() if RobotMotionObserver._detect_motion(curr_joint_values, old_joint_values, move_tolerance): motion_started = True rospy.loginfo("Changed joint values detected: " + str(curr_joint_values)) rospy.loginfo("Motion started.") break if (rospy.get_time() - start_time > wait_time_out): rospy.loginfo("Reached timeout when waiting for motion start.") break return motion_started def wait_motion_stop( self, wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC, move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC): """TRUE if the motion stopped in the given time interval, otherwise FALSE.""" motion_stopped = False start_time = rospy.get_time() rospy.loginfo("Wait until motion stops...") while True: if not self.is_robot_moving(sleep_interval, move_tolerance): motion_stopped = True rospy.loginfo("Motion stopped.") break if (rospy.get_time() - start_time > wait_time_out): rospy.loginfo("Reached timeout when waiting for motion stop.") break return motion_stopped
class Pick_Place: def __init__(self): # 检索参数: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # 创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # 创建规划现场,机器人指挥官: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # 清理现场: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # 将表和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) self._pose_table = self._add_table2(self._table2_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name) rospy.sleep(1.0) # 定义目标位置姿势: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x-0.5 self._pose_place.position.y = self._pose_coke_can.position.y-0.85 self._pose_place.position.z = self._pose_coke_can.position.z-0.3 self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # 检索组 (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('pickup') self._arm.go(wait=True) print("Pickup pose") # 创建抓取生成器“生成”动作客户端: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # 创建移动组“zhua取”操作客户端: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # 创建移动组“放置”动作客户端: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # 放置可乐可能会在支撑表面(桌子)上的其他地方产生异物: while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') print "-------------test1--------------" rospy.sleep(1.0) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) print "-------------test2------------------" def _add_table(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.85 p.pose.position.y = 0.0 p.pose.position.z = 0.70 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (1, 1, 0.05)) print "-------------test3------------------" return p.pose def _add_table2(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.85 p.pose.position.z = 0.40 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.5, 0.05)) print "-------------test4------------------" return p.pose def _add_grasp_block_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test5------------------" return p.pose def _add_grasp_block2_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.3 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test6------------------" return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例 Generate the grasp Pose Array data for visualization, and then send the grasp goal to the grasp server. 生成抓取姿势阵列数据以进行可视化,然后将抓取目标发送到抓取服务器。 """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # 发送目标并等待结果: # 将目标发送到ActionServer,等待目标完成,并且必须抢占目标. state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # 发布掌握(用于调试/可视化目的): self._publish_grasps(grasps) print "-------------test7------------------" return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # 生成预安置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) print "-------------test8------------------" return places def _create_pickup_goal(self, group, target, grasps): """ 创建一个MoveIt! 接送目标 创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # 配置目标计划选项: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test9------------------" return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test10------------------" return goal def _pickup(self, group, target, width): """ 使用计划小组选择目标 """ # 从掌握生成器服务器获取可能的掌握: grasps = self._generate_grasps(self._pose_coke_can, width) # 创建并发送提货目标: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test11------------------" return True def _place(self, group, target, place): """ 使用计划组放置目标 """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test12------------------" return True def _publish_grasps(self, grasps): """ 使用PoseArray消息将抓取发布为姿势 """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) print "-------------test13------------------" def _publish_places(self, places): """ 使用PoseArray消息将位置发布为姿势 """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) print "-------------test14------------------"
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) rospy.sleep(1.0) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.loginfo("added table") rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.loginfo("added grasping box") rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) def _add_table(self, name): rospy.loginfo("entered table") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.2 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (9, 9, 0.02)) return p.pose def _add_grasp_block_(self, name): rospy.loginfo("entered box grabber") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.1, 0.1, 0.1)) return p.pose
class Pick_Place: def __init__(self, x, y, z): self._x = x self._y = y self._z = z # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface() #未知 self._robot = RobotCommander() #未知 self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name, self._x, self._y, self._z) rospy.sleep(0.5) # Define target place pose:定义目标位置姿势 self._pose_place = Pose() self._pose_place.position.x = 0 #self._pose_coke_can.position.x self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20 self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('up') self._arm.go(wait=True) print("up pose") print("第一部分恢复位置初始") # Create grasp generator 'generate' action client: # #创建抓取生成器“生成”动作客户端: print("开始Action通信") self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return print("结束Action通信") # Create move group 'pickup' action client: # 创建移动组“抓取”操作客户端: print("开始pickup通信") self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return print("结束pickup通信") # Create move group 'place' action client: # 创建移动组“放置”动作客户端: print("开始place通信") self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return print("结束place通信") # Pick Coke can object:抓取快 while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(0.5) print("抓取物体") rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(0.5) rospy.loginfo('Place successfully') def _generate_grasps(self, pose, width): """ 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例。 生成抓取姿势阵列数据以进行可视化, 然后将抓取目标发送到抓取服务器. """ self._grasps_ac.wait_for_server() rospy.loginfo("Successfully connected!") # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: # Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary. # rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal)) #self._grasps_ac.wait_for_result() #发送目标并等待结果: # #将目标发送到ActionServer,等待目标完成,然后抢占目标是必要的。 t_start = rospy.Time.now() state = self._grasps_ac.send_goal_and_wait(goal) t_end = rospy.Time.now() t_toal = t_end - t_start rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec())) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) """ print("---------grasps--start------------") print(grasps) print("-----------test-grasps-----end-----------------") rospy.sleep(2) print("-grasps---sleep----end-----") """ return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() #print("---------------test3--------------------------") for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach:生成前置位置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 #print("place1---test=====") # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.06 #print("place2---test=====") # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) """ print(places) print("------test4---------------------------------------") """ return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal Create a goal for picking up the grasping object创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group #规划的组 goal.target_name = target #要拾取的对象的名称( #可能使用的抓握列表。 必须至少填写一个掌握 goal.possible_grasps.extend(grasps) #障碍物的可选清单,我们掌握有关语义的信息,可以在抓取过程中触摸/推动/移动这些障碍物; 注意:如果使用对象名称“ all”,则在进近和提升过程中禁止与所有对象发生碰撞。 goal.allowed_touch_objects.append(target) #如果没有可用的名称,则在碰撞图中支撑表面(例如桌子)的名称可以留空 goal.support_surface_name = self._table_object_name # Configure goal planning options:配置目标计划选项: #运动计划者可以规划的最长时间 goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 """ print("----goal-------test-----") print(goal) print("----goal---end------test---") """ rospy.sleep(0.5) print("-goal---sleep----end-----") return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _add_grasp_block_(self, name, _x, _y, _z): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = _x p.pose.position.y = _y p.pose.position.z = _z q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. #self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _pickup(self, group, target, width): """ Pick up a target using the planning group使用规划小组选择目标 """ # Obtain possible grasps from the grasp generator server:从掌握生成器服务器获取可能的抓握: grasps = self._generate_grasps(self._pose_coke_can, width) #print("--goal--start----") # Create and send Pickup goal:创建并发送zhua取目标: goal = self._create_pickup_goal(group, target, grasps) #print("--goal--end----") #print("---pick---up---1----") state = self._pickup_ac.send_goal_and_wait(goal) #print("-----------------state-------------------") #print(state) #print("---pick---up---2----") if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() """ print("-----test1--------") print(result) print("------test1--end------") """ # Check for error: err = result.error_code.val print(err) if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print("pickup-----end-----") rospy.sleep(0.5) return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places:获取可能的位置: places = self._generate_places(place) # Create and send Place goal:创建并发送地方目标: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() """ print("-----test2--------") print(result) print("------test2--end------") """ # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message使用PoseArray消息将抓取发布为姿势 """ print(self._grasps_pub.get_num_connections()) if self._grasps_pub.get_num_connections() != 1: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) print(msg) rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses') self._grasps_pub.publish(msg) """ print("11-111") rospy.loginfo('Publisher'+str(len(msg))+'poses') print("11-111") rospy.sleep(2) """ def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
class PigeonPickAndPlace: #Class constructor (parecido com java, inicializa o que foi instanciado) def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016) self._arm_group = rospy.get_param('~arm', 'arm_move_group') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) # Create planning scene where we will add the objects etc. self._scene = PlanningSceneInterface() # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene (remove the old objects: self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: # TODO get the position of the detected object self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name) rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Pick object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') def __del__(self): # Clean the scene self._scene.remove_world_object(self._grasp_object_name) def _add_object_grasp(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() #pose p.header.stamp = rospy.Time.now() rospy.loginfo(self._robot.get_planning_frame()) p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01 p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01 p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0 #p.pose.orientation.x = 0.0028925 #p.pose.orientation.y = -0.0019311 #p.pose.orientation.z = -0.71058 #p.pose.orientation.w = 0.70361 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/, # using the measure tape tool from meshlab. # The box is the bounding box of the lego cylinder. # The values are taken from the cylinder base diameter and height. # the size is length (x),thickness(y),height(z) # I don't know if the detector return this values of object self._scene.add_box(name, p, (0.032, 0.016, 0.020)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps #rospy.logerr('Generated: %f grasps:' % grasps.size) # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) #goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 5.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 10 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_object_grasp, 0.016) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg)
p.pose.position.x = 0.5 p.pose.position.y = 0 p.pose.position.z = 0.5 planning_scene_interface.add_box("object", p, (0.02, 0.02, 0.2)) if __name__=='__main__': # Initialise moveit and ROS node roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) # Get handles to the planning scene and robot scene = PlanningSceneInterface() robot = RobotCommander() # Select planning group group = robot.get_group('panda_arm') # Set a liberal planner timeout group.set_planning_time(seconds=45.0) # IMPORTANT: you must call sleep after the previous line # to ensure the planning scene object is initialised before # using it. rospy.sleep(2) # Setup scene for the pick and place example addCollisionObjects(scene) rospy.sleep(1) # Plan and execute the pick operation
class TestFacilitySensors(object): """Abstraction layer for easy usage of the PILZ test facility sensors.""" _DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 3.0 _DEFAULT_WAIT_TIME_IS_ROBOT_MOVING_SEC = 1.0 _DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01 _DEFAULT_SLEEP_INTERVAL_SEC = 0.01 _DEFAULT_GROUP_NAME = "manipulator" def __init__(self, group_name=_DEFAULT_GROUP_NAME): self._robot_commander = RobotCommander() self._group_name = group_name def set_default_motion_detection_tolerance(self, motion_tolerance_rad): self._DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = motion_tolerance_rad def _get_current_joint_states(self): """Returns the current joint state values of the robot. :raises RobotCurrentStateError if given planning group does not exist. """ return self._robot_commander.get_group( self._group_name).get_current_joint_values() def is_robot_at_position( self, expected_joint_values, tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD): """True if the robot stands at the specified position, otherwise False.""" return _are_joint_values_equal(self._get_current_joint_states(), expected_joint_values, tolerance) def is_robot_moving(self, wait_time_out=_DEFAULT_WAIT_TIME_IS_ROBOT_MOVING_SEC): """True if the robot is moving, otherwise false.""" return self.wait_for_robot_motion(wait_time_out=wait_time_out) def _wait_till(self, motion_expected, wait_time_out, move_tolerance, sleep_interval): """Waits till the robot has started moving or until the robot has stopped, depending on the given parameters. """ old_joint_values = self._get_current_joint_states() rospy.loginfo("Start joint values: " + str(old_joint_values)) start_time = rospy.get_time() rospy.loginfo("Start observing robot motion...") while not rospy.is_shutdown(): rospy.sleep(sleep_interval) curr_joint_values = self._get_current_joint_states() motion_detected = not _are_joint_values_equal( curr_joint_values, old_joint_values, move_tolerance) if motion_expected and motion_detected: rospy.loginfo("Motion detected at pose: " + str(curr_joint_values)) return True if not motion_expected and not motion_detected: rospy.loginfo("Robot stands still at pose: " + str(curr_joint_values)) return True if rospy.get_time() - start_time > wait_time_out: rospy.loginfo("Timeout reached") return False def wait_for_robot_motion( self, wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC, move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC): """Waits until a robot motion is detected.""" return self._wait_till(motion_expected=True, wait_time_out=wait_time_out, move_tolerance=move_tolerance, sleep_interval=sleep_interval) def wait_till_robot_stopped( self, wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC, move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD, sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC): """Waits till the robot stops it's motion.""" return self._wait_till(motion_expected=False, wait_time_out=wait_time_out, move_tolerance=move_tolerance, sleep_interval=sleep_interval)
def commander(): global solution # Makes a ROS node # rospy.init_node('rubik_commander', anonymous=True) # Gets a handle on the robot that MoveIt is controlling robot = RobotCommander() rospy.sleep(1) rospy.Subscriber('cube_steps', StringArray, callback) # Picks a group called "manipulator" to work with # arm = robot.get_group("manipulator") arm.set_max_velocity_scaling_factor(1.0) arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.005) rate = rospy.Rate(2) while not rospy.is_shutdown(): if len(solution) > 0: for move in solution: print("### " + str(move) + " ###") ##### PICK ##### go_home(arm) if "F" in move: front_to_right(arm, robot) go_home(arm) pick_rubik(arm, robot, "right") if "B" in move: pick_rubik(arm, robot, "back") if "L" in move: pick_rubik(arm, robot, "left") if "R" in move: pick_rubik(arm, robot, "right") if "U" in move: expose_up_down(arm, robot) go_home(arm) pick_rubik(arm, robot, "right") if "D" in move: expose_up_down(arm, robot) go_home(arm) pick_rubik(arm, robot, "left") go_home(arm) ##### TURN ##### if "'" in move: turn_rubik(arm, robot, "counter") elif "2" in move: turn_rubik(arm, robot, "180") else: turn_rubik(arm, robot, "clockwise") go_home(arm) ##### PLACE if "F" in move: place_rubik(arm, robot, "right") go_home(arm) right_to_front(arm, robot) if "B" in move: place_rubik(arm, robot, "back") if "L" in move: place_rubik(arm, robot, "left") if "R" in move: place_rubik(arm, robot, "right") if "U" in move: place_rubik(arm, robot, "right") go_home(arm) reset_up_down(arm, robot) if "D" in move: place_rubik(arm, robot, "left") go_home(arm) reset_up_down(arm, robot) go_home(arm) solution = [] rate.sleep()
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # # self._pose_table = self._scene.get_object_poses(self._table_object_name) # self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) rospy.sleep(1.0) self._scene.remove_world_object(self._grasp_object_name) rospy.sleep(1.0) # # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(2.0) # rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): # while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): # rospy.logwarn('Place failed! Retrying ...') # rospy.sleep(1.0) # # rospy.loginfo('Place successfully') def setup(): # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_attached_object() def _add_table(self, name): rospy.loginfo("entered table") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.2 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (9 , 9, 0.02)) return p.pose def _add_grasp_block_(self, name): rospy.loginfo("entered box grabber") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.1, 0.1, 0.1)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) def _add_objects(self): """ Here add all the objects that you want to add to the _scene """ self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class Grasp_Place: def __init__(self): # Retrieve params: #if there is no param named 'table_object_name' then use the default self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object1') self._grasp_object_name_capscrew4 = rospy.get_param( '~grasp_object_name4', 'capscrew4') #capscrew self._grasp_object_name_screw = rospy.get_param( '~grasp_object_name5', 'screw') # screw #print self._grasp_object_name_capscrew4 self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: #self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) ############################################################################################################################## ############################################################################################################################## #Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) #get the pose of endfection print( self._arm.get_current_pose(self._arm.get_end_effector_link()).pose) #self._Init_robot_state() #robot stata initilizations #self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (before a grasp) #self._move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) #move to the taget object for grasping #self._grasp_action(GRIPPER_CLOSED) #Close the gripper to grasp the object #self._move_up(displacementUp) #move up after the gripper grasp the target object #self._move_to_placeGoal(place_Pose_use, gripper_depth) #move to the above area of the placeGoal pose(a little bit higher) #self._move_down(displacementDown) #move down to the placeGoal pose #self._place_action() #place -> release or open the gripper #self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (after a place) #Costom_Command (Int) # 0: _Init_robot_state() # 1: _Go_to_default_pose() # 2: _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) # 3: _grasp_action(GRIPPER_CLOSED) # 4: _move_up(displacementUp) # 5: _move_to_placeGoal(place_Pose_use, gripper_depth) # 6: _move_down(displacementDown) # 7: _place_action() ##########Receive Command and respongding data from the server########### # _Go_to_default_pose() #让机械臂到达默认位姿 # _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) # 移->到达螺母预抓取位姿态 # _grasp_action(GRIPPER_CLOSED) # 抓->控制爪子关闭 # _move_up(displacementUp) # 移->向上提螺母 # _move_to_placeGoal(place_Pose_use, gripper_depth) #移->趋向螺栓 # _move_down(displacementDown) # 移-> 插入螺栓 # _place_action() # 放->控制爪子放开 self._Init_robot_state() #Initialzing the state of the robot def _Init_robot_state(self): rospy.loginfo('Initializing the robot state') self._arm.set_goal_position_tolerance(0.001) self._arm.set_goal_orientation_tolerance(0.001) self._arm.allow_replanning(True) self._arm.set_pose_reference_frame(self._robot.get_planning_frame()) self._arm.set_planning_time(7) #Set the 'home' pose of the ur5 before graspsing or after placing sth def _Go_to_default_pose(self): rospy.set_param('Excute_Lock', False) rospy.loginfo('Set the defaulit pose of the ur5 ') self._arm.set_named_target( 'home' ) #go to the 'home' place that are set in moveit_setup_assistant. self._arm.go() self._gripper.set_joint_value_target( 'gripper_finger1_joint', [0.0]) #[0.0] is the wide open pose of the gripper group. self._gripper.go() #move to the object for grasp #gripper_depth : default->0.15 def _move_to_obj_for_grasp(self, pose, gripper_depth): rospy.set_param('Excute_Lock', False) #Step-1##move-to-the-object-for-grasp### rospy.loginfo('moving-to-the-object-for-grasp') pose.position.z = pose.position.z + gripper_depth self._arm.set_pose_target(pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #grasp action #gripper_depth : default->0.15 #GRIPPER_CLOSED is a list type and can be modified accodring to the object width def _grasp_action(self, GRIPPER_CLOSED): rospy.set_param('Excute_Lock', False) G_C = [GRIPPER_CLOSED] #Step-2##grasp### #grasp_clsoe posture() rospy.loginfo('grasping the object') self._gripper.set_joint_value_target('gripper_finger1_joint', G_C) self._gripper.go() #rospy.sleep(1) #move up 0.1 def _move_up(self, displacementUp): rospy.set_param('Excute_Lock', False) #Step-3##move-up after the grsap### rospy.loginfo('moving up') current_pose = self._arm.get_current_pose( self._arm.get_end_effector_link( )).pose #get the current pose of the end_effector current_pose.position.z = current_pose.position.z + displacementUp self._arm.set_pose_target(current_pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #move to the placeGoal def _move_to_placeGoal(self, pose, gripper_depth): rospy.set_param('Excute_Lock', False) #Step-4##move-to-the-placeGoal(a little bit higer in case of collision)### rospy.loginfo( 'move-to-the-placeGoal: a little bit higer in case of collision') pose.position.z = pose.position.z + gripper_depth self._arm.set_pose_target(pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #maybe->Step-5##move-down-to the placeGoal### def _move_down(self, displacementDown): rospy.set_param('Excute_Lock', False) #Step-5##move-down-to the placeGoal### rospy.loginfo('moving down') current_pose = self._arm.get_current_pose( self._arm.get_end_effector_link( )).pose #get the current pose of the end_effector current_pose.position.z = current_pose.position.z - displacementDown self._arm.set_pose_target(current_pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #maybe->Step-5##move-down-to the placeGoal### def _place_action(self): rospy.set_param('Excute_Lock', False) #Step-6##place### #grasp_clsoe posture() rospy.loginfo('opening the gripper and placing the object') self._gripper.set_joint_value_target('gripper_finger1_joint', [0.0]) self._gripper.go()