[JTC] Set allow_nonzero_velocity_at_trajectory_end default false a…

…nd rename class variables (backport #834) (#843)

joint_trajectory_controller/doc/parameters.rst

@@ -66,7 +66,7 @@ start_with_holding (bool)
 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
+  Default: false
 
 cmd_timeout (double)
   Timeout after which the input command is considered stale.
@@ -147,5 +147,4 @@ gains.<joint_name>.angle_wraparound (bool)
   Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
   position :math:`s` from the state interface.
 
-
   Default: false

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -167,8 +167,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   std::vector<PidPtr> pids_;
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
   std::vector<double> ff_velocity_scale_;
-  // Configuration for every joint, if position error is normalized
-  std::vector<bool> normalize_joint_error_;
+  // Configuration for every joint, if position error is wrapped around
+  std::vector<bool> joints_angle_wraparound_;
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -65,12 +65,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
   }
 
   // TODO(christophfroehlich): remove deprecation warning
-  if (params_.allow_nonzero_velocity_at_trajectory_end)
+  if (params_.allow_nonzero_velocity_at_trajectory_end == false)
   {
     RCLCPP_WARN(
       get_node()->get_logger(),
-      "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to "
-      "true. The default behavior will change to false.");
+      "\"allow_nonzero_velocity_at_trajectory_end\" is set to false. (The default behavior changed "
+      "to false, and trajectories might get discarded.)");
   }
 
   return CallbackReturn::SUCCESS;
@@ -142,7 +142,7 @@ controller_interface::return_type JointTrajectoryController::update(
                                    const JointTrajectoryPoint & desired)
   {
     // error defined as the difference between current and desired
-    if (normalize_joint_error_[index])
+    if (joints_angle_wraparound_[index])
     {
       // if desired, the shortest_angular_distance is calculated, i.e., the error is
       //  normalized between -pi<error<pi
@@ -728,19 +728,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   }
 
   // Configure joint position error normalization from ROS parameters (angle_wraparound)
-  normalize_joint_error_.resize(dof_);
+  joints_angle_wraparound_.resize(dof_);
   for (size_t i = 0; i < dof_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
     if (gains.normalize_error)
     {
       // TODO(anyone): Remove deprecation warning in the end of 2023
       RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!");
-      normalize_joint_error_[i] = gains.normalize_error;
+      joints_angle_wraparound_[i] = gains.normalize_error;
     }
     else
     {
-      normalize_joint_error_[i] = gains.angle_wraparound;
+      joints_angle_wraparound_[i] = gains.angle_wraparound;
     }
   }
 

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -80,7 +80,7 @@ joint_trajectory_controller:
   }
   allow_nonzero_velocity_at_trajectory_end: {
     type: bool,
-    default_value: true,
+    default_value: false,
     description: "If false, the last velocity point has to be zero or the goal will be rejected",
   }
   cmd_timeout: {

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -653,7 +653,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr
 
   // will be accepted despite nonzero last point
   EXPECT_TRUE(gh_future.get());
-  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+  if ((traj_controller_->has_effort_command_interface()) == false)
+  {
+    // can't succeed with effort cmd if
+    EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+  }
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false)

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -201,8 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
 TEST_P(TrajectoryControllerTestParameterized, cleanup)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params);
 
   // send msg
@@ -492,14 +491,13 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
 // Floating-point value comparison threshold
 const double EPS = 1e-6;
 /**
- * @brief check if position error of revolute joints are normalized if not configured so
+ * @brief check if position error of revolute joints are angle_wraparound if not configured so
  */
-TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
+TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
   subscribeToState();
 
@@ -742,14 +740,13 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
 }
 
 /**
- * @brief check if position error of revolute joints are normalized if configured so
+ * @brief check if position error of revolute joints are angle_wraparound if configured so
  */
-TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
+TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  std::vector<rclcpp::Parameter> params = {};
   SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true);
   subscribeToState();
 
@@ -790,7 +787,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
   EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
   EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
 
-  // is error.positions[2] normalized?
+  // is error.positions[2] angle_wraparound?
   EXPECT_NEAR(
     state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
   EXPECT_NEAR(
@@ -819,7 +816,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
         k_p * allowed_delta);
-      // is error of positions[2] normalized?
+      // is error of positions[2] angle_wraparound?
       EXPECT_GT(0.0, joint_vel_[2]);
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
@@ -847,7 +844,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
         k_p * allowed_delta);
-      // is error of positions[2] normalized?
+      // is error of positions[2] angle_wraparound?
       EXPECT_GT(0.0, joint_eff_[2]);
       EXPECT_NEAR(
         k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -200,7 +200,7 @@ class TrajectoryControllerTest : public ::testing::Test
   }
 
   void SetPidParameters(
-    double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false)
+    double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false)
   {
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
@@ -213,27 +213,31 @@ class TrajectoryControllerTest : public ::testing::Test
       const rclcpp::Parameter k_d(prefix + ".d", 0.0);
       const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0);
       const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default);
-      const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default);
-      node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error});
+      const rclcpp::Parameter angle_wraparound(
+        prefix + ".angle_wraparound", angle_wraparound_default);
+      node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound});
     }
   }
 
   void SetUpAndActivateTrajectoryController(
     rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
     bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
-    bool normalize_error = false)
+    bool angle_wraparound = false)
   {
     SetUpTrajectoryController(executor);
 
+    // add this to simplify tests, can be overwritten by parameters
+    rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true);
+    traj_controller_->get_node()->set_parameter(nonzero_vel_parameter);
+
     // set pid parameters before configure
-    SetPidParameters(k_p, ff, normalize_error);
+    SetPidParameters(k_p, ff, angle_wraparound);
+
+    // set optional parameters
     for (const auto & param : parameters)
     {
       traj_controller_->get_node()->set_parameter(param);
     }
-    // ignore velocity tolerances for this test since they aren't committed in test_robot->write()
-    rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0);
-    traj_controller_->get_node()->set_parameter(stopped_velocity_parameters);
 
     traj_controller_->get_node()->configure();