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, name):
     # stuff for grasp planning
     self.tf_listener = tf.TransformListener()
     self.tf_broadcaster = tf.TransformBroadcaster()
     self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
     self.last_objects = RecognizedObjectArray()
     #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
     self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
     self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
     
     rospy.loginfo("Connecting to pickup AS")
     self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
     #pickup_ac.wait_for_server() # needed?
     
     rospy.loginfo("Connecting to grasp generator AS")
     self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
     #grasps_ac.wait_for_server() # needed? 
     
     #planning scene for motion planning
     self.scene = PlanningSceneInterface()
     
     # blocking action server
     self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
     self.feedback = GraspObjectFeedback()
     self.result = GraspObjectResult()
     self.current_goal = None
     self.grasp_obj_as.start()

 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 __init__(self):

		self.node_name = "PickAndPlaceServer"
		rospy.loginfo("Initalizing PickAndPlaceServer...")
		self.sg = SphericalGrasps()

		# Get the object size
		self.object_height = 0.1
		self.object_width = 0.05
		self.object_depth = 0.05
		self.pick_pose = rospy.get_param('~pickup_marker_pose')
		self.place_pose = rospy.get_param('~place_marker_pose')

		rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
		self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
		connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
		if not connected:
			rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to pickup action server", self.node_name)

		rospy.loginfo("%s: Waiting for place action server...", self.node_name)
		self.place_ac = SimpleActionClient('/place', PlaceAction)
		if not self.place_ac.wait_for_server(rospy.Duration(3000)):
			rospy.logerr("%s: Could not connect to place action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to place action server", self.node_name)

		self.scene = PlanningSceneInterface()
		rospy.loginfo("Connecting to /get_planning_scene service")
		self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
		self.scene_srv.wait_for_service()
		rospy.loginfo("Connected.")

		rospy.loginfo("Connecting to clear octomap service...")
		self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
		self.clear_octomap_srv.wait_for_service()
		rospy.loginfo("Connected!")

		# Get the links of the end effector exclude from collisions
		self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
		if self.links_to_allow_contact is None:
			rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
		else:
			rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))

		self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
			execute_cb=self.pick_cb, auto_start=False)
		self.pick_as.start()

		self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
			execute_cb=self.place_cb, auto_start=False)
		self.place_as.start()

    def __init__(self, name):
        # stuff for grasp planning
        rospy.loginfo("Getting a TransformListener...")
        self.tf_listener = tf.TransformListener()
        rospy.loginfo("Getting a TransformBroadcaster...")
        self.tf_broadcaster = tf.TransformBroadcaster()
        rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
        self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
        self.last_clusters = None
        rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
        self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)

        if DEBUG_MODE:
            self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)

        rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
        self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
        self.pickup_ac.wait_for_server()

        rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
        self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
        self.place_ac.wait_for_server()

        rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
        self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
        self.grasps_ac.wait_for_server()

        rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
        self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
        self.depth_service.wait_for_service()

        rospy.loginfo("Getting a PlanningSceneInterface instance...")
        self.scene = PlanningSceneInterface()

        # blocking action server
        rospy.loginfo("Creating Action Server '" + name + "'...")
        self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
        self.as_feedback = ObjectManipulationFeedback()
        self.as_result = ObjectManipulationActionResult()
        self.current_goal = None

        # Take care of left and right arm grasped stuff
        self.right_hand_object = None
        self.left_hand_object = None
        self.current_side = 'right'

        rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
        self.grasp_obj_as.start()

    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}

    def __init__(self):



        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('scene_generator')

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()


        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################



        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()

    def __init__(self):
        smach.State.__init__(self, 
            outcomes=['succeeded','failed'],
            input_keys=[],
            output_keys=['new_box'])

        # initialize tf listener
        self.listener = tf.TransformListener()
        
        ### Create a handle for the Move Group Commander
        self.mgc = MoveGroupCommander("manipulator")
        
        ### Create a handle for the Planning Scene Interface
        self.psi = PlanningSceneInterface()
#         
        
        ### initialize service for gripper on universal arm 
        self.io_srv = rospy.ServiceProxy('set_io', SetIO)
        
        self.eef_step = 0.01
        self.jump_threshold = 2
        rospy.logwarn("Initializing Grasp")
        rospy.sleep(1)

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)

#!/usr/bin/env python


import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import 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


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

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    rospy.logwarn("cleaning world")
    # right_arm.set_named_target("r_start")
    # right_arm.go()

    # right_gripper.set_named_target("open")
    # right_gripper.go()

    rospy.sleep(3)

#!/usr/bin/env python

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

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('move_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("manipulator")

    # specify the planner
    group.set_planner_id("RRTkConfigDefault")

    rospy.sleep(3)

    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.

    # display_trajectory_publisher = rospy.Publisher(
    #                                   '/move_group/display_planned_path',
    #                                   moveit_msgs.msg.DisplayTrajectory)

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('SceneSetup')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
     
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)

        # Set the reference frame for pose targets
        reference_frame = 'rack1'
        table_id = 'table'
        bowl_id = 'bowl'
        pitcher_id = 'pitcher'
  
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(bowl_id)
        scene.remove_world_object(pitcher_id)

        # Set the height of the table off the ground
        table_ground = 0.5762625 
        
        # Set the length, width and height of the table and boxes
        table_size = [1.0128, 0.481, 0.0238125]

        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0
        table_pose.pose.position.y = 0.847725
        table_pose.pose.position.z = table_ground

        # Set the height of the bowl
        bowl_ground = 0.57816875 
        bowl_pose = PoseStamped()
        bowl_pose.header.frame_id = reference_frame
        bowl_pose.pose.position.x = 0
        bowl_pose.pose.position.y = 0.847725
        bowl_pose.pose.position.z = bowl_ground

        # Set the height of the pitcher
        pitcher_ground = 0.57816875 
        pitcher_pose = PoseStamped()
        pitcher_pose.header.frame_id = reference_frame
        pitcher_pose.pose.position.x = 0.4
        pitcher_pose.pose.position.y = 0.847725
        pitcher_pose.pose.position.z = pitcher_ground
        pitcher_pose.pose.orientation.w = -0.5
        pitcher_pose.pose.orientation.z = 0.707

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0.4, 0, 1.0)
        self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
        self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
        self.sendColors()   


        
        scene.add_box(table_id, table_pose, table_size)
        scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
               #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

class Planner(object):
    move_group = None
    goals = None
    jspub = None
    namespace = None

    # These will eventually go to model objects
    robot_data = {
        'group_name': 'right_arm_and_torso',
        'eef_link': 'r_wrist_joint_link'
    }

    # Current state of the 'session' (right now, only one)
    current_scene = None
    status = None
    link_poses = None

    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}

    def get_scene(self):
        return self.current_scene

    def set_scene(self, scene):
        self.current_scene = scene
        psw = PlanningSceneWorld()
        for co_json in scene['objects']:
            # TODO: Fix orientation by using proper quaternions on the client
            pose = self._make_pose(co_json['pose'])
            # TODO: Decide what to do with STL vs. Collada. The client has a Collada
            # loader but the PlanningSceneInterface can only deal with STL.
            # TODO: Proper mapping between filenames and URLs
            # filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
            filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
            co = self.ps.make_mesh(co_json['name'], pose, filename)
            psw.collision_objects.append(co)
        self.psw_pub.publish(psw)


    def get_link_poses(self):
        if self.link_poses is None:
            self.link_poses = self._move_group.get_link_poses_compressed()
        return self.link_poses

    # Create link back to socket.io namespace to allow emitting information
    def set_socket(self, namespace):
        self.namespace = namespace

    def emit(self, event, data=None):
        if self.namespace:
            self.namespace.emit(event, data)

    def emit_new_goal(self, pose):
        self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])

    def set_random_goal(self):
        goal_pose = self.move_group.get_random_pose()
        # goal_pose = self.move_group.get_random_pose('base_footprint')
        self.emit_new_goal(goal_pose)

    def _make_pose(self, json_pose):
        pose = PoseStamped()
        pose.header.frame_id = "odom_combined"
        pp = json_pose['position']
        pose.pose.position.x = pp['x']
        pose.pose.position.y = pp['y']
        pose.pose.position.z = pp['z']
        # TODO: Orientation is not working. See about
        # properly using Quaternions everywhere
        pp = json_pose['orientation']
        pose.pose.orientation.x = pp['x']
        pose.pose.orientation.y = pp['y']
        pose.pose.orientation.z = pp['z']
        pose.pose.orientation.w = pp['w']
        return pose

    def plan_to_poses(self, poses):
        goal_pose = self._make_pose(poses[0])
        self.move_group.set_pose_target(goal_pose)
        # self.move_group.set_pose_target(goal_pose,'base_footprint')
#.........这里部分代码省略.........

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

import copy
import random
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GenerateGraspsResult

moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
    if not name[:1] == '_':
        code = MoveItErrorCodes.__dict__[name]
        moveit_error_dict[code] = name


if __name__=='__main__':
    rospy.init_node("part2_attacher")

    
    scene = PlanningSceneInterface()
    
    rospy.sleep(1)
    
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = '/base_link'
    
    p.pose.position.x = 0.6
    p.pose.position.y = 0.0    
    p.pose.position.z = 0.45
    p.pose.orientation.w = 1.0
    #scene.add_box("table", p, (0.5, 1.5, 0.9))
    p.pose.position.x = 0.45
    p.pose.position.y = -0.1
    p.pose.position.z = 1.0

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

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ## Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Prepare Gripper and open it
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
        self.ac.send_goal(g_open)

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        rospy.sleep(2)
        
        # PREPARE THE SCENE
       	
       	while self.pwh is None:
            rospy.sleep(0.05)

        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

        # Remove leftover objects from a previous run
        scene.remove_world_object(target_id)
        scene.remove_world_object(table_id)
        #scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose = self.pwh.pose[self.tid]
        table_pose.pose.position.z += 1
        scene.add_box(table_id, table_pose, table_size)
        
        #obstacle1_pose = PoseStamped()
        #obstacle1_pose.header.frame_id = REFERENCE_FRAME
        #obstacle1_pose.pose = self.pwh.pose[self.o1id]
        ## Add the target object to the scene
        #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
#.........这里部分代码省略.........

    arm = MoveGroupCommander(PLANNING_GROUP)

    # # tolerance
    # arm.set_goal_position_tolerance(0.01)
    # arm.set_goal_orientation_tolerance(0.01)
    # arm.set_goal_joint_tolerance(0.01)
    # arm.set_goal_tolerance(0.01)

    # Action Clients for Place
    rospy.loginfo("Connecting to place AS")
    place_ac = actionlib.SimpleActionClient('/place', PlaceAction)
    place_ac.wait_for_server()
    rospy.loginfo("Successfully connected")

    # create Planning Scene
    scene = PlanningSceneInterface()
    rospy.sleep(1)

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = REFERENCE_LINK
    p.header.stamp = rospy.Time.now()

    quat = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
    p.pose.orientation = Quaternion(*quat)

    p.pose.position.x = 0.6
    p.pose.position.y = 0.0
    p.pose.position.z = 0.0
    # add table
    scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))

#    a = robot.right_arm
#    a.set_start_state(RobotState())
#    r = a.get_random_joint_values()
#    print "Planning to random joint position: "
#    print r
#    p = a.plan(r)
#    print "Solution:"
#    print p
#
#    roscpp_shutdown()
    
    
    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("pole")
    #scene.remove_world_object("table")
    scene.remove_world_object("part")

    # 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 = 1.15
    p.pose.orientation.w = 1.0

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # 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)

        # 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 10 seconds per planning attempt
        right_arm.set_planning_time(10)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name        
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
        tool_id = 'tool'
                
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)
        
        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.65
        
        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.55
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
#.........这里部分代码省略.........

# Author: Ioan Sucan

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_effe