add init trajectory for future start time

[knorth55]

Future motion trajectory support

Background

this PR supports FollowJointTrajectoryGoal with future start time. For example, I want to send FollowJointTrajectoryGoal for 3 seconds ahead from now, to reserve future motion like below. We can reserve future motion FollowJointTrajectory server by giving trajectory with future time.

• msg

  $ rostopic echo /arm_controller/follow_joint_trajectory/goal 
  header: 
  seq: 1
  stamp: 
  secs: 139
  nsecs:  30000000
  frame_id: ''
  goal_id: 
  stamp: 
  secs: 0
  nsecs:         0
  id: "139030000000_/fetch_1556207748218709686_19549_/arm_controller/follow_joint_trajectory_1"
  goal: 
  trajectory: 
  header: 
    seq: 1
    stamp: 
      secs: 142
      nsecs:  30000000
    frame_id: ''
  joint_names: [shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint,
  wrist_flex_joint, wrist_roll_joint]
  points: 
    - 
      positions: [1.3197694449880988, 1.3991785917859492, -0.19928290787532266, 1.7191481448291217, 0.00038196209399465886, 1.6598884208000166, -0.00035014670009214655]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 0
        nsecs:         0
    - 
      positions: [1.107530577097858, 1.1981623276989923, -0.15998741079181833, 1.6934575521432187, -0.04578636799192892, 1.5575675935764846, -0.17920807032302388]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 634640512
    - 
      positions: [0.8952917092076174, 0.9971460636120364, -0.120691913708314, 1.6677669594573157, -0.0919546980778525, 1.4552467663529534, -0.3580659939459556]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 959281024
    - 
      positions: [0.9123999186165688, 0.9151214273993706, -0.10540328738142896, 1.3492815707563999, -0.009772269783560009, 1.8071148701552806, -0.1383160317512896]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 720491392
    - 
      positions: [0.9295081280255202, 0.8330967911867053, -0.09011466105454391, 1.030796182055485, 0.07241015851073251, 2.158982973957608, 0.08143393044337643]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 481701824
    - 
      positions: [0.7436162034321185, 0.6666139034448078, -0.07225045783024109, 0.8244876532397174, 0.057833318553449975, 1.727184043800877, 0.06498808708117165]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 954060544
    - 
      positions: [0.5577242788387169, 0.5001310157029106, -0.05438625460593835, 0.6181791244239503, 0.043256478596167464, 1.2953851136441452, 0.0485422437189669]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 426419296
    - 
      positions: [0.37183235424531524, 0.3336481279610135, -0.036522051381635556, 0.4118705956081832, 0.028679638638884966, 0.8635861834874143, 0.032096400356762145]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 898778048
    - 
      positions: [0.1859404296519137, 0.16716524021911616, -0.01865784815733279, 0.20556206679241595, 0.014102798681602441, 0.4317872533306826, 0.015650556994557405]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 4
        nsecs: 371136800
    - 
      positions: [4.850505851209156e-05, 0.0006823524772189555, -0.0007936449330300092, -0.0007464620233513415, -0.00047404127568006506, -1.1676826048642426e-05, -0.0007952863676473497]
      velocities: []
      accelerations: []
      effort: []
      time_from_start: 
        secs: 4
        nsecs: 963495552
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
  secs: 0
  nsecs:         0
  ---

The most important point is msg.trajectory.header.stamp is 3 seconds ahead from msg.header.stamp.

• msg.header.stamp

  header: 
  stamp: 
  secs: 139
  nsecs:  30000000

• msg.trajectory.header.stamp

  goal: 
  trajectory: 
  header: 
    stamp: 
      secs: 142
      nsecs:  30000000

Problem

When I send the FollowJointTrajectoryGoal as above, fetch robot stays in arm servo off before motion. That is because there is no arm effort command after FollowJointTrajectoryGoal is received and before motion starts. In Gazebo simulation, fetch arm is sinking into its body. I confirm that same problem happens with real robot, too.

This PR

This PR put current position as trajectory at the top of the trajectory when there is gap between motion start time and motion request receiving time. With this PR, Fetch robot arm does not stay in arm survo off as below.

Related issue

https://github.com/jsk-ros-pkg/jsk_robot/pull/1070#issuecomment-486311568

cc. @pazeshun

[knorth55]

I also want to backport this PR to indigo-devel branch.

[knorth55]

@rctoris can you review this PR? This bug may cause robot malfunction

[knorth55]

@moriarty can you review this PR? this error may cause robot malfunction. (i.e. robot arm moves quickly after servo off)

[knorth55]

@moriarty kindly ping. can you review this PR?

[knorth55]

kindly ping. can anyone review this PR?

[moriarty]

Hi sorry you've pinged me a couple times but I didn't see any of these until now.

I'm currently working remotely and can't test this on a Fetch. The change looks fine.

I'll ping @erelson otherwise I'll be able to test in on Monday.

[erelson]

Looking at the code, and your example, this looks good and makes sense. I unfortunately won't have time to test it this week, so I'll leave that to @moriarty...

[knorth55]

I updated to use executable_trajectory.points[0].time and tested on Gazebo.

[knorth55]

How is this PR review going?

[knorth55]

Kindly ping

[erelson]

As expected, no effect on regular operation of a live robot (18.04). Sorry for the delay in getting this tested!

[knorth55]

@erelson Thank you for merging this PR!