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 __init__(self):
     self._gdict = {}
     self._group_name = ""
     self._prev_group_name = ""
     self._planning_scene_interface = PlanningSceneInterface()
     self._robot = RobotCommander()
     self._last_plan = None
     self._db_host = None
     self._db_port = 33829
     self._trace = False

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()

def main():
    rospy.init_node('moveit_py_place', anonymous=True)
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    #group = MoveGroupCommander("head")
    right_arm = MoveGroupCommander("right_arm")
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.logwarn("cleaning world")
    GRIPPER_FRAME = 'gripper_bracket_f2'
    #scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    p.pose.position.x = 0.67
    p.pose.position.y = -0.
    p.pose.position.z = 0.75
    scene.add_box("part", p, (0.07, 0.01, 0.2))

    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)

    result = False
    n_attempts = 0
       
    # repeat until will succeed
    while result == False:
        result = robot.right_arm.pick("part")      
        n_attempts += 1
        print "Attempts pickup: ", n_attempts
        rospy.sleep(0.2)
    
    #robot.right_arm.pick("part")
    #right_arm.go()
    rospy.sleep(5)

    def __init__(self):
        rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
        rospy.wait_for_service(IK_SERVICE_NAME)
        self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
        
        self.joint_list = ['torso_1_joint', 'torso_2_joint',
                           'head_1_joint', 'head_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint', 
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        
        
#         # Store answer so we ask only one time
#         self.joint_state = JointState()
#         self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
#         self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
#         self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
        self.r_commander = RobotCommander()
        self.initial_robot_state = self.r_commander.get_current_state()

        if DEBUG_MODE:
            self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
            self.ok_markers = MarkerArray()
        
            self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
            self.fail_markers = MarkerArray()
            self.markers_id = 5    

class MoveGroupCommandInterpreter(object):
    """
    Interpreter for simple commands
    """

    DEFAULT_FILENAME = "move_group.cfg"
    GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
               "left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
               "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }

    def __init__(self):
        self._gdict = {}
        self._group_name = ""
        self._prev_group_name = ""
        self._planning_scene_interface = PlanningSceneInterface()
        self._robot = RobotCommander()
        self._last_plan = None
        self._db_host = None
        self._db_port = 33829
        self._trace = False

    def get_active_group(self):
        if len(self._group_name) > 0:
            return self._gdict[self._group_name]
        else:
            return None

    def get_loaded_groups(self):
        return self._gdict.keys()

    def execute(self, cmd):
        cmd = self.resolve_command_alias(cmd)
        result = self.execute_generic_command(cmd)
        if result != None:
            return result
        else:
            if len(self._group_name) > 0:
                return self.execute_group_command(self._gdict[self._group_name], cmd)
            else:
                return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")

    def execute_generic_command(self, cmd):
        if os.path.isfile("cmd/" + cmd):
            cmd = "load cmd/" + cmd
        cmdlow = cmd.lower()
        if cmdlow.startswith("use"):
            if cmdlow == "use":
                return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
            clist = cmd.split()
            clist[0] = clist[0].lower()
            if len(clist) == 2:
                if clist[0] == "use":
                    if clist[1] == "previous":
                        clist[1] = self._prev_group_name
                        if len(clist[1]) == 0:
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                    if self._gdict.has_key(clist[1]):
                        self._prev_group_name = self._group_name
                        self._group_name = clist[1]
                        return (MoveGroupInfoLevel.DEBUG, "OK")
                    else:
                        try:
                            mg = MoveGroupCommander(clist[1])
                            self._gdict[clist[1]] = mg
                            self._group_name = clist[1]
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                        except MoveItCommanderException as e:
                            return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
                        except:
                            return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
        elif cmdlow.startswith("trace"):
            if cmdlow == "trace":
                return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
            clist = cmdlow.split()
            if clist[0] == "trace" and clist[1] == "on":
                self._trace = True
                return (MoveGroupInfoLevel.DEBUG, "OK")
            if clist[0] == "trace" and clist[1] == "off":
                self._trace = False
                return (MoveGroupInfoLevel.DEBUG, "OK")
        elif cmdlow.startswith("load"):
            filename = self.DEFAULT_FILENAME
            clist = cmd.split()
            if len(clist) > 2:
                return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
            if len(clist) == 2:
                filename = clist[1]
                if not os.path.isfile(filename):
                    filename = "cmd/" + filename
            line_num = 0
            line_content = ""
            try:
                f = open(filename, 'r')
                for line in f:
                    line_num += 1
                    line = line.rstrip()
                    line_content = line
                    if self._trace:
                        print ("%s:%d:  %s" % (filename, line_num, line_content))
                    comment = line.find("#")
#.........这里部分代码省略.........

def simple_function():
        rc = RobotCommander()
        mgc = MoveGroupCommander("manipulator")
        # print(rc.get_group_names())
        # print(rc.get_group('manipulator'))
        
        
        # exit()
        
        eef_step = 0.01
        jump_threshold = 2
        ### Create a handle for the Planning Scene Interface
        psi = PlanningSceneInterface()
        
        
        rc.get_current_state()
        rospy.logwarn(rc.get_current_state())
        sss.wait_for_input()
        #rate = rospy.Rate(0.1) # 10hz
        rate = rospy.Rate(1) # 10hz
        rospy.sleep(1)
        
        
        theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
        
        while not rospy.is_shutdown():
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = "table"
            approach_pose.header.stamp = rospy.Time(0)
            approach_pose.pose.position.x = 0.146
            approach_pose.pose.position.y = 0.622
            approach_pose.pose.position.z = 0.01
            quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
            approach_pose.pose.orientation.x = quat[0]
            approach_pose.pose.orientation.y = quat[1]
            approach_pose.pose.orientation.z = quat[2]
            approach_pose.pose.orientation.w = quat[3]
#             mgc.set_start_state_to_current_state()
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed before adding collision objects")
#             else:
#                 rospy.logwarn("Planning succeeded before adding collision objects")
# 
#             rospy.logwarn("waiting for input to add dummy box")
#             sss.wait_for_input()
#             
            box_dummy_pose = PoseStamped()
            box_dummy_pose.header.frame_id =  "table"
            box_dummy_pose.pose.position.x = 0.147
            box_dummy_pose.pose.position.y = 0.636
            box_dummy_pose.pose.position.z = 0
            psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC  
#             rospy.logwarn("I have added the box")
#             rospy.sleep(1)
#             rospy.logwarn("waiting for input to try planning with dummy box")
#             sss.wait_for_input()
#             
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
#             else:
#                 rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#                 
            rospy.logwarn("waiting for input to add end effector box box")
            sss.wait_for_input()
            # end effector collision object
            link_attached_to_ef = "ee_link"
            mb_ef_collisonobj = "metal_bracket"
            mb_ef_pose = PoseStamped()
            mb_ef_pose.header.frame_id =  link_attached_to_ef
            mb_ef_pose.pose.position.x = 0.065/2.0
            mb_ef_pose.pose.position.y = 0.0
            mb_ef_pose.pose.position.z = 0.0
            mb_ef_size = (0.065,0.06,0.06)



            psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
            
            
            raw_input("0 hi")
            
            mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
            

            
            
            rospy.logwarn("I have added the attached box to the end effector")
            
            
            rospy.sleep(1)
            raw_input("1 hi")           
            
            rospy.logwarn(rc.get_current_state())
            # mgc.attach_object(object_name, link_name, touch_links)
            mgc.set_start_state_to_current_state()
            rospy.logwarn(rc.get_current_state())
            raw_input("2 hi")
            rospy.logwarn("waiting for input to try planning with end effector box")
#.........这里部分代码省略.........

#!/usr/bin/env python

import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped

if __name__=='__main__':
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    rospy.sleep(1)
    # clean the scene
    #scene.remove_world_object("block1")
    #scene.remove_world_object("block2")
    #scene.remove_world_object("block3")
    #scene.remove_world_object("block4")
    #scene.remove_world_object("table")
    #scene.remove_world_object("bowl")
    #scene.remove_world_object("box")
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0.7
    p.pose.position.y = -0.4
    p.pose.position.z = 0.85
    p.pose.orientation.w = 1.57
    scene.add_box("block1", p, (0.044, 0.044, 0.044))
    p.pose.position.y = -0.2
    p.pose.position.z = 0.175

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
#.........这里部分代码省略.........

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
#.........这里部分代码省略.........

 def setUpClass(self):
     self.commander = RobotCommander("%srobot_description" % self.PLANNING_NS, self.PLANNING_NS)
     self.group = self.commander.get_group(self.PLANNING_GROUP)

class trajectoryConstructor():
    def __init__(self):
        rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
        rospy.wait_for_service(IK_SERVICE_NAME)
        self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
        
        self.joint_list = ['torso_1_joint', 'torso_2_joint',
                           'head_1_joint', 'head_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint', 
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        
        
#         # Store answer so we ask only one time
#         self.joint_state = JointState()
#         self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
#         self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
#         self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
        self.r_commander = RobotCommander()
        self.initial_robot_state = self.r_commander.get_current_state()

        if DEBUG_MODE:
            self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
            self.ok_markers = MarkerArray()
        
            self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
            self.fail_markers = MarkerArray()
            self.markers_id = 5    
        
        
        
    def getIkPose(self, pose, group="right_arm", previous_state=None):
        """Get IK of the pose specified, for the group specified, optionally using
        the robot_state of previous_state (if not, current robot state will be requested) """
        # point point to test if there is ik
        # returns the answer of the service
        rqst = GetPositionIKRequest()
        rqst.ik_request.avoid_collisions = True
        rqst.ik_request.group_name = group
        rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
        rqst.ik_request.pose_stamped.header.frame_id = 'base_link'


        # Set point to check IK for
        rqst.ik_request.pose_stamped.pose.position = pose.position
        rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
        
        if previous_state == None:
            cs = self.r_commander.get_current_state()
            rqst.ik_request.robot_state = cs
        else:
            rqst.ik_request.robot_state = previous_state

        ik_answer = GetPositionIKResponse()
        if DEBUG_MODE:
            timeStart = rospy.Time.now()
        ik_answer = self.ik_serv.call(rqst)
        
        if DEBUG_MODE:
            durationCall= rospy.Time.now() - timeStart
            rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
        
        return ik_answer   
        
    def computeJointTrajFromCartesian(self, points, arm="right_arm_torso"):
        #fjt_goal = FollowJointTrajectoryGoal()
        poselist = []
        for point in points:
            qt = quaternion_from_euler(point.positions[3], point.positions[4], point.positions[5])
            pose = Pose(Point(point.positions[0], point.positions[1], point.positions[2]),
                        Quaternion(*qt.tolist()))
            poselist.append(pose)
        fjt_goal = self.computeIKsPose(poselist, arm)
        
        return fjt_goal
        
        
    def computeIKsPose(self, poselist, arm="right_arm"):
        rospy.loginfo("Computing " + str(len(poselist)) + " IKs" )
        fjt_goal = FollowJointTrajectoryGoal()

        if arm == 'right_arm_torso':
            fjt_goal.trajectory.joint_names = self.right_arm_torso
#.........这里部分代码省略.........

from std_msgs.msg import String
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped


#http://moveit.ros.org/doxygen/annotated.html
#https://github.com/ros-planning/moveit_commander

if __name__ == '__main__':
    
  
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    rospy.sleep(1)
   

    
    #Print Current State
    state = robot.get_current_variable_values()
    
    print state
    
    print "---------------------------------------"
    print "CURRENT STATE RIGHT ARM"
    print "---------------------------------------"
    print state['r_joint1'], "  ", state['r_joint2'], "  " ,state['r_joint3'], "  " ,state['r_joint4'], "  " ,state['r_joint5'], "  " ,state['r_joint6'], "  " ,state['r_joint7'] 

    

    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #right_arm.set_planner_id("KPIECEkConfigDefault");
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object(GRIPPER_FRAME, "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        #right_arm.set_named_target("r_start")
        #right_arm.go()
      
        #right_gripper.set_named_target("right_gripper_open")
        #right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.7
        p.pose.position.y = -0.2
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.47636
        start_pose.pose.position.y = -0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)
   
        result = False
        n_attempts = 0
       
        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)      
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)
          
        rospy.spin()
        roscpp_shutdown()

import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
import copy

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print group.get_end_effector_link()
    print group.get_pose_reference_frame()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"
    tl = tf.TransformListener()
    
    rospy.sleep(1)

import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print right_arm.get_end_effector_link()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"

    
    rospy.sleep(1)

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 = SimpleActionClien