def __init__(self):
 ##########################################################################
         rospy.loginfo("BipedCommander started")
         self.legs_group = MoveGroupCommander("legs")
         self.arms_group = MoveGroupCommander("arms")
         self.rarm_group = MoveGroupCommander("RArm")
         self.larm_group = MoveGroupCommander("LArm")

 def __init__(self):
     self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
     self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
     self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
     self.current_joint_states = None
     rospy.loginfo("Getting first joint_states")
     while self.current_joint_states == None:
         rospy.sleep(0.1)
     rospy.loginfo("Gotten!")
     rospy.loginfo("Connecting with right hand AS")
     self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
     self.right_hand_as.wait_for_server()
     rospy.loginfo("Connecting with left hand AS")
     self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
     self.left_hand_as.wait_for_server()
     rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
     self.right_arm_mgc = MoveGroupCommander("right_arm")
     self.right_arm_mgc.set_pose_reference_frame('base_link')
     self.left_arm_mgc = MoveGroupCommander("left_arm")
     self.left_arm_mgc.set_pose_reference_frame('base_link')
     self.torso_mgc = MoveGroupCommander("right_arm_torso")
     self.torso_mgc.set_pose_reference_frame('base_link')
     self.head_mgc = MoveGroupCommander("head")
     self.head_mgc.set_pose_reference_frame('base_link')
     self.last_hydra_message = None
     self.tmp_pose_right = PoseStamped()
     self.tmp_pose_left = PoseStamped()
     self.read_message = False

class HeadMover():
    """ Moves head to specified joint values """
    def __init__(self):
        self.group_head = MoveGroupCommander('head')
        
    def move_head(self, name, joint_values):
        rospy.loginfo('Moving head to specified position')
        self.group_head.set_joint_value_target(joint_values)
        self.group_head.go()

def init():
    #Wake up Baxter
    baxter_interface.RobotEnable().enable()
    rospy.sleep(0.25)
    print "Baxter is enabled"
    
    print "Intitializing clients for services"
    global ik_service_left
    ik_service_left = rospy.ServiceProxy(
            "ExternalTools/left/PositionKinematicsNode/IKService",
            SolvePositionIK)

    global ik_service_right
    ik_service_right = rospy.ServiceProxy(
            "ExternalTools/right/PositionKinematicsNode/IKService",
            SolvePositionIK)

    global obj_loc_service 
    obj_loc_service = rospy.ServiceProxy(
        "object_location_service", ObjLocation)

    global stopflag
    stopflag = False
    #Taken from the MoveIt Tutorials
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()

    global scene
    scene = moveit_commander.PlanningSceneInterface()

    #Activate Left Arm to be used with MoveIt
    global left_group
    left_group = MoveGroupCommander("left_arm")
    left_group.set_goal_position_tolerance(0.01)
    left_group.set_goal_orientation_tolerance(0.01)
    
    
    global right_group
    right_group = MoveGroupCommander("right_arm")
    pose_right = Pose()
    pose_right.position = Point(0.793, -0.586, 0.329)
    pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
    request_pose(pose_right, "right", right_group)
    

    global left_gripper
    left_gripper = baxter_interface.Gripper('left')
    left_gripper.calibrate()
    print left_gripper.parameters()

    global end_effector_subs
    end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback)
    rospy.sleep(1)

    global pubpic
    pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
    rospy.sleep(1)

def get_move_group_commander(group):
    '''
    Gets the move_group_commander for this process.

    '''
    global _mgc_dict
    with _mgc_dict_creation_lock:
        if not group in _mgc_dict:
            _mgc_group = MoveGroupCommander(group)
            _mgc_group.set_planner_id('RRTConnectkConfigDefault')
            _mgc_dict[group] = _mgc_group
	    add_ground()
        return _mgc_dict[group]

    def __init__(self):
        """
        This constructor initialises the different necessary connections to the topics and services
        and resets the world to start in a good position.
        """
        rospy.init_node("smart_grasper")
        
        self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, 
                                                  self.__joint_state_cb, queue_size=1)

        rospy.wait_for_service("/gazebo/get_model_state", 10.0)
        rospy.wait_for_service("/gazebo/reset_world", 10.0)
        self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
        self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)

        rospy.wait_for_service("/gazebo/pause_physics")
        self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
        rospy.wait_for_service("/gazebo/unpause_physics")
        self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
        rospy.wait_for_service("/controller_manager/switch_controller")
        self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
        rospy.wait_for_service("/gazebo/set_model_configuration")
        self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
        
        rospy.wait_for_service("/gazebo/delete_model")
        self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
        rospy.wait_for_service("/gazebo/spawn_sdf_model")
        self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
        
        rospy.wait_for_service('/get_planning_scene', 10.0)
        self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
        self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)

        self.arm_commander = MoveGroupCommander("arm")
        self.hand_commander = MoveGroupCommander("hand")
        
        self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", 
                                                     FollowJointTrajectoryAction)
        self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", 
                                                    FollowJointTrajectoryAction)
                                              
        if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
            rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
            raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
                                              
        if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
            rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
            raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")

        
        self.reset_world()

    def __init__(self, config, debug=0):
        print "[INFO] Initialise Robot"
        self.DEBUG = debug
        # configuration
        self.config = config
        # initialise move groups
        self.arm = MoveGroupCommander(ARM_GROUP)
        self.gripper = MoveGroupCommander(GRIPPER_GROUP)
        # initialise pick and place manager
        if self.DEBUG is 1:
            # verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
        else:
            # non verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)

        # tolerance: position (in m), orientation (in rad)
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.place_manager = None
        self.pick_manager = None

        self.initialise_move_actions()
        self.start_position()

class MoveitCommanderMoveGroupState(EventState):
  """
  Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
  """
  
  def __init__(self, planning_group, joint_names):
    """Constructor"""
    super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
                    input_keys=['target_joint_config'])

    self._planning_group = planning_group
    self._joint_names = joint_names
    Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
    self.group_to_move = MoveGroupCommander(self._planning_group)
    Logger.loginfo("finished making mgc in init.")

    self._done = False


  def execute(self, userdata):
    """Execute this state"""
    if self._done is not False:
      return self._done
  
  def on_enter(self, userdata):
    # create the motion goal
    Logger.loginfo("Entering MoveIt Commander code!")

    if len(self._joint_names) != len(userdata.target_joint_config):
      Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
                      % (len(self._joint_names), len(userdata.target_joint_config)))

    self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))               


    # execute the motion
    try: 
      Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
      result = self.group_to_move.go()
    except Exception as e:
      Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
      self._done = "failed"

    if result:
      self._done = "reached"
    else:
      self._done = "failed"

	def __init__(self, planArm="left_arm"):
		# Initialize the move_group API
		#moveit_commander.roscpp_initialize(sys.argv)

		# Initialize the ROS node
		rospy.init_node('pick_place', anonymous=True)

		# Connect to the arm move group
		if planArm == "right_arm":
			self.arm = MoveGroupCommander('right_arm')
			self.hand = Gripper(1)
		else:
			self.arm = MoveGroupCommander('left_arm')
			self.hand = Gripper(0)

		rospy.sleep(1)
		self.hand.release()
		
		# Allow replanning to increase the odds of a solution
		self.arm.allow_replanning(True)

  def __init__(self, planning_group, joint_names):
    """Constructor"""
    super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
                    input_keys=['target_joint_config'])

    self._planning_group = planning_group
    self._joint_names = joint_names
    Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
    self.group_to_move = MoveGroupCommander(self._planning_group)
    Logger.loginfo("finished making mgc in init.")

    self._done = False

    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_test_cartesian', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)

        rospy.sleep(2)

        pose = PoseStamped().pose

        # same orientation for all
        q = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
        pose.orientation = Quaternion(*q)

        i = 1
        while i <= 10:
            waypoints = list()
            j = 1
            while j <= 5:
                # random pose
                rand_pose = arm.get_random_pose(arm.get_end_effector_link())
                pose.position.x = rand_pose.pose.position.x
                pose.position.y = rand_pose.pose.position.y
                pose.position.z = rand_pose.pose.position.z
                waypoints.append(copy.deepcopy(pose))
                j += 1

            (plan, fraction) = arm.compute_cartesian_path(
                                      waypoints,   # waypoints to follow
                                      0.01,        # eef_step
                                      0.0)         # jump_threshold

            print "====== waypoints created ======="
            # print waypoints

            # ======= show cartesian path ====== #
            arm.go()
            rospy.sleep(10)
            i += 1

        print "========= end ========="

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

	def run(self, cycle, verbose = 2):

		#enable robot if not already done
		enableProcess = subprocess.Popen(["rosrun", "baxter_tools", 
		"enable_robot.py", "-e"])
		enableProcess.wait()
		
		#start moveit servers. Since we do not know how long it will take them 
		#to start, pause for 15 seconds
		jointServer = subprocess.Popen(["rosrun", "baxter_interface", 
		"joint_trajectory_action_server.py"])
		planServer = subprocess.Popen(["roslaunch", "baxter_moveit_config", 
		"move_group.launch"])
		time.sleep(15)
		
		raw_input("press enter")
		
		try:
			#left = MoveGroupCommander("left_arm")
			right = MoveGroupCommander("right_arm")
			
			upRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			x = 0.9667369015076861, y = -0.1190874231664102, z = -0.07489146157788866 ))
			
			#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.35, z = 0.8))
			
			#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = -0.45, z = 0.3))
			
			#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.45, z = 0.3))
			
			right.set_pose_target(upRight)
			#left.set_pose_target(upLeft)
			right.go()
			#left.go()
			
			#right.set_pose_target(downRight)
			#left.set_pose_target(downLeft)
			
			right.go()
			#left.go()
			
		finally:
			#clean up - kill servers
			jointServer.kill()
			planServer.kill()
			
			sys.exit()

def main():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    
    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')
        
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_hand"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"
        
        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = 0.5
        request.ik_request.pose_stamped.pose.position.y = 0.5
        request.ik_request.pose_stamped.pose.position.z = 0.3        
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0
        
        try:
            #Send the request to the service
            response = compute_ik(request)
            
            #Print the response HERE
            print(response)
            group = MoveGroupCommander("left_arm")
            
            #this command tries to achieve a pose: which is position, orientation
            group.set_pose_target(request.ik_request.pose_stamped)

            #then, this command tries to achieve a position only.  orientation is airbitrary.
            group.set_position_target([0.5,0.5,0.3])
            group.go()
            
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

    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):
        group = MoveGroupCommander("arm")
        
        #group.set_orientation_tolerance([0.3,0.3,0,3])
        
        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492
        
        
        q = quaternion_from_euler(2, 0, 0)
        
        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]
        


        
        print "Planning frame: " ,group.get_planning_frame()
        print "Pose reference frame: ",group.get_pose_reference_frame()
        
        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
        
        group.set_pose_target(p, "katana_gripper_tool_frame")
        
        group.go()
        print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")

    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 __init__(self, debug=False, online = True, robot_frame="odom_combined"):
        
        self._tf = TransformListener()
        
        self._online = online
        
        # self.snd_handle = SoundClient()
        
        if self._online:
        
            #self._interface = ROSpeexInterface()
            #self._interface.init()
            self._speech_client = SpeechSynthesisClient_NICT()
            
        else:
            
            self.snd_handle = SoundClient()
        
        rospy.sleep(1)
        
        self.say('Hello world!')
        
        rospy.sleep(1)
        
        self._debug = debug
        self._robot_frame = robot_frame
        
        self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
        
        self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
        self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
        
        self._left_arm = MoveGroupCommander("left_arm")
        self._right_arm = MoveGroupCommander("right_arm")
        
        print "r.f.: " + self._left_arm.get_pose_reference_frame()

        self.face = None
        # self.face_from = rospy.Time(0)
        self.face_last_dist = 0
        self.last_invited_at = rospy.Time(0)
        self._person_prob_left = 0
        
        self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
        
        self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
        self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
        
        self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
        self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
        
        self.no_face_random_delay = None
        
        self._initialized = False
        
        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
        
        
        self._move_buff = Queue.Queue()
        
        self._head_buff = Queue.Queue()
        
        self._move_thread = threading.Thread(target=self.movements)
        self._move_thread.daemon = True
        self._move_thread.start()
        
        self._head_thread = threading.Thread(target=self.head)
        self._head_thread.daemon = True
        self._head_thread.start()
        
        self.new_face = False
        self.face_last_dist = 0.0
        
        self.face_counter = 0
        
        self.actions = [self.stretchingAction,
                        self.introduceAction,
                        self.waveHandAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.advertAction,
                        self.numberOfFacesAction]
        
        self.goodbye_strings = ["Thanks for stopping by.",
                                "Enjoy the event.",
                                "See you later!",
                                "Have a nice day!"]
        
        self.invite_strings = ["Hello. It's nice to see you.",
                               "Come here and take some flyer.",
                               "I hope you are enjoying the event."]
        
        rospy.loginfo("Ready")

def close_hand():
    msg = Float64()
    msg.data = -1.0
    for i in range(30):
        pub.publish(msg)
        rospy.sleep(0.1)

if __name__ == '__main__':
    rospy.init_node('task_2_cheat', anonymous=True)

    # for for 5 sec
    rospy.sleep(5)

    rospy.loginfo("start program %f" % rospy.get_time())
    arm = MoveGroupCommander("ur5_arm")
    arm.set_planner_id('RRTConnectkConfigDefault')
    pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1)
    client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    client.wait_for_server()

    rospy.loginfo("init pose")
    msg = FollowJointTrajectoryGoal()
    msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
    msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
                                  'ur5_arm_shoulder_lift_joint',
                                  'ur5_arm_elbow_joint',
                                  'ur5_arm_wrist_1_joint',
                                  'ur5_arm_wrist_2_joint',
                                  'ur5_arm_wrist_3_joint']
    msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],

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

        # Initialize the ROS node
        rospy.init_node('moveit_constraints_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the arm move group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Increase the planning time since constraint planning can take a while
        arm.set_planning_time(5)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Start in the "resting" configuration stored in the SRDF file
        arm.set_named_target('l_arm_init')

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        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.263803774718
        target_pose.pose.position.y = 0.295405791959
        target_pose.pose.position.z = 0.690438884208
        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the start state and target pose, then plan and execute
        arm.set_start_state(robot.get_current_state())
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # Close the gripper
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = 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 = 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 = 0.1
        orientation_constraint.weight = 1.0
        # q = quaternion_from_euler(0, 0, -1.57079633)
        # orientation_constraint.orientation.x = q[0]
        # orientation_constraint.orientation.y = q[1]
        # orientation_constraint.orientation.z = q[2]
        # orientation_constraint.orientation.w = q[3]

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the arm
        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.39000848183
        target_pose.pose.position.y = 0.185900663329
        target_pose.pose.position.z = 0.732752341378
#.........这里部分代码省略.........

			break
		count = count+1
		
		rate.sleep()

	return eef_pose
	
if __name__ == '__main__':

	rospy.init_node('spawnobject')
	scene = PlanningSceneInterface()

	br = tf.TransformBroadcaster()
	listener = tf.TransformListener()

	robot = MoveGroupCommander("sia5d");
	gripper = MoveGroupCommander("gripper");
	rospy.sleep(1)

	p = PoseStamped()
	p1 = PoseStamped()
	p2 = PoseStamped()
	bowl2eef = PoseStamped()	
	p_goal = PoseStamped()
	p.header.frame_id = "world"
	p1.header.frame_id = "world"
	p2.header.frame_id = "world"
	bowl2eef.header.frame_id = "world"
	p_goal.header.frame_id = "world"
	tflist = Transform()