diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7acbe20166..ba4bd2acaa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -317,6 +317,13 @@ class ControllerManager : public rclcpp::Node void initialize_parameters(); + /** + * Call shutdown to change the given controller lifecycle node to the finalized state. + * + * \param[in] controller controller to be shutdown. + */ + void shutdown_controller(controller_manager::ControllerSpec & controller) const; + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd59c89ea8..110627dec1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -28,7 +28,7 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" -#include "controller_manager_parameters.hpp" +#include "controller_manager/controller_manager_parameters.hpp" namespace // utility { @@ -58,6 +58,13 @@ static const rmw_qos_profile_t qos_services = { false}; #endif +inline bool is_controller_unconfigured( + const controller_interface::ControllerInterfaceBase & controller) +{ + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; +} + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_lifecycle_state().id() == @@ -695,39 +702,17 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::ERROR; } - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + RCLCPP_DEBUG(get_logger(), "Shutdown controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - if (is_controller_inactive(*controller.c)) + if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c)) { RCLCPP_DEBUG( - get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); + get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - try - { - const auto new_state = controller.c->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_WARN( - get_logger(), "Failed to clean-up the controller '%s' before unloading!", - controller_name.c_str()); - } - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_logger(), - "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s", - typeid(e).name(), controller_name.c_str(), e.what()); - } - catch (...) - { - RCLCPP_ERROR( - get_logger(), "Failed to clean-up the controller '%s' before unloading", - controller_name.c_str()); - } + shutdown_controller(controller); } executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -745,6 +730,33 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::OK; } +void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const +{ + try + { + const auto new_state = controller.c->get_node()->shutdown(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + RCLCPP_WARN( + get_logger(), "Failed to shutdown the controller '%s' before unloading!", + controller.info.name.c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), + "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s", + typeid(e).name(), controller.info.name.c_str(), e.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Failed to shutdown the controller '%s' before unloading", + controller.info.name.c_str()); + } +} + std::vector ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index ee8377f2a3..306799a66e 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -160,6 +160,15 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &) +{ + if (shutdown_calls) + { + (*shutdown_calls)++; + } + return CallbackReturn::SUCCESS; +} + void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 0728be877b..a28dfa420e 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -54,6 +54,8 @@ class TestController : public controller_interface::ControllerInterface CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + void set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg); @@ -66,9 +68,10 @@ class TestController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; - // Variable where we store when cleanup was called, pointer because the controller - // is usually destroyed after cleanup + // Variable where we store when shutdown was called, pointer because the controller + // is usually destroyed after shutdown size_t * cleanup_calls = nullptr; + size_t * shutdown_calls = nullptr; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 49c02d28ac..4e7065b094 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -228,7 +228,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } @@ -429,7 +429,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 26a5299a07..3a1ea5877b 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -395,14 +395,14 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + size_t shutdown_calls = 0; + test_controller->shutdown_calls = &shutdown_calls; test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(cleanup_calls, 1u); + ASSERT_EQ(shutdown_calls, 1u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); @@ -428,8 +428,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + shutdown_calls = 0; + test_controller->shutdown_calls = &shutdown_calls; test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -439,7 +439,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_EQ(test_controller_weak.use_count(), 0) << "No more references to the controller after reloading."; - ASSERT_EQ(cleanup_calls, 1u) + ASSERT_EQ(shutdown_calls, 1u) << "Controller should have been stopped and cleaned up with force_kill = true"; } @@ -491,7 +491,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -526,7 +526,7 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle unload_request->name = test_controller::TEST_CONTROLLER_NAME; auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); @@ -578,7 +578,7 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index d5695e41e5..efe903be6f 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -45,6 +45,12 @@ TestControllerWithInterfaces::on_cleanup(const rclcpp_lifecycle::State & /*previ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithInterfaces::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + } // namespace test_controller_with_interfaces #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index 751cb4f3a8..8227b62cb2 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -54,6 +54,9 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; }; } // namespace test_controller_with_interfaces diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index cbe42999ab..27883b8937 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -201,7 +201,7 @@ class JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ virtual bool enforce( - JointLimitsStateDataType & current_joint_states, + const JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) { joint_limits_ = *(updated_limits_.readFromRT()); @@ -234,7 +234,7 @@ class JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ virtual bool on_enforce( - JointLimitsStateDataType & current_joint_states, + const JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; /** \brief Checks if the logging interface is set. diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 8a17896139..b84c2662c8 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -49,7 +49,7 @@ bool is_limited(double value, double min, double max); */ PositionLimits compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, - const std::optional & prev_command_pos, double dt); + const std::optional & act_pos, const std::optional & prev_command_pos, double dt); /** * @brief Computes the velocity limits based on the position and acceleration limits. diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 9af574078d..3b7451e2ee 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -67,7 +67,7 @@ class JointSaturationLimiter : public JointLimiterInterface & act_vel, - const std::optional & prev_command_pos, double dt) + const std::optional & act_pos, const std::optional & prev_command_pos, double dt) { PositionLimits pos_limits(limits.min_position, limits.max_position); if (limits.has_velocity_limits) @@ -50,8 +50,10 @@ PositionLimits compute_position_limits( : limits.max_velocity; const double max_vel = std::min(limits.max_velocity, delta_vel); const double delta_pos = max_vel * dt; - pos_limits.lower_limit = std::max(prev_command_pos.value() - delta_pos, pos_limits.lower_limit); - pos_limits.upper_limit = std::min(prev_command_pos.value() + delta_pos, pos_limits.upper_limit); + const double position_reference = + act_pos.has_value() ? act_pos.value() : prev_command_pos.value(); + pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit); + pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit); } internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit); return pos_limits; diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 2279b910ae..c2ed78fcd9 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -43,7 +43,7 @@ bool JointSaturationLimiter::on_init() template <> bool JointSaturationLimiter::on_enforce( - JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { bool limits_enforced = false; @@ -112,8 +112,8 @@ bool JointSaturationLimiter::on_enforce( if (desired.has_position()) { - const auto limits = - compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds); + const auto limits = compute_position_limits( + joint_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds); limits_enforced = is_limited(desired.position.value(), limits.lower_limit, limits.upper_limit); desired.position = std::clamp(desired.position.value(), limits.lower_limit, limits.upper_limit); } diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 47a88e658d..e61cb429ec 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -27,7 +27,7 @@ namespace joint_limits { template <> bool JointSaturationLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { bool limits_enforced = false; @@ -44,23 +44,21 @@ bool JointSaturationLimiter::on_enfo // velocity max is implicitly already violated due to max_acc * dt > 2.0 // check for required inputs combination - bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); - bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); - bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); - bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); - bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); + const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + const bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + const bool has_desired_acceleration = + (desired_joint_states.accelerations.size() == number_of_joints_); + const bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + const bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); // pos state and vel or pos cmd is required, vel state is optional if (!(has_current_position && (has_desired_position || has_desired_velocity))) { return false; } - - if (!has_current_velocity) - { - // First update() after activating does not have velocity available, assume 0 - current_joint_states.velocities.resize(number_of_joints_, 0.0); - } + const std::vector & current_joint_velocities = + has_current_velocity ? current_joint_states.velocities + : std::vector(number_of_joints_, 0.0); // TODO(destogl): please check if we get too much malloc from this initialization, // if so then we should use members instead local variables and initialize them in other method @@ -140,8 +138,7 @@ bool JointSaturationLimiter::on_enfo current_joint_states.positions[index] + desired_vel[index] * dt_seconds; } - desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds; } } @@ -174,16 +171,15 @@ bool JointSaturationLimiter::on_enfo if ( std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) { - desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds; } // check if decelerating - if velocity is changing toward 0 bool deceleration_limit_applied = false; bool limit_applied = false; if ( - (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || - (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + (desired_acc[index] < 0 && current_joint_velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_velocities[index] < 0)) { // limit deceleration if (joint_limits_[index].has_deceleration_limits) @@ -204,13 +200,12 @@ bool JointSaturationLimiter::on_enfo if (limit_applied) { // vel_cmd from integration of desired_acc, needed even if no vel output - desired_vel[index] = - current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds; if (has_desired_position) { // pos_cmd from from double integration of desired_acc desired_pos[index] = current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + + current_joint_velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } } @@ -315,7 +310,7 @@ bool JointSaturationLimiter::on_enfo // Compute accel to stop // Here we aren't explicitly maximally decelerating, but for joints near their limits this // should still result in max decel being used - desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; + desired_acc[index] = -current_joint_velocities[index] / dt_seconds; if (joint_limits_[index].has_deceleration_limits) { desired_acc[index] = std::copysign( @@ -332,13 +327,12 @@ bool JointSaturationLimiter::on_enfo // Recompute velocity and position if (has_desired_velocity) { - desired_vel[index] = - current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds; } if (has_desired_position) { desired_pos[index] = current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + + current_joint_velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } } diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index a2292cb033..14eab8e41a 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -13,13 +13,14 @@ // limitations under the License. /// \author Adrià Roig Moreno +#define _USE_MATH_DEFINES #include "joint_limits/joint_soft_limiter.hpp" namespace joint_limits { bool JointSoftLimiter::on_enforce( - JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { bool limits_enforced = false; @@ -141,8 +142,8 @@ bool JointSoftLimiter::on_enforce( if (desired.has_position()) { - const auto position_limits = - compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + const auto position_limits = compute_position_limits( + hard_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds); double pos_low = -std::numeric_limits::infinity(); double pos_high = std::numeric_limits::infinity(); diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 083646d84f..41ab569289 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -142,6 +142,32 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) EXPECT_FALSE(desired_state_.has_effort()); EXPECT_FALSE(desired_state_.has_jerk()); + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 0.0; + desired_state_.position = 5.0 * M_PI; + const rclcpp::Duration test_period(0, 100); // 0.1 second + for (size_t i = 0; i < 2000; i++) + { + desired_state_.position = 5.0 * M_PI; + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(actual_state_.position.value()) + + ", desired position: " + std::to_string(desired_state_.position.value()) + + " for the joint limits : " + limits.to_string() + " Iteration : " + std::to_string(i)); + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, test_period)); + EXPECT_NEAR( + desired_state_.position.value(), + std::min( + actual_state_.position.value() + (limits.max_velocity * test_period.seconds()), M_PI), + COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + actual_state_.position = desired_state_.position.value() / 2.0; + } + // Now test when there are no position limits, then the desired position is not saturated limits = joint_limits::JointLimits(); ASSERT_TRUE(Init(limits)); @@ -652,19 +678,40 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting) test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + // Now enforce the limits with actual position and velocity ASSERT_TRUE(Init(limits)); // Desired position and velocity affected due to the acceleration limits test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true); test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); - test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 1.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); + test_limit_enforcing(5.0, 1.0, 6.0, 3.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); } int main(int argc, char ** argv) diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index 7deb2796ea..3659be4048 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -162,6 +162,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) soft_limits.max_position = 1.5; soft_limits.min_position = -1.5; ASSERT_TRUE(Init(limits, soft_limits)); + actual_state_.position = 0.0; desired_state_.position = 2.0; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); @@ -170,6 +171,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) soft_limits.min_position = -0.75; ASSERT_TRUE(Init(limits, soft_limits)); desired_state_.position = 2.0; + actual_state_ = {}; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); desired_state_.position = -3.0; @@ -286,6 +288,32 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) actual_state_.position = expected_pos; } + // More generic test case to mock a slow system + soft_limits.k_position = 0.5; + soft_limits.max_position = 2.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + actual_state_.position = -0.1; + for (auto i = 0; i < 10000; i++) + { + SCOPED_TRACE( + "Testing for iteration: " + std::to_string(i) + + " for desired position: " + std::to_string(desired_state_.position.value()) + + " and actual position : " + std::to_string(actual_state_.position.value()) + + " for the joint limits : " + limits.to_string()); + desired_state_.position = 4.0; + const double delta_pos = std::min( + (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position, + limits.max_velocity * period.seconds()); + const double expected_pos = actual_state_.position.value() + delta_pos; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD); + actual_state_.position = expected_pos / 1.27; + } + // Now test when there are no position limits and soft limits, then the desired position is not // saturated limits = joint_limits::JointLimits(); @@ -1021,9 +1049,13 @@ TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting) // Desired position and velocity affected due to the acceleration limits test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + // If the actual position doesn't change, the desired position should not change + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true); test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);