Python JointTrajectory.header方法代码示例

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
    def joint_trajectory(self, joint_trajectory):
        '''
        Returns just the part of the trajectory corresponding to the arm.

        **Args:**
        
            **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert

        **Returns:**
            A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory

        **Raises:**
            
            **ValueError:** If some arm joint is not found in joint_trajectory
        '''
        arm_trajectory = JointTrajectory()
        arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
        arm_trajectory.joint_names = copy.copy(self.joint_names)
        indexes = [-1]*len(self.joint_names)
        for (i, an) in enumerate(self.joint_names):
            for (j, n) in enumerate(joint_trajectory.joint_names):
                if an == n:
                    indexes[i] = j
                    break
            if indexes[i] < 0:
                raise ValueError('Joint '+n+' is missing from joint trajectory')
        
        for joint_point in joint_trajectory.points:
            arm_point = JointTrajectoryPoint()
            arm_point.time_from_start = joint_point.time_from_start
            for i in indexes:
                arm_point.positions.append(joint_point.positions[i])
            arm_trajectory.points.append(arm_point)
        return arm_trajectory

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
    def convert_trajectory(self, traj):
        """ Converts a trajectory into a joint trajectory
        Args:
            traj: Trajectory to convert
        Returns:
            joint trajectory
        """
        new_traj = JointTrajectory()

        new_traj.header = traj.header
        # Take each joint in the trajectory and add it to the joint trajectory
        for joint_name in traj.joint_names:
            new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
        # For each poiint in the trajectory
        for point in traj.points:
            new_point = JointTrajectoryPoint()
            for i, pos in enumerate(point.positions):
                new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
            for i, vel in enumerate(point.velocities):
                new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))

            new_point.time_from_start = point.time_from_start
            new_traj.points.append(new_point)

        return new_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
 def push_one_arm(self, new_plan, left_arm_joints):
     # Build a new joint trajectory with left_arm_joint values at those times
     temp_traj = JointTrajectory()
     new_traj = new_plan.joint_trajectory
     temp_traj.header = new_traj.header
     temp_traj.joint_names = self.traj.joint_names
     zero_vec = [0.0]*7
     for i in range(len(new_traj.points)):
         temp_traj.points[i].positions = left_arm_joints.values() + new_traj.points[i].positions
         temp_traj.points[i].velocities = zero_vec + new_traj.points[i].velocities
         temp_traj.points[i].accelerations = zero_vec + new_traj.points[i].accelerations
         temp_traj.points[i].time_from_start = new_traj.points[i].time_from_start
     self.traj = temp_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
def talker():
    pub_traj = rospy.Publisher('joint_path_command', JointTrajectory)
    rospy.init_node('path_publisher')
        
    traj = JointTrajectory()
    traj.header = Header(frame_id='base_link', stamp=rospy.Time.now() + rospy.Duration(1.0))
    traj.joint_names = ['joint_1', 'joint_2', 'joint_3', 
                        'joint_4', 'joint_5', 'joint_6']
    traj.points = [JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0],
                                        velocities=[0, 0, 0, 0, 0, 0],
                                        time_from_start=rospy.Duration(1)),
                   JointTrajectoryPoint(positions=[1, 0, 0, 0, 0, 0],
                                        velocities=[0, 0, 0, 0, 0, 0])]
    rospy.loginfo(traj)
    pub_traj.publish(traj)
        
    rospy.sleep(1.0)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
 def substitute_trajectory(self, trajectory):
     jt = JointTrajectory()
     jt.joint_names = self.real_joint_names
     jt.header = trajectory.header
     for jtp_in in trajectory.points:
         jtp = JointTrajectoryPoint()
         if jtp_in.positions:
             joint_position = jtp_in.positions[0]
             # Each finger will go to half of the size
             jtp.positions = [joint_position / 2.0, joint_position / 2.0]
             jtp.time_from_start = jtp_in.time_from_start
         else:
             rospy.logwarn("Trajectory has no positions...")
         if jtp_in.velocities:
             jtp.velocities.append(jtp_in.velocities[0])
         if jtp_in.accelerations:
             jtp.accelerations.append(jtp_in.accelerations[0])
         if jtp_in.effort:
             jtp.effort.append(jtp_in.effort[0])
         jt.points.append(jtp)
     return jt

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import header [as 别名]
def functional(axis, angle):
    pub_rosey = rospy.Publisher(
      '/cyton_joint_trajectory_action_controller/command',
      JointTrajectory, queue_size=10)
    rospy.init_node('traj_maker', anonymous=True)
    time.sleep(1)

    axis = int(axis)
    angle = float(angle)

    rate = rospy.Rate(0.01)
    while not rospy.is_shutdown():

           #  first way to define a point
           traj_waypoint_1_rosey = JointTrajectoryPoint()

           traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0]
           traj_waypoint_1_rosey.time_from_start = Duration(2)
           
           #  second way to define a point
           traj_waypoint_2_rosey = traj_waypoint_1_rosey
           traj_waypoint_2_rosey.time_from_start = Duration(4)
           traj_waypoint_2_rosey.positions[axis-1] = angle
           
           time.sleep(1)

           traj_waypoint_3_rosey = traj_waypoint_2_rosey
           traj_waypoint_3_rosey.time_from_start = Duration(7)

           #traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0],
            #time_from_start = Duration(4))
           #traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0],
            #time_from_start = Duration(6))
           #traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0],
            #time_from_start = Duration(8))
           #traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0],
            #time_from_start = Duration(10))   
           #traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12))

           #debug in terminal
           print traj_waypoint_2_rosey.positions
           
           #  making message
           message_rosey = JointTrajectory()
           
           #  required headers
           header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
           message_rosey.header = header_rosey
           
           #  adding in joints
           joint_names = ['shoulder_roll_joint', \
  'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \
  'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint']
           message_rosey.joint_names = joint_names
           
           #  appending up to 100 points
           # ex. for i in enumerate(len(waypoints)): append(waypoints[i])

           message_rosey.points.append(traj_waypoint_1_rosey)
           message_rosey.points.append(traj_waypoint_2_rosey)
           message_rosey.points.append(traj_waypoint_3_rosey)
           #message_rosey.points.append(traj_waypoint_4_rosey)
           #message_rosey.points.append(traj_waypoint_5_rosey)
           #message_rosey.points.append(traj_waypoint_6_rosey)
           #  publishing to ROS node
           pub_rosey.publish(message_rosey)
         
           rate.sleep()
           
           if rospy.is_shutdown():
               break