[JTC] Angle wraparound for first segment of trajectory (#796)

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -155,7 +155,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 wrapped around
+  // Configuration for every joint if it wraps around (ie. is continuous, position error is
+  // normalized)
   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/include/joint_trajectory_controller/trajectory.hpp

@@ -44,14 +44,19 @@ class Trajectory
     const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
     std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
 
-  /// Set the point before the trajectory message is replaced/appended
-  /// Example: if we receive a new trajectory message and it's first point is 0.5 seconds
-  /// from the current one, we call this function to log the current state, then
-  /// append/replace the current trajectory
+  /**
+   *  Set the point before the trajectory message is replaced/appended
+   * Example: if we receive a new trajectory message and it's first point is 0.5 seconds
+   * from the current one, we call this function to log the current state, then
+   * append/replace the current trajectory
+   * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that
+   * wrap around (ie. is continuous).
+   */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void set_point_before_trajectory_msg(
     const rclcpp::Time & current_time,
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_point);
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
+    const std::vector<bool> & joints_angle_wraparound = std::vector<bool>());
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
@@ -189,6 +194,17 @@ inline std::vector<size_t> mapping(const T & t1, const T & t2)
   return mapping_vector;
 }
 
+/**
+ * \param current_position The current position given from the controller, which will be adapted.
+ * \param next_position Next position from which to compute the wraparound offset, i.e.,
+ *      the first trajectory point
+ * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that
+ * wrap around (ie. is continuous).
+ */
+void wraparound_joint(
+  std::vector<double> & current_position, const std::vector<double> next_position,
+  const std::vector<bool> & joints_angle_wraparound);
+
 }  // namespace joint_trajectory_controller
 
 #endif  // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -153,11 +153,13 @@ controller_interface::return_type JointTrajectoryController::update(
       first_sample = true;
       if (params_.open_loop_control)
       {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(
+          time, last_commanded_state_, joints_angle_wraparound_);
       }
       else
       {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(
+          time, state_current_, joints_angle_wraparound_);
       }
     }
 

joint_trajectory_controller/src/trajectory.cpp

@@ -16,6 +16,7 @@
 
 #include <memory>
 
+#include "angles/angles.h"
 #include "hardware_interface/macros.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
@@ -44,10 +45,39 @@ Trajectory::Trajectory(
 
 void Trajectory::set_point_before_trajectory_msg(
   const rclcpp::Time & current_time,
-  const trajectory_msgs::msg::JointTrajectoryPoint & current_point)
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
+  const std::vector<bool> & joints_angle_wraparound)
 {
   time_before_traj_msg_ = current_time;
   state_before_traj_msg_ = current_point;
+
+  // Compute offsets due to wrapping joints
+  wraparound_joint(
+    state_before_traj_msg_.positions, trajectory_msg_->points[0].positions,
+    joints_angle_wraparound);
+}
+
+void wraparound_joint(
+  std::vector<double> & current_position, const std::vector<double> next_position,
+  const std::vector<bool> & joints_angle_wraparound)
+{
+  double dist;
+  // joints_angle_wraparound is even empty, or has the same size as the number of joints
+  for (size_t i = 0; i < joints_angle_wraparound.size(); i++)
+  {
+    if (joints_angle_wraparound[i])
+    {
+      dist = angles::shortest_angular_distance(current_position[i], next_position[i]);
+
+      // Deal with singularity at M_PI shortest distance
+      if (std::abs(std::abs(dist) - M_PI) < 1e-9)
+      {
+        dist = next_position[i] > current_position[i] ? std::abs(dist) : -std::abs(dist);
+      }
+
+      current_position[i] = next_position[i] - dist;
+    }
+  }
 }
 
 void Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)

joint_trajectory_controller/test/test_trajectory.cpp

@@ -821,3 +821,59 @@ TEST(TestTrajectory, skip_interpolation)
     }
   }
 }
+
+TEST(TestWrapAroundJoint, no_wraparound)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  std::vector<double> current_position(initial_position);
+  std::vector<bool> joints_angle_wraparound(3, false);
+  joint_trajectory_controller::wraparound_joint(
+    current_position, next_position, joints_angle_wraparound);
+  EXPECT_EQ(current_position[0], initial_position[0]);
+  EXPECT_EQ(current_position[1], initial_position[1]);
+  EXPECT_EQ(current_position[2], initial_position[2]);
+}
+
+TEST(TestWrapAroundJoint, wraparound_single_joint)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  std::vector<double> current_position(initial_position);
+  std::vector<bool> joints_angle_wraparound{true, false, false};
+  joint_trajectory_controller::wraparound_joint(
+    current_position, next_position, joints_angle_wraparound);
+  EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+  EXPECT_EQ(current_position[1], initial_position[1]);
+  EXPECT_EQ(current_position[2], initial_position[2]);
+}
+
+TEST(TestWrapAroundJoint, wraparound_all_joints)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  std::vector<double> current_position(initial_position);
+  std::vector<bool> joints_angle_wraparound(3, true);
+  joint_trajectory_controller::wraparound_joint(
+    current_position, next_position, joints_angle_wraparound);
+  EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+  EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI);
+  EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI);
+}
+
+TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  std::vector<double> current_position(next_position);
+  std::vector<bool> joints_angle_wraparound(3, true);
+  joint_trajectory_controller::wraparound_joint(
+    current_position, next_position, joints_angle_wraparound);
+  EXPECT_EQ(current_position[0], next_position[0]);
+  EXPECT_EQ(current_position[1], next_position[1]);
+  EXPECT_EQ(current_position[2], next_position[2]);
+}

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -636,14 +636,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal
 }
 
 /**
- * @brief check if position error of revolute joints are angle_wraparound if not configured so
+ * @brief check if position error of revolute joints are wrapped around if not configured so
  */
 TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   constexpr double k_p = 10.0;
   std::vector<rclcpp::Parameter> params = {};
-  SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
+  bool angle_wraparound = false;
+  SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
+  subscribeToState();
 
   size_t n_joints = joint_names_.size();
 
@@ -653,10 +655,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
   // *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);
+  publish(time_from_start, points, rclcpp::Time());
   traj_controller_->wait_for_trajectory(executor);
 
   updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
@@ -720,35 +720,24 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
 
   if (traj_controller_->has_effort_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
-    {
-      // we expect u = k_p * (s_d-s) for positions
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
-        k_p * COMMON_THRESHOLD);
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
-        k_p * COMMON_THRESHOLD);
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
-        k_p * COMMON_THRESHOLD);
-    }
-    else
-    {
-      // interpolated points_velocities only
-      // check command interface
-      EXPECT_LT(0.0, joint_eff_[0]);
-      EXPECT_LT(0.0, joint_eff_[1]);
-      EXPECT_LT(0.0, joint_eff_[2]);
-    }
+    // with effort command interface, use_closed_loop_pid_adapter is always true
+    // we expect u = k_p * (s_d-s) for positions
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
+      k_p * COMMON_THRESHOLD);
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
+      k_p * COMMON_THRESHOLD);
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
+      k_p * COMMON_THRESHOLD);
   }
 
   executor.cancel();
 }
 
 /**
- * @brief check if position error of revolute joints are angle_wraparound if configured so
+ * @brief check if position error of revolute joints are wrapped around if configured so
  */
 TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 {
@@ -791,7 +780,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
   EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD);
   EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD);
 
-  // is error.positions[2] angle_wraparound?
+  // is error.positions[2] wrapped around?
   EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
   EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
   EXPECT_NEAR(
@@ -810,15 +799,15 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
     // use_closed_loop_pid_adapter_
     if (traj_controller_->use_closed_loop_pid_adapter())
     {
-      // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
+      // we expect u = k_p * (s_d-s) for joint0 and joint1
       EXPECT_NEAR(
         k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
         k_p * COMMON_THRESHOLD);
       EXPECT_NEAR(
         k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
         k_p * COMMON_THRESHOLD);
-      // is error of positions[2] angle_wraparound?
-      EXPECT_GT(0.0, joint_vel_[2]);
+      // is error of positions[2] wrapped around?
+      EXPECT_GT(0.0, joint_vel_[2]);  // direction change because of angle wrap
       EXPECT_NEAR(
         k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2],
         k_p * COMMON_THRESHOLD);
@@ -835,30 +824,19 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 
   if (traj_controller_->has_effort_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
-    {
-      // we expect u = k_p * (s_d-s) for positions[0] and positions[1]
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
-        k_p * COMMON_THRESHOLD);
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
-        k_p * COMMON_THRESHOLD);
-      // is error of positions[2] angle_wraparound?
-      EXPECT_GT(0.0, joint_eff_[2]);
-      EXPECT_NEAR(
-        k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
-        k_p * COMMON_THRESHOLD);
-    }
-    else
-    {
-      // interpolated points_velocities only
-      // check command interface
-      EXPECT_LT(0.0, joint_eff_[0]);
-      EXPECT_LT(0.0, joint_eff_[1]);
-      EXPECT_LT(0.0, joint_eff_[2]);
-    }
+    // with effort command interface, use_closed_loop_pid_adapter is always true
+    // we expect u = k_p * (s_d-s) for joint0 and joint1
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
+      k_p * COMMON_THRESHOLD);
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
+      k_p * COMMON_THRESHOLD);
+    // is error of positions[2] wrapped around?
+    EXPECT_GT(0.0, joint_eff_[2]);
+    EXPECT_NEAR(
+      k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2],
+      k_p * COMMON_THRESHOLD);
   }
 
   executor.cancel();