Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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))
Ejemplo n.º 5
0
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)
Ejemplo n.º 7
0
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()
Ejemplo n.º 8
0
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
Ejemplo n.º 10
0
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
Ejemplo n.º 12
0
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)
Ejemplo n.º 13
0
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)
Ejemplo n.º 14
0
    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
Ejemplo n.º 15
0
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)
Ejemplo n.º 16
0
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()