TrajectoryController: Use desired state to calculate hold trajectory

joint_trajectory_controller/CMakeLists.txt

@@ -96,6 +96,11 @@ if(CATKIN_ENABLE_TESTING)
                     test/joint_trajectory_controller_test.cpp)
   target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES})
 
+  add_rostest_gtest(joint_trajectory_controller_stopramp_test
+                    test/joint_trajectory_controller_stopramp.test
+                    test/joint_trajectory_controller_test.cpp)
+  target_link_libraries(joint_trajectory_controller_stopramp_test ${catkin_LIBRARIES})
+
   add_rostest_gtest(joint_trajectory_controller_vel_test
                     test/joint_trajectory_controller_vel.test
                     test/joint_trajectory_controller_test.cpp)

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h

@@ -208,7 +208,7 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
   ros::Duration state_publisher_period_;
   ros::Duration action_monitor_period_;
 
-  typename Segment::Time stop_trajectory_duration_;
+  typename Segment::Time stop_trajectory_duration_;  ///< Duration for stop ramp. If zero, the controller stops at the actual position.
   boost::dynamic_bitset<> successful_joint_traj_;
   bool allow_partial_joints_goal_;
 
@@ -240,8 +240,9 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
   /**
    * \brief Hold the current position.
    *
-   * Substitutes the current trajectory with a single-segment one going from the current position and velocity to the
-   * current position and zero velocity.
+   * Substitutes the current trajectory with a single-segment one going from the current position and velocity to
+   * zero velocity.
+   * \see parameter stop_trajectory_duration
    * \note This method is realtime-safe.
    */
   void setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr());

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -138,6 +138,13 @@ starting(const ros::Time& time)
   time_data.uptime = ros::Time(0.0);
   time_data_.initRT(time_data);
 
+  // Initialize the desired_state with the current state on startup
+  for (unsigned int i = 0; i < joints_.size(); ++i)
+  {
+    desired_state_.position[i] = joints_[i].getPosition();
+    desired_state_.velocity[i] = joints_[i].getVelocity();
+  }
+
   // Hold current position
   setHoldPosition(time_data.uptime);
 
@@ -736,45 +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 unsigned int n_joints = joints_.size();
   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)
   {
-    hold_start_state_.position[0]     =  joints_[i].getPosition();
-    hold_start_state_.velocity[0]     =  joints_[i].getVelocity();
-    hold_start_state_.acceleration[0] =  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(start_time,  hold_start_state_,
+                                              start_time, hold_start_state_);
+      // Set goal handle for the segment
+      (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
+    }
+  }
+  else
+  {
+    // 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]       =  joints_[i].getPosition();
-    hold_end_state_.velocity[0]       = -joints_[i].getVelocity();
-    hold_end_state_.acceleration[0]   =  0.0;
+    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;
+
+      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;
 
-    // Sample segment at its midpoint, that should have zero velocity
-    (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
+      (*hold_trajectory_ptr_)[i].front().init(start_time,  hold_start_state_,
+                                                               end_time_2x, 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_);
+      // Sample segment at its midpoint, that should have zero velocity
+      (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
 
-    // Set goal handle for the segment
-    (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
+      // 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_);
 }

joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test

@@ -0,0 +1,39 @@
+<launch>
+  <arg name="display_plots" default="false"/>
+
+  <!-- Load RRbot model -->
+  <param name="robot_description"
+      command="$(find xacro)/xacro '$(find joint_trajectory_controller)/test/rrbot.xacro'" />
+
+  <!-- Start RRbot -->
+  <node name="rrbot"
+      pkg="joint_trajectory_controller"
+      type="rrbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find joint_trajectory_controller)/test/rrbot_controllers_stopramp.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="rrbot_controller" />
+
+  <group if="$(arg display_plots)">
+    <!-- rqt_plot monitoring -->
+    <node name="rrbot_pos_monitor"
+          pkg="rqt_plot"
+          type="rqt_plot"
+          args="/rrbot_controller/state/desired/positions[0]:positions[1],/rrbot_controller/state/actual/positions[0]:positions[1]" />
+
+    <node name="rrbot_vel_monitor"
+          pkg="rqt_plot"
+          type="rqt_plot"
+          args="/rrbot_controller/state/desired/velocities[0]:velocities[1],/rrbot_controller/state/actual/velocities[0]:velocities[1]" />
+  </group>
+
+  <!-- Controller test -->
+  <test test-name="joint_trajectory_controller_stopramp_test"
+        pkg="joint_trajectory_controller"
+        type="joint_trajectory_controller_stopramp_test"
+        time-limit="85.0"/>
+</launch>