preserve jump, if stop_trajectory_duration==0

old behaviour for stop_trajectory_duration==0

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -743,47 +743,64 @@ template <class SegmentImpl, class HardwareInterface>
 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
 setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh)
 {
-  // Settle position in a fixed time. We do the following:
-  // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time
-  // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity
-  // - Create segment that goes from current state to above zero velocity state, in the desired time
-  // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types
-
   assert(joint_names_.size() == hold_trajectory_ptr_->size());
 
   typename Segment::State hold_start_state_ = typename Segment::State(1);
   typename Segment::State hold_end_state_ = typename Segment::State(1);
-
-  const typename Segment::Time start_time  = time.toSec();
-  const typename Segment::Time end_time    = time.toSec() + stop_trajectory_duration_;
-  const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_;
-
-  // Create segment that goes from current (pos,vel) to (pos,-vel)
   const unsigned int n_joints = joints_.size();
-  for (unsigned int i = 0; i < n_joints; ++i)
+
+  if(stop_trajectory_duration_ == 0.0)
+  {
+    // stop at current actual position
+    for (unsigned int i = 0; i < n_joints; ++i)
+    {
+      hold_start_state_.position[0]     =  joints_[i].getPosition();
+      hold_start_state_.velocity[0]     =  0.0;
+      hold_start_state_.acceleration[0] =  0.0;
+      (*hold_trajectory_ptr_)[i].front().init(time.toSec(),  hold_start_state_,
+                                                           time.toSec(), hold_start_state_);
+      // Set goal handle for the segment
+      (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
+    }
+  }
+  else
   {
-    // If there is a time delay in the system it is better to use calculate the hold trajectory starting from the
-    // desired position. Otherwise there would be a jerk in the motion.
-    hold_start_state_.position[0]     =  desired_state_.position[i];
-    hold_start_state_.velocity[0]     =  desired_state_.velocity[i];
-    hold_start_state_.acceleration[0] =  0.0;
+    // Settle position in a fixed time. We do the following:
+    // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time
+    // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity
+    // - Create segment that goes from current state to above zero velocity state, in the desired time
+    // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types
 
-    hold_end_state_.position[0]       =  desired_state_.position[i];
-    hold_end_state_.velocity[0]       = -desired_state_.velocity[i];
-    hold_end_state_.acceleration[0]   =  0.0;
+    const typename Segment::Time start_time  = time.toSec();
+    const typename Segment::Time end_time    = time.toSec() + stop_trajectory_duration_;
+    const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_;
 
-    (*hold_trajectory_ptr_)[i].front().init(start_time,  hold_start_state_,
-                                                             end_time_2x, hold_end_state_);
+    // Create segment that goes from current (pos,vel) to (pos,-vel)
+    for (unsigned int i = 0; i < n_joints; ++i)
+    {
+      // If there is a time delay in the system it is better to calculate the hold trajectory starting from the
+      // desired position. Otherwise there would be a jerk in the motion.
+      hold_start_state_.position[0]     =  desired_state_.position[i];
+      hold_start_state_.velocity[0]     =  desired_state_.velocity[i];
+      hold_start_state_.acceleration[0] =  0.0;
 
-    // Sample segment at its midpoint, that should have zero velocity
-    (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
+      hold_end_state_.position[0]       =  desired_state_.position[i];
+      hold_end_state_.velocity[0]       = -desired_state_.velocity[i];
+      hold_end_state_.acceleration[0]   =  0.0;
 
-    // Now create segment that goes from current state to one with zero end velocity
-    (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
-                                                             end_time,   hold_end_state_);
+      (*hold_trajectory_ptr_)[i].front().init(start_time,  hold_start_state_,
+                                                               end_time_2x, hold_end_state_);
 
-    // Set goal handle for the segment
-    (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
+      // Sample segment at its midpoint, that should have zero velocity
+      (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
+
+      // Now create segment that goes from current state to one with zero end velocity
+      (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
+                                                               end_time,   hold_end_state_);
+
+      // Set goal handle for the segment
+      (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
+    }
   }
   curr_trajectory_box_.set(hold_trajectory_ptr_);
 }