[JTC] Configurable joint positon error normalization behavior

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -166,6 +166,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_;
   // 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

@@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update(
                                    const JointTrajectoryPoint & desired)
   {
     // error defined as the difference between current and desired
-    error.positions[index] =
-      angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
+    if (normalize_joint_error_[index])
+    {
+      // if desired, the shortest_angular_distance is calculated, i.e., the error is
+      //  normalized between -pi<error<pi
+      error.positions[index] =
+        angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
+    }
+    else
+    {
+      error.positions[index] = desired.positions[index] - current.positions[index];
+    }
     if (has_velocity_state_interface_ && has_velocity_command_interface_)
     {
       error.velocities[index] = desired.velocities[index] - current.velocities[index];
@@ -243,8 +252,7 @@ controller_interface::return_type JointTrajectoryController::update(
           {
             tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
                               pids_[i]->computeCommand(
-                                state_desired_.positions[i] - state_current_.positions[i],
-                                state_desired_.velocities[i] - state_current_.velocities[i],
+                                state_error_.positions[i], state_error_.velocities[i],
                                 (uint64_t)period.nanoseconds());
           }
         }
@@ -567,6 +575,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     }
   }
 
+  // Configure joint position error normalization from ROS parameters
+  normalize_joint_error_.resize(dof_);
+  for (size_t i = 0; i < dof_; ++i)
+  {
+    const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
+    normalize_joint_error_[i] = gains.normalize_error;
+  }
+
   if (params_.state_interfaces.empty())
   {
     RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty.");

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -101,6 +101,11 @@ joint_trajectory_controller:
         default_value: 0.0,
         description: "Feed-forward scaling of velocity."
       }
+      normalize_error: {
+        type: bool,
+        default_value: false,
+        description: "Use position error normalization to -pi to pi."
+      }
   constraints:
     stopped_velocity_tolerance: {
       type: double,

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -487,6 +487,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
   EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros);
 }
 
+// Floating-point value comparison threshold
+const double EPS = 1e-6;
+/**
+ * @brief check if position error of revolute joints are normalized if not configured so
+*/
+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);
+  subscribeToState();
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  // *INDENT-ON*
+  publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // Spin to receive latest state
+  executor.spin_some();
+  auto state_msg = getState();
+  ASSERT_TRUE(state_msg);
+
+  const auto allowed_delta = 0.1;
+
+  // no update of state_interface
+  EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
+
+  // has the msg the correct vector sizes?
+  EXPECT_EQ(n_joints, state_msg->desired.positions.size());
+  EXPECT_EQ(n_joints, state_msg->actual.positions.size());
+  EXPECT_EQ(n_joints, state_msg->error.positions.size());
+
+  // are the correct desired positions used?
+  EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
+  EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
+  EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
+
+  // no normalization of position error
+  EXPECT_NEAR(
+    state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+
+  if (traj_controller_->has_position_command_interface())
+  {
+    // check command interface
+    EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
+    EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
+    EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
+  }
+
+  if (traj_controller_->has_velocity_command_interface())
+  {
+    // check command interface
+    EXPECT_LE(0.0, joint_vel_[0]);
+    EXPECT_LE(0.0, joint_vel_[1]);
+    EXPECT_LE(0.0, joint_vel_[2]);
+
+    // use_closed_loop_pid_adapter_
+    if (traj_controller_->use_closed_loop_pid_adapter())
+    {
+      // we expect u = k_p * (s_d-s)
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+        k_p * allowed_delta);
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+        k_p * allowed_delta);
+      // no normalization of position error
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
+        k_p * allowed_delta);
+    }
+  }
+
+  if (traj_controller_->has_effort_command_interface())
+  {
+    // check command interface
+    EXPECT_LE(0.0, joint_eff_[0]);
+    EXPECT_LE(0.0, joint_eff_[1]);
+    EXPECT_LE(0.0, joint_eff_[2]);
+  }
+
+  executor.cancel();
+}
+
+/**
+ * @brief check if position error of revolute joints are normalized if configured so
+*/
+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);
+  subscribeToState();
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  // *INDENT-ON*
+  publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // Spin to receive latest state
+  executor.spin_some();
+  auto state_msg = getState();
+  ASSERT_TRUE(state_msg);
+
+  const auto allowed_delta = 0.1;
+
+  // no update of state_interface
+  EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS);
+
+  // has the msg the correct vector sizes?
+  EXPECT_EQ(n_joints, state_msg->desired.positions.size());
+  EXPECT_EQ(n_joints, state_msg->actual.positions.size());
+  EXPECT_EQ(n_joints, state_msg->error.positions.size());
+
+  // are the correct desired positions used?
+  EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta);
+  EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta);
+  EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta);
+
+  // is error.positions[2] normalized?
+  EXPECT_NEAR(
+    state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[2],
+    state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);
+
+  if (traj_controller_->has_position_command_interface())
+  {
+    // check command interface
+    EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
+    EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
+    EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
+  }
+
+  if (traj_controller_->has_velocity_command_interface())
+  {
+    // check command interface
+    EXPECT_LE(0.0, joint_vel_[0]);
+    EXPECT_LE(0.0, joint_vel_[1]);
+
+    // use_closed_loop_pid_adapter_
+    if (traj_controller_->use_closed_loop_pid_adapter())
+    {
+      EXPECT_GE(0.0, joint_vel_[2]);
+      // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+        k_p * allowed_delta);
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+        k_p * allowed_delta);
+      // is error of positions[2] normalized?
+      EXPECT_NEAR(
+        k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
+        k_p * allowed_delta);
+    }
+    else
+    {
+      // interpolated points_velocities only
+      EXPECT_LE(0.0, joint_vel_[2]);
+    }
+  }
+
+  if (traj_controller_->has_effort_command_interface())
+  {
+    // check command interface
+    EXPECT_LE(0.0, joint_eff_[0]);
+    EXPECT_LE(0.0, joint_eff_[1]);
+    EXPECT_LE(0.0, joint_eff_[2]);
+  }
+
+  executor.cancel();
+}
+
 void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count)
 {
   rclcpp::Parameter state_publish_rate_param(

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -112,6 +112,8 @@ class TestableJointTrajectoryController
 
   bool has_effort_command_interface() { return has_effort_command_interface_; }
 
+  bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; }
+
   rclcpp::WaitSet joint_cmd_sub_wait_set_;
 };
 
@@ -171,32 +173,35 @@ class TrajectoryControllerTest : public ::testing::Test
     node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params});
   }
 
-  void SetPidParameters()
+  void SetPidParameters(
+    double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false)
   {
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
 
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
       const std::string prefix = "gains." + joint_names_[i];
-      const rclcpp::Parameter k_p(prefix + ".p", 0.0);
+      const rclcpp::Parameter k_p(prefix + ".p", p_default);
       const rclcpp::Parameter k_i(prefix + ".i", 0.0);
       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", 1.0);
-      node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale});
+      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});
     }
   }
 
   void SetUpAndActivateTrajectoryController(
     rclcpp::Executor & executor, bool use_local_parameters = true,
     const std::vector<rclcpp::Parameter> & parameters = {},
-    bool separate_cmd_and_state_values = false)
+    bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
+    bool normalize_error = false)
   {
     SetUpTrajectoryController(executor, use_local_parameters);
 
     // set pid parameters before configure
-    SetPidParameters();
+    SetPidParameters(k_p, ff, normalize_error);
     for (const auto & param : parameters)
     {
       traj_controller_->get_node()->set_parameter(param);