Python moveit_commander.MoveGroupCommander类代码示例

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