Python msg.FollowJointTrajectoryGoal类代码示例

    def send_arm_goal(self, arm_goal):
        arm_joint_names = ['joint_lift',
                           'joint_waist',
                           'joint_big_arm',
                           'joint_forearm',
                           'joint_wrist_pitching',
                           'joint_wrist_rotation']

        rospy.loginfo("Waiting for follow_joint_trajectory server")
        self.arm_client.wait_for_server()
        rospy.loginfo("Connected to follow_joint_trajectory server")
        rospy.sleep(1)
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joint_names
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].time_from_start = rospy.Duration(10)
        rospy.loginfo("Preparing for moving the arm to goal position!")
        rospy.sleep(1)
        arm_goal_pos = FollowJointTrajectoryGoal()
        arm_goal_pos.trajectory = arm_trajectory

        arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
        self.arm_client.send_goal(arm_goal_pos)
        rospy.loginfo("Send goal to the trajectory server successfully!")
        self.arm_client.wait_for_result()

def createHeadGoal(j1, j2):
    """Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
    @arg j1 float value for head_1_joint
    @arg j2 float value for head_2_joint
    @returns FollowJointTrajectoryGoal with the specified goal"""
    fjtg = FollowJointTrajectoryGoal()
    fjtg.trajectory.joint_names.append('head_1_joint')
    fjtg.trajectory.joint_names.append('head_2_joint')
    point = JointTrajectoryPoint()
    point.positions.append(j1)
    point.positions.append(j2)
    point.velocities.append(0.0)
    point.velocities.append(0.0)
    point.time_from_start = rospy.Duration(4.0)
    for joint in fjtg.trajectory.joint_names:  # Specifying high tolerances for the hand as they are slow compared to other hardware
        goal_tol = JointTolerance()
        goal_tol.name = joint
        goal_tol.position = 5.0
        goal_tol.velocity = 5.0
        goal_tol.acceleration = 5.0
        fjtg.goal_tolerance.append(goal_tol)
    fjtg.goal_time_tolerance = rospy.Duration(3)

    fjtg.trajectory.points.append(point)
    fjtg.trajectory.header.stamp = rospy.Time.now()
    return fjtg

    def __init__(self):
        rospy.init_node('arm_simple_trajectory')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['joint_lift',
                      'joint_waist',
                      'joint_big_arm', 
                      'joint_forearm',
                      'joint_wrist_pitching',
                      'joint_wrist_rotation']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        else:
            # Set a goal configuration for the arm
            arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
    
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for xm arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
                                                  FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        rospy.sleep(1)
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        rospy.sleep(1)
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        rospy.loginfo('...done')
        rospy.sleep(1)

def createHandGoal(side, j1, j2, j3):
    """Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
    with the hand specified in side
    @arg side string 'right' or 'left'
    @arg j1 float value for joint 'hand_'+side+'_thumb_joint'
    @arg j2 float value for joint 'hand_'+side+'_middle_joint'
    @arg j3 float value for joint 'hand_'+side+'_index_joint'
    @return FollowJointTrajectoryGoal with the specified goal"""
    fjtg = FollowJointTrajectoryGoal()
    fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
    fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
    fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
    point = JointTrajectoryPoint()
    point.positions.append(j1)
    point.positions.append(j2)
    point.positions.append(j3)
    point.velocities.append(0.0)
    point.velocities.append(0.0)
    point.velocities.append(0.0)
    point.time_from_start = rospy.Duration(4.0)
    fjtg.trajectory.points.append(point)
    for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
        goal_tol = JointTolerance()
        goal_tol.name = joint
        goal_tol.position = 5.0
        goal_tol.velocity = 5.0
        goal_tol.acceleration = 5.0
        fjtg.goal_tolerance.append(goal_tol)
    fjtg.goal_time_tolerance = rospy.Duration(3)
    fjtg.trajectory.header.stamp = rospy.Time.now()
    return fjtg

 def execute(self, userdata):
     rospy.loginfo('In create_move_group_joints_goal')
     
     _move_joint_goal = FollowJointTrajectoryGoal()
     _move_joint_goal.trajectory.joint_names = userdata.move_joint_list
     
     jointTrajectoryPoint = JointTrajectoryPoint()
     jointTrajectoryPoint.positions = userdata.move_joint_poses
     
     for x in range(0, len(userdata.move_joint_poses)):
         jointTrajectoryPoint.velocities.append(0.0)
      
     jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
     
     _move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
     
     for joint in _move_joint_goal.trajectory.joint_names:
         goal_tol = JointTolerance()
         goal_tol.name = joint
         goal_tol.position = 5.0
         goal_tol.velocity = 5.0
         goal_tol.acceleration = 5.0
         _move_joint_goal.goal_tolerance.append(goal_tol)
     _move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
     _move_joint_goal.trajectory.header.stamp = rospy.Time.now()
     
     rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
     userdata.move_joint_goal = _move_joint_goal
     return 'succeeded'

 def move_head(self, pan , tilt):   
     # Which joints define the head?
     head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
     # Create a single-point head trajectory with the head_goal as the end-point
     head_trajectory = JointTrajectory()
     head_trajectory.joint_names = head_joints
     head_trajectory.points.append(JointTrajectoryPoint())
     head_trajectory.points[0].positions = pan , tilt
     head_trajectory.points[0].velocities = [0.0 for i in head_joints]
     head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
     head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the head action server
     rospy.loginfo('Moving the head to goal position...')
     
     head_goal = FollowJointTrajectoryGoal()
     head_goal.trajectory = head_trajectory
     head_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal
     self.head_client.send_goal(head_goal)
     
     # Wait for up to 5 seconds for the motion to complete 
     self.head_client.wait_for_result(rospy.Duration(2.0))
     
     rospy.loginfo('...done')

    def move_to(self, result_trajectory, duration=5.0):
        trajectory = result_trajectory
        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()

 def execute_trajectory(self, trajectory, duration=5.0):
     follow_goal = FollowJointTrajectoryGoal()
     follow_goal.trajectory = trajectory
     self.client.send_goal(follow_goal)
     #rospy.sleep(1.5)
     #self.client.cancel_goal()
     self.client.wait_for_result()
     self.finish_time = rospy.Time.now() 
     self.believed_state = trajectory.points[-1].positions

    def __init__(self):
        rospy.init_node('trajectory_demo')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['link_1_joint',
                      'servo_2_joint',
                      'link_3_joint', 
                      'link_4_joint',
                      'link_5_joint']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0, 0, 0, 0, 0]
  
        else:
            # Set a goal configuration for the arm
            arm_goal  = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)
                
        rospy.loginfo('...done')

 def build_follow_trajectory(self, traj):
     # Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
     traj_goal = FollowJointTrajectoryGoal()
     traj_goal.trajectory = traj
     tolerance = JointTolerance()
     tolerance.position = 0.05
     tolerance.velocity = 0.1
     traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
     traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
     traj_goal.goal_time_tolerance = rospy.Duration(3)
     return traj_goal

    def btnPlay(self):
        if not self.ui.chkAllMotors.isChecked():
            print("all motors should have torque")
            return
        if self.ui.spinSetFrame.value()>self.ui.spinPlayTo.value() or self.ui.spinSetFrame.value()<1 or self.ui.spinPlayTo.value()>self.totalFrames:
            print("\nwrong range in play\n")
            return

        trajectories=[]
        for cat in self.motorTrajectoryTopics:
            jointTraj=JointTrajectory()
            jts_names=[]
            for name in self.names:
                #if self.motorNamesAndCat[name]==cat and name!='l_hip_yaw':
                if self.motorNamesAndCat[name]==cat:
                    jointTraj.joint_names.append(name+"_joint")
                    jts_names.append(name)

            fcount=self.ui.spinPlayTo.value()-self.ui.spinSetFrame.value()+1
            totalDelay=0.0
            for n in range(0,fcount):
                pos_arr=JointTrajectoryPoint()
                readLoc=n+self.ui.spinSetFrame.value()
                delaySecs=float(self.TimeDelayFromPrevious[readLoc-1])/1000.0
                totalDelay=totalDelay+delaySecs
                pos_arr.time_from_start=rospy.Duration().from_sec(totalDelay)
                for name in jts_names:
                    pos_arr.positions.append(self.TrajectoryInfo[name][readLoc-1])
                    print("\n "+name+": "+str(self.TrajectoryInfo[name][readLoc-1]))
                    velocity=1.0#2*pi rad/sec
                    if n!=0:
                        velocity=float(self.TrajectoryInfo[name][readLoc-1]-self.TrajectoryInfo[name][readLoc-2])/delaySecs
                    ##pos_arr.velocities.append(velocity)
                jointTraj.points.append(pos_arr)
            jointTraj.header.stamp=rospy.Time.now()
            trajectories.append(jointTraj)

        trajClients=[]
        mm=0
        for cat in self.motorTrajectoryTopics:
            trajClients.append(actionlib.SimpleActionClient(self.motorTrajectoryTopics[cat],FollowJointTrajectoryAction))
            trajClients[mm].wait_for_server()
            mm=mm+1
            #trajectories[nn] .. how to send this goal

        nn=0
        for traj in trajClients:
            goal=FollowJointTrajectoryGoal()
            #goal.trajectory.points=trajectories[nn].points
            goal.trajectory=trajectories[nn]
            traj.send_goal(goal)
            nn=nn+1

        print ("\nDone Sending Play Info...\n")

 def move_to(self, positions, duration=5.0):
     print "============= Moving"
     trajectory = JointTrajectory()
     trajectory.joint_names = self.joint_names
     trajectory.points.append(JointTrajectoryPoint())
     trajectory.points[0].positions = positions
     trajectory.points[0].velocities = [0.0 for _ in positions]
     trajectory.points[0].accelerations = [0.0 for _ in positions]
     trajectory.points[0].time_from_start = rospy.Duration(duration)
     follow_goal = FollowJointTrajectoryGoal()
     follow_goal.trajectory = trajectory

    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal);
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False

    def execute(self, trajectory, test=None, epsilon=0.1):
        """
        Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
        This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
        Non-blocking needs should deal with the JointTrajectory action server
        :param trajectory: either a RobotTrajectory or a JointTrajectory
        :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
        :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
        :return: True if execution ended successfully, False otherwise
        """
        def distance_to_first_point(point):
            joint_pos = np.array(point.positions)
            return np.linalg.norm(current_array - joint_pos)

        self.display(trajectory)
        with self._stop_lock:
            self._stop_reason = ''
        if isinstance(trajectory, RobotTrajectory):
            trajectory = trajectory.joint_trajectory
        elif not isinstance(trajectory, JointTrajectory):
            raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
        ftg = FollowJointTrajectoryGoal()
        ftg.trajectory = trajectory

        # check if it is necessary to move to the first point
        current = self.get_current_state()
        current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])

        if distance_to_first_point(trajectory.points[0]) > epsilon:
            # convert first point to robot state
            rs = RobotState()
            rs.joint_state.name = trajectory.joint_names
            rs.joint_state.position = trajectory.points[0].positions
            # move to the first point
            self.move_to_controlled(rs)

        # execute the input trajectory
        self.client.send_goal(ftg)
        # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory

        while self.client.simple_state != SimpleGoalState.DONE:
            if callable(test) and test():
                self.client.cancel_goal()
                return True

            if self._stop_reason!='':
                self.client.cancel_goal()
                return False

            self._rate.sleep()

        return True

    def __init__(self):
        rospy.init_node("gripper_trajectory_demo")

        # Set to True to move back to the starting configurations
        reset = rospy.get_param("~reset", False)

        # Set the duration of the trajectory in seconds
        duration = rospy.get_param("~duration", 2.0)

        # Which joints define the head?
        gripper_joints = ["right_gripper_finger_joint"]

        # Set a goal configuration for the head
        if reset:
            gripper_goal = [0]
        else:
            gripper_goal = [0.25]

        # Connect to the head trajectory action server
        rospy.loginfo("Waiting for gripper trajectory controller...")

        gripper_client = actionlib.SimpleActionClient(
            "right_gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction
        )

        gripper_client.wait_for_server()

        rospy.loginfo("...connected.")

        # Create a head trajectory with a single end point using the gripper_goal positions
        trajectory = JointTrajectory()
        trajectory.joint_names = gripper_joints
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[0].positions = gripper_goal
        trajectory.points[0].velocities = [0.0 for i in gripper_joints]
        trajectory.points[0].accelerations = [0.0 for i in gripper_joints]
        trajectory.points[0].time_from_start = rospy.Duration(duration)

        # Send the trajectory to the head action server
        rospy.loginfo("Moving the gripper to goal position...")

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal
        gripper_client.send_goal(goal)

        # Wait for up to 5 seconds for the motion to complete
        gripper_client.wait_for_result(rospy.Duration(5.0))

        rospy.loginfo("...done")