From 3e0fd9e10399f4f56f3d0ae57c94e084ef9579f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 29 Nov 2023 19:50:16 +0100 Subject: [PATCH] [JTC] Set `allow_nonzero_velocity_at_trajectory_end` default false and rename class variables (backport #834) (#843) --- .../doc/parameters.rst | 3 +-- .../joint_trajectory_controller.hpp | 4 ++-- .../src/joint_trajectory_controller.cpp | 14 +++++------ ...oint_trajectory_controller_parameters.yaml | 2 +- .../test/test_trajectory_actions.cpp | 6 ++++- .../test/test_trajectory_controller.cpp | 23 ++++++++----------- .../test/test_trajectory_controller_utils.hpp | 20 +++++++++------- 7 files changed, 38 insertions(+), 34 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 95a6599191..8ad2b406d6 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/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..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 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 64e4ccbc03..e371932726 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 @@ -167,8 +167,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 normalized - std::vector normalize_joint_error_; + // Configuration for every joint, if position error is wrapped around + 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/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e527a76307..cebd54a74a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/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 -pihas_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) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0dd6890a8b..6aa35d00b9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/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 params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector 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 params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector 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 params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector 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], diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 355b818ca6..956b2c0aa9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/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 & 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();