diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 93ab6d7e1e..9b52673250 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -155,7 +155,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector 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 joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 3bd4873a31..b00d79481c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/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 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 & joints_angle_wraparound = std::vector()); JOINT_TRAJECTORY_CONTROLLER_PUBLIC void update(std::shared_ptr joint_trajectory); @@ -189,6 +194,17 @@ inline std::vector 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 & current_position, const std::vector next_position, + const std::vector & joints_angle_wraparound); + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7d66a2c3c1..a5b24e5292 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -202,11 +202,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_); } } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index fae4c41ff9..0ed7f2ff13 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -16,6 +16,7 @@ #include +#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 & 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 & current_position, const std::vector next_position, + const std::vector & 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 joint_trajectory) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b52aa67a04..6e0c53ac77 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -821,3 +821,59 @@ TEST(TestTrajectory, skip_interpolation) } } } + +TEST(TestWrapAroundJoint, no_wraparound) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector 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 initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector 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 initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector 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 initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(next_position); + std::vector 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]); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6d0057c8f8..ae6e0974e0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -481,14 +481,16 @@ 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 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 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(); @@ -498,10 +500,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> 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)); @@ -565,35 +565,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) { @@ -636,7 +625,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( @@ -655,15 +644,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); @@ -680,30 +669,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();