Python JointTrajectoryPoint.duration方法代码示例

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import duration [as 别名]
  def sendWaypointTrajectory(self, waypoints, durations = 0., vels = 0., accs = 0., effs = 0.):
    if not self.ang_cmd_wait(waypoints[0]):
      print 'Cannot go to the first point in the trajectory'
      return None
#    else: 
#      print 'Went to first'
      
    if not self.traj_connection:
      print 'Action server connection was not established'
      return None
    joint_traj = JointTrajectory()
    joint_traj.joint_names = self.arm_joint_names;
    
    if not durations == 0:
      if not len(durations) == waypoints:
        raise Exception('The number of duration points is not equal to the number of provided waypoints')
    if not vels == 0:
      if not len(vels) == waypoints:
        raise Exception('The number velocity points is not equal to the number of provided waypoints')
    if not accs == 0:
      if not len(accs) == waypoints:
        raise Exception('The number acceleration points is not equal to the number of provided waypoints')
    if not effs == 0:
      if not len(effs) == waypoints:
        raise Exception('The number effort points is not equal to the number of provided waypoints')

    if not effs == 0:    
      if not (vels == 0 and accs == 0):
        raise Exception('Cannot specify efforts with velocities and accelerations at the same time')
    if (not accs == 0) and vels == 0:
      raise Exception('Cannot specify accelerations without velocities')
   
    total_time_from_start = 0.5; 
    for t in range(0, len(waypoints)):
      point = JointTrajectoryPoint()
  
      waypoint = waypoints[t]   
      if not len(waypoint) == len(joint_traj.joint_names):
        raise Exception('The number of provided joint positions is not equal to the number of available joints for index: ' + str(t))
      point.positions = waypoint
    
      if not vels == 0.:
        velocity = vels[t]
        if not len(velocity) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t))
        point.velocities = velocity

      if not accs == 0.:
        acceleration = accs[t]
        if not len(acceleration) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t))
        point.accelerations = accelerations
      
      if not effs == 0.:
        effort = effs[t]
        if not len(effort) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t))
        point.effort = effort 

      if not durations == 0.:
        point.duration = duration

      # Deal with increasing time for each trajectory point
      point.time_from_start = rospy.Duration(total_time_from_start)
      total_time_from_start = total_time_from_start + 1.0 

      # Set the points
      joint_traj.points.append(point)
     
    traj_goal = FollowJointTrajectoryGoal()
    traj_goal.trajectory = joint_traj

    self.smooth_joint_trajectory_client.send_goal(traj_goal)
    self.smooth_joint_trajectory_client.wait_for_result()
    return self.smooth_joint_trajectory_client.get_result()