From 7b51739cb6474383ab1680f435213e1348dce111 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 12:59:40 +0000 Subject: [PATCH 1/7] Fix format warning --- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b5e660be54..9ddf57ac3b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -150,7 +150,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { From 840b04fd44753b08e6824c61ee03775a5007ae7b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 14:59:40 +0000 Subject: [PATCH 2/7] Convert lambdas to member functions --- .../joint_trajectory_controller.hpp | 36 +++++++++- .../src/joint_trajectory_controller.cpp | 68 ++++++++----------- 2 files changed, 63 insertions(+), 41 deletions(-) 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 d16e4f8267..9127c178f3 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 @@ -221,6 +221,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void goal_accepted_callback( std::shared_ptr> goal_handle); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + + /** + * Computes the error for a specific joint in the trajectory. + * + * @param[out] error The computed error for the joint. + * @param[in] index The index of the joint in the trajectory. + * @param[in] current The current state of the joints. + * @param[in] desired The desired state of the joints. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const; // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -263,7 +277,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_active_trajectory() const; - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, @@ -295,6 +308,27 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + + // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not + // changed, but its value only? + // TODO(anyone): Use auto in parameter declaration with c++20 + + /** + * @brief Assigns the values from a trajectory point interface to a joint interface. + * + * @tparam T The type of the joint interface. + * @param[out] joint_interface The reference_wrapper to assign the values to + * @param[in] trajectory_point_interface Containing the values to assign. + */ + template + void assign_interface_from_point( + T & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + } }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c6a5169855..b2cd6a5e97 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -128,35 +128,6 @@ controller_interface::return_type JointTrajectoryController::update( } } - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, size_t index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) - { - // error defined as the difference between current and desired - if (joints_angle_wraparound_[index]) - { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piupdate(*new_external_msg); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; - // current state update state_current_.time_from_start.set__sec(0); read_state_from_state_interfaces(state_current_); @@ -1186,6 +1146,34 @@ void JointTrajectoryController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } +void JointTrajectoryController::compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const +{ + // error defined as the difference between current and desired + if (joints_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi trajectory_msg) const { From 916cc1c748413096e0c0975171ef68fbe34c6ef5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 15:06:22 +0000 Subject: [PATCH 3/7] Make member function const --- .../joint_trajectory_controller/joint_trajectory_controller.hpp | 2 +- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 9127c178f3..e515922c1c 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 @@ -243,7 +243,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // sorts the joints of the incoming message to our local order JOINT_TRAJECTORY_CONTROLLER_PUBLIC void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); + std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b2cd6a5e97..75932fb141 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1235,7 +1235,7 @@ void JointTrajectoryController::fill_partial_goal( } void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) + std::shared_ptr trajectory_msg) const { // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); From 7f6ebb99094ea27b43479d6bcb8c89c941ad59c8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 17:16:47 +0000 Subject: [PATCH 4/7] Add a test for compute_error function --- .../test/test_trajectory_controller.cpp | 189 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 24 +++ 2 files changed, 213 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..7181a08a16 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -466,6 +466,195 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=true + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + + 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> 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}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + } + EXPECT_NEAR(error.positions[0], 0., EPS); + EXPECT_NEAR(error.positions[1], 0., EPS); + EXPECT_NEAR(error.positions[2], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + EXPECT_NEAR(error.velocities[0], 0., EPS); + EXPECT_NEAR(error.velocities[1], 0., EPS); + EXPECT_NEAR(error.velocities[2], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + EXPECT_NEAR(error.accelerations[0], 0., EPS); + EXPECT_NEAR(error.accelerations[1], 0., EPS); + EXPECT_NEAR(error.accelerations[2], 0., EPS); + } + + // angle wraparound of position error + desired.positions[0] += 3.0 * M_PI_2; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + } + EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0] - 2.0 * M_PI, EPS); + EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS); + EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); + EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); + EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); + EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); + EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); + } + + executor.cancel(); +} + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=false + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + + 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> 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}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + } + EXPECT_NEAR(error.positions[0], 0., EPS); + EXPECT_NEAR(error.positions[1], 0., EPS); + EXPECT_NEAR(error.positions[2], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + EXPECT_NEAR(error.velocities[0], 0., EPS); + EXPECT_NEAR(error.velocities[1], 0., EPS); + EXPECT_NEAR(error.velocities[2], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + EXPECT_NEAR(error.accelerations[0], 0., EPS); + EXPECT_NEAR(error.accelerations[1], 0., EPS); + EXPECT_NEAR(error.accelerations[2], 0., EPS); + } + + // no normalization of position error + desired.positions[0] += 3.0 * M_PI_4; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + } + EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0], EPS); + EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS); + EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); + EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); + EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); + EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); + EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are angle_wraparound if not configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7b68d882ff..332d72bf07 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -108,6 +108,13 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } + void testable_compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + compute_error_for_joint(error, index, current, desired); + } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; @@ -155,6 +162,23 @@ class TestableJointTrajectoryController trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + /** + * a copy of the private member function + */ + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + { + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From 6f67f65ea8ae64aa7c0b45bee1ab895409ccf67b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 17:37:25 +0000 Subject: [PATCH 5/7] Make reference_wrapper argument const --- .../joint_trajectory_controller.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) 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 e515922c1c..3cc4293219 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 @@ -309,20 +309,17 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - // TODO(anyone): Use auto in parameter declaration with c++20 - /** * @brief Assigns the values from a trajectory point interface to a joint interface. * * @tparam T The type of the joint interface. * @param[out] joint_interface The reference_wrapper to assign the values to * @param[in] trajectory_point_interface Containing the values to assign. + * @todo: Use auto in parameter declaration with c++20 */ template void assign_interface_from_point( - T & joint_interface, const std::vector & trajectory_point_interface) + const T & joint_interface, const std::vector & trajectory_point_interface) { for (size_t index = 0; index < dof_; ++index) { From 0ed1a944ba7f97f08b6e0f20cd3fb66b3d671d61 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 4 Jan 2024 10:25:06 +0000 Subject: [PATCH 6/7] Update confusing comments --- .../test/test_trajectory_controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8c0e3a0069..b7cc999d26 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -506,7 +506,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru (traj_controller_->has_velocity_command_interface() || traj_controller_->has_effort_command_interface())) { - // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + // expect: error.velocities = desired.velocities - current.velocities; EXPECT_NEAR(error.velocities[0], 0., EPS); EXPECT_NEAR(error.velocities[1], 0., EPS); EXPECT_NEAR(error.velocities[2], 0., EPS); @@ -515,7 +515,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru traj_controller_->has_acceleration_state_interface() && traj_controller_->has_acceleration_command_interface()) { - // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + // expect: error.accelerations = desired.accelerations - current.accelerations; EXPECT_NEAR(error.accelerations[0], 0., EPS); EXPECT_NEAR(error.accelerations[1], 0., EPS); EXPECT_NEAR(error.accelerations[2], 0., EPS); @@ -538,7 +538,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru (traj_controller_->has_velocity_command_interface() || traj_controller_->has_effort_command_interface())) { - // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + // expect: error.velocities = desired.velocities - current.velocities; EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); @@ -547,7 +547,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru traj_controller_->has_acceleration_state_interface() && traj_controller_->has_acceleration_command_interface()) { - // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + // expect: error.accelerations = desired.accelerations - current.accelerations; EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); @@ -600,7 +600,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal (traj_controller_->has_velocity_command_interface() || traj_controller_->has_effort_command_interface())) { - // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + // expect: error.velocities = desired.velocities - current.velocities; EXPECT_NEAR(error.velocities[0], 0., EPS); EXPECT_NEAR(error.velocities[1], 0., EPS); EXPECT_NEAR(error.velocities[2], 0., EPS); @@ -609,7 +609,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal traj_controller_->has_acceleration_state_interface() && traj_controller_->has_acceleration_command_interface()) { - // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + // expect: error.accelerations = desired.accelerations - current.accelerations; EXPECT_NEAR(error.accelerations[0], 0., EPS); EXPECT_NEAR(error.accelerations[1], 0., EPS); EXPECT_NEAR(error.accelerations[2], 0., EPS); @@ -632,7 +632,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal (traj_controller_->has_velocity_command_interface() || traj_controller_->has_effort_command_interface())) { - // error.velocities[index] = desired.velocities[index] - current.velocities[index]; + // expect: error.velocities = desired.velocities - current.velocities; EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); @@ -641,7 +641,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal traj_controller_->has_acceleration_state_interface() && traj_controller_->has_acceleration_command_interface()) { - // error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + // expect: error.accelerations = desired.accelerations - current.accelerations; EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); From 8076b99cccb7d14740616e91fe078909d03d056d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Jan 2024 21:35:51 +0000 Subject: [PATCH 7/7] Iterate over error fields instead --- .../test/test_trajectory_controller.cpp | 161 ++++++++---------- 1 file changed, 73 insertions(+), 88 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index b7cc999d26..dce04bf43a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -497,28 +497,22 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru for (size_t i = 0; i < n_joints; ++i) { traj_controller_->testable_compute_error_for_joint(error, i, current, desired); - } - EXPECT_NEAR(error.positions[0], 0., EPS); - EXPECT_NEAR(error.positions[1], 0., EPS); - EXPECT_NEAR(error.positions[2], 0., EPS); - if ( - traj_controller_->has_velocity_state_interface() && - (traj_controller_->has_velocity_command_interface() || - traj_controller_->has_effort_command_interface())) - { - // expect: error.velocities = desired.velocities - current.velocities; - EXPECT_NEAR(error.velocities[0], 0., EPS); - EXPECT_NEAR(error.velocities[1], 0., EPS); - EXPECT_NEAR(error.velocities[2], 0., EPS); - } - if ( - traj_controller_->has_acceleration_state_interface() && - traj_controller_->has_acceleration_command_interface()) - { - // expect: error.accelerations = desired.accelerations - current.accelerations; - EXPECT_NEAR(error.accelerations[0], 0., EPS); - EXPECT_NEAR(error.accelerations[1], 0., EPS); - EXPECT_NEAR(error.accelerations[2], 0., EPS); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } } // angle wraparound of position error @@ -529,28 +523,31 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru for (size_t i = 0; i < n_joints; ++i) { traj_controller_->testable_compute_error_for_joint(error, i, current, desired); - } - EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0] - 2.0 * M_PI, EPS); - EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS); - EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS); - if ( - traj_controller_->has_velocity_state_interface() && - (traj_controller_->has_velocity_command_interface() || - traj_controller_->has_effort_command_interface())) - { - // expect: error.velocities = desired.velocities - current.velocities; - EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); - EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); - EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); - } - if ( - traj_controller_->has_acceleration_state_interface() && - traj_controller_->has_acceleration_command_interface()) - { - // expect: error.accelerations = desired.accelerations - current.accelerations; - EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); - EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); - EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); + if (i == 0) + { + EXPECT_NEAR( + error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS); + } + else + { + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + } + + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } } executor.cancel(); @@ -591,28 +588,22 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal for (size_t i = 0; i < n_joints; ++i) { traj_controller_->testable_compute_error_for_joint(error, i, current, desired); - } - EXPECT_NEAR(error.positions[0], 0., EPS); - EXPECT_NEAR(error.positions[1], 0., EPS); - EXPECT_NEAR(error.positions[2], 0., EPS); - if ( - traj_controller_->has_velocity_state_interface() && - (traj_controller_->has_velocity_command_interface() || - traj_controller_->has_effort_command_interface())) - { - // expect: error.velocities = desired.velocities - current.velocities; - EXPECT_NEAR(error.velocities[0], 0., EPS); - EXPECT_NEAR(error.velocities[1], 0., EPS); - EXPECT_NEAR(error.velocities[2], 0., EPS); - } - if ( - traj_controller_->has_acceleration_state_interface() && - traj_controller_->has_acceleration_command_interface()) - { - // expect: error.accelerations = desired.accelerations - current.accelerations; - EXPECT_NEAR(error.accelerations[0], 0., EPS); - EXPECT_NEAR(error.accelerations[1], 0., EPS); - EXPECT_NEAR(error.accelerations[2], 0., EPS); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } } // no normalization of position error @@ -623,28 +614,22 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal for (size_t i = 0; i < n_joints; ++i) { traj_controller_->testable_compute_error_for_joint(error, i, current, desired); - } - EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0], EPS); - EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS); - EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS); - if ( - traj_controller_->has_velocity_state_interface() && - (traj_controller_->has_velocity_command_interface() || - traj_controller_->has_effort_command_interface())) - { - // expect: error.velocities = desired.velocities - current.velocities; - EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS); - EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS); - EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS); - } - if ( - traj_controller_->has_acceleration_state_interface() && - traj_controller_->has_acceleration_command_interface()) - { - // expect: error.accelerations = desired.accelerations - current.accelerations; - EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS); - EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS); - EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS); + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } } executor.cancel();