[JTC] Reject trajectories with nonzero terminal velocity (#567)

* Reject receiving trajectory of last velocity point is non-zero

* Update docs

* Add tests

* Change to parameterized test

* Rename parameter

* not true -> false

---------

Co-authored-by: Bence Magyar <[email protected]>
(cherry picked from commit 99b67d7)

Author: 2 people

joint_trajectory_controller/doc/userdoc.rst

@@ -159,6 +159,11 @@ open_loop_control (boolean)
 
   If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits<double>::quiet_NaN()) or state values when the hardware is started.
 
+allow_nonzero_velocity_at_trajectory_end (boolean)
+  If false, the last velocity point has to be zero or the goal will be rejected.
+
+  Default: true
+
 constraints (structure)
   Default values for tolerances if no explicit values are states in JointTrajectory message.
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -67,6 +67,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
     return CallbackReturn::ERROR;
   }
 
+  // TODO(christophfroehlich): remove deprecation warning
+  if (params_.allow_nonzero_velocity_at_trajectory_end)
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to "
+      "true. The default behavior will change to false.");
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -1414,6 +1423,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
     }
   }
 
+  if (!params_.allow_nonzero_velocity_at_trajectory_end)
+  {
+    for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
+    {
+      if (trajectory.points.back().velocities.at(i) != 0.)
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
+          trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
+        return false;
+      }
+    }
+  }
+
   rclcpp::Duration previous_traj_time(0ms);
   for (size_t i = 0; i < trajectory.points.size(); ++i)
   {

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -74,6 +74,11 @@ joint_trajectory_controller:
       one_of<>: [["splines", "none"]],
     }
   }
+  allow_nonzero_velocity_at_trajectory_end: {
+    type: bool,
+    default_value: true,
+    description: "If false, the last velocity point has to be zero or the goal will be rejected",
+  }
   gains:
     __map_joints:
       p: {

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -629,6 +629,130 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
   }
 }
 
+TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true)
+{
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpExecutor(params);
+  SetUpControllerHardware();
+
+  std::shared_future<typename GoalHandle::SharedPtr> gh_future;
+  // send goal with nonzero last velocities
+  {
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point1;
+    point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
+    point1.positions.resize(joint_names_.size());
+    point1.velocities.resize(joint_names_.size());
+
+    point1.positions[0] = 4.0;
+    point1.positions[1] = 5.0;
+    point1.positions[2] = 6.0;
+    point1.velocities[0] = 4.0;
+    point1.velocities[1] = 5.0;
+    point1.velocities[2] = 6.0;
+    points.push_back(point1);
+
+    JointTrajectoryPoint point2;
+    point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
+    point2.positions.resize(joint_names_.size());
+    point2.velocities.resize(joint_names_.size());
+
+    point2.positions[0] = 7.0;
+    point2.positions[1] = 8.0;
+    point2.positions[2] = 9.0;
+    point2.velocities[0] = 4.0;
+    point2.velocities[1] = 5.0;
+    point2.velocities[2] = 6.0;
+    points.push_back(point2);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
+  }
+  controller_hw_thread_.join();
+
+  // will be accepted despite nonzero last point
+  EXPECT_TRUE(gh_future.get());
+  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+}
+
+TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false)
+{
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)};
+  SetUpExecutor(params);
+  SetUpControllerHardware();
+
+  std::shared_future<typename GoalHandle::SharedPtr> gh_future;
+  // send goal with nonzero last velocities
+  {
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point1;
+    point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
+    point1.positions.resize(joint_names_.size());
+    point1.velocities.resize(joint_names_.size());
+
+    point1.positions[0] = 4.0;
+    point1.positions[1] = 5.0;
+    point1.positions[2] = 6.0;
+    point1.velocities[0] = 4.0;
+    point1.velocities[1] = 5.0;
+    point1.velocities[2] = 6.0;
+    points.push_back(point1);
+
+    JointTrajectoryPoint point2;
+    point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
+    point2.positions.resize(joint_names_.size());
+    point2.velocities.resize(joint_names_.size());
+
+    point2.positions[0] = 7.0;
+    point2.positions[1] = 8.0;
+    point2.positions[2] = 9.0;
+    point2.velocities[0] = 4.0;
+    point2.velocities[1] = 5.0;
+    point2.velocities[2] = 6.0;
+    points.push_back(point2);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
+  }
+  controller_hw_thread_.join();
+
+  EXPECT_FALSE(gh_future.get());
+
+  // send goal with last velocity being zero
+  {
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point1;
+    point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
+    point1.positions.resize(joint_names_.size());
+    point1.velocities.resize(joint_names_.size());
+
+    point1.positions[0] = 4.0;
+    point1.positions[1] = 5.0;
+    point1.positions[2] = 6.0;
+    point1.velocities[0] = 4.0;
+    point1.velocities[1] = 5.0;
+    point1.velocities[2] = 6.0;
+    points.push_back(point1);
+
+    JointTrajectoryPoint point2;
+    point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
+    point2.positions.resize(joint_names_.size());
+    point2.velocities.resize(joint_names_.size());
+
+    point2.positions[0] = 7.0;
+    point2.positions[1] = 8.0;
+    point2.positions[2] = 9.0;
+    point2.velocities[0] = 0.0;
+    point2.velocities[1] = 0.0;
+    point2.velocities[2] = 0.0;
+    points.push_back(point2);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
+  }
+
+  EXPECT_TRUE(gh_future.get());
+}
+
 // position controllers
 INSTANTIATE_TEST_SUITE_P(
   PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -65,6 +65,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   SetUpTrajectoryController(executor);
+  traj_controller_->get_node()->set_parameter(
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
 
   // const auto future_handle_ = std::async(std::launch::async, spin, &executor);
 
@@ -202,7 +204,9 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
 TEST_P(TrajectoryControllerTestParameterized, cleanup)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor);
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpAndActivateTrajectoryController(executor, true, params);
 
   // send msg
   constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
@@ -259,6 +263,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   SetUpTrajectoryController(executor, false);
+  traj_controller_->get_node()->set_parameter(
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
 
   // This call is replacing the way parameters are set via launch
   SetParameters();
@@ -511,7 +517,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false);
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false);
   subscribeToState();
 
   size_t n_joints = joint_names_.size();
@@ -609,7 +617,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true);
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true);
   subscribeToState();
 
   size_t n_joints = joint_names_.size();