movegroup运动控制函数总结

1.set_pose_target( ):

INPUT：pose, end_effector_link = “”
Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:
[x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]
Example:

     target_pose = PoseStamped()
     target_pose.header.frame_id = reference_frame
     target_pose.header.stamp = rospy.Time.now()     
     target_pose.pose.position.x = 0.25
     target_pose.pose.position.y = -0.15
     target_pose.pose.position.z = 0.28
     target_pose.pose.orientation.x = 0.0
     target_pose.pose.orientation.y = 0.0
     target_pose.pose.orientation.z = 0.0
     target_pose.pose.orientation.w = 1.0
     # Set the start state to the current state
     right_arm.set_start_state_to_current_state()
     # Set the goal pose of the end effector to the stored pose
     right_arm.set_pose_target(target_pose, end_effector_link)
     # Plan the trajectory to the goal
     traj = right_arm.plan()
     # Execute the planned trajectory
     right_arm.execute(traj)

2.set_pose_targets():

INPUT：poses, end_effector_link = “”
Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]

3.set_joint_value_target():

INPUT： arg1, arg2 = None, arg3 = None
Specify a target joint configuration for the group.

# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)

4.shift_pose_target():

INPUT：axis, value, end_effector_link = “”
Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target

     # Shift the end-effector to the right 5cm
        right_arm.shift_pose_target(1, -0.05, end_effector_link)
        right_arm.go()
        rospy.sleep(1)

        # Rotate the end-effector 90 degrees
        right_arm.shift_pose_target(3, -1.57, end_effector_link)
        right_arm.go()
        rospy.sleep(2) 

5.compute_cartesian_path( ):

INPUT：waypoints, eef_step, jump_threshold, avoid_collisions = True
RETURN：(RobotTrajectory, fraction)
Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints.
Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath.
The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory.

if cartesian:                 
     fraction = 0.0
     maxtries = 100
     attempts = 0
     # Set the internal state to the current state
     right_arm.set_start_state_to_current_state()
     # Plan the Cartesian path connecting the waypoints
     while fraction < 1.0 and attempts < maxtries:
        (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        #avoid_collisions

          # Increment the number of attempts 
          attempts += 1
          # Print out a progress message
          if attempts % 10 == 0:
             rospy.loginfo("Still trying after " + str(attempts) + " attempts...")         
          # If we have a complete plan, execute the trajectory
          if fraction == 1.0:
              rospy.loginfo("Path computed successfully. Moving the arm."
              right_arm.execute(plan)            
              rospy.loginfo("Path execution complete.")