From 31315a1eaf978e54157199cd0af9060e57d0261b Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Tue, 15 Dec 2020 23:16:25 -0500 Subject: [PATCH 01/14] updated joint_limits_interface with new State/Command Interface instead of Handles. Watch out for those command handles, no longer support copying so had to move those constructor command args to std::move semantics --- joint_limits_interface/COLCON_IGNORE | 0 .../joint_limits_interface.hpp | 94 +++++++++---------- 2 files changed, 47 insertions(+), 47 deletions(-) delete mode 100644 joint_limits_interface/COLCON_IGNORE diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp index c2b850d04e..0bb042387d 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -17,8 +17,8 @@ #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#include #include +#include #include #include @@ -55,25 +55,25 @@ class JointLimitHandle {} JointLimitHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const JointLimits & limits) : jposh_(jposh), jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(jcmdh), + jcmdh_(std::move(jcmdh)), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) {} JointLimitHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, const JointLimits & limits) : jposh_(jposh), jvelh_(jvelh), - jcmdh_(jcmdh), + jcmdh_(std::move(jcmdh)), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) @@ -99,9 +99,9 @@ class JointLimitHandle } protected: - hardware_interface::JointHandle jposh_; - hardware_interface::JointHandle jvelh_; - hardware_interface::JointHandle jcmdh_; + hardware_interface::StateInterface jposh_; + hardware_interface::StateInterface jvelh_; + hardware_interface::CommandInterface jcmdh_; joint_limits_interface::JointLimits limits_; // stored state - track position and velocity of last update @@ -133,21 +133,21 @@ class JointSoftLimitsHandle : public JointLimitHandle JointSoftLimitsHandle() {} JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jcmdh, limits), + : JointLimitHandle(jposh, std::move(jcmdh), limits), soft_limits_(soft_limits) {} JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits), + : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits), soft_limits_(soft_limits) {} @@ -164,10 +164,10 @@ class PositionJointSaturationHandle : public JointLimitHandle PositionJointSaturationHandle() {} PositionJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const JointLimits & limits) - : JointLimitHandle(jposh, jcmdh, limits) + : JointLimitHandle(jposh, std::move(jcmdh), limits) { if (limits_.has_position_limits) { min_pos_limit_ = limits_.min_position; @@ -250,11 +250,11 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle {} PositionJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) + : JointSoftLimitsHandle(jposh, std::move(jcmdh), limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -337,11 +337,11 @@ class EffortJointSaturationHandle : public JointLimitHandle EffortJointSaturationHandle() {} EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits) + : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -356,11 +356,11 @@ class EffortJointSaturationHandle : public JointLimitHandle } EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits) : EffortJointSaturationHandle(jposh, - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) + hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) { } @@ -404,12 +404,12 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle EffortJointSoftLimitsHandle() {} EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) + : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -424,13 +424,13 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle } EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : EffortJointSoftLimitsHandle(jposh, hardware_interface::JointHandle( - hardware_interface::HW_IF_VELOCITY), jcmdh, limits, soft_limits) + hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits, soft_limits) { } @@ -496,11 +496,11 @@ class VelocityJointSaturationHandle : public JointLimitHandle VelocityJointSaturationHandle() {} VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jvelh, // currently unused - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jvelh, // currently unused + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits) : JointLimitHandle(hardware_interface::JointHandle( - hardware_interface::HW_IF_POSITION), jvelh, jcmdh, limits) + hardware_interface::HW_IF_POSITION), jvelh, std::move(jcmdh), limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -510,10 +510,10 @@ class VelocityJointSaturationHandle : public JointLimitHandle } VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jcmdh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits) : JointLimitHandle(hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) + hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -565,12 +565,12 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle VelocityJointSoftLimitsHandle() {} VelocityJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) + : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) { if (limits.has_velocity_limits) { max_vel_limit_ = limits.max_velocity; From abdcd01794758103a3b0b6b1443bd051fa6aec82 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Tue, 15 Dec 2020 23:30:39 -0500 Subject: [PATCH 02/14] updated tests with state and command interfaces --- .../test/joint_limits_interface_test.cpp | 250 +++++++++--------- 1 file changed, 131 insertions(+), 119 deletions(-) diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp index 7d14410c9d..b8eeffe680 100644 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ b/joint_limits_interface/test/joint_limits_interface_test.cpp @@ -63,11 +63,7 @@ class JointLimitsTest JointLimitsTest() : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), name("joint_name"), - period(0, 100000000), - cmd_handle(hardware_interface::JointHandle(name, "position_command", &cmd)), - pos_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_POSITION, &pos)), - vel_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_VELOCITY, &vel)), - eff_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_EFFORT, &eff)) + period(0, 100000000) { limits.has_position_limits = true; limits.min_position = -1.0; @@ -90,10 +86,24 @@ class JointLimitsTest double pos, vel, eff, cmd; std::string name; rclcpp::Duration period; - hardware_interface::JointHandle cmd_handle; - hardware_interface::JointHandle pos_handle, vel_handle, eff_handle; joint_limits_interface::JointLimits limits; joint_limits_interface::SoftJointLimits soft_limits; + + inline hardware_interface::CommandInterface command_handle() { + return hardware_interface::CommandInterface(name, "position_command", &cmd); + } + + inline hardware_interface::StateInterface position_handle() { + return hardware_interface::StateInterface(name, "position", &pos); + } + + inline hardware_interface::StateInterface velocity_handle() { + return hardware_interface::StateInterface(name, "velocity", &vel); + } + + inline hardware_interface::StateInterface effort_handle() { + return hardware_interface::StateInterface(name, "effort", &eff); + } }; class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; @@ -104,7 +114,7 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) joint_limits_interface::JointLimits limits_bad; EXPECT_THROW( joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); @@ -112,7 +122,7 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) // descriptive try { joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -123,14 +133,14 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) limits_bad.has_effort_limits = true; EXPECT_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, // but exception message should be descriptive try { joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -141,14 +151,14 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) limits_bad.has_velocity_limits = true; EXPECT_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, but exception message should // be descriptive try { joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -158,13 +168,13 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) joint_limits_interface::JointLimits limits_bad; EXPECT_THROW( joint_limits_interface::VelocityJointSaturationHandle( - pos_handle, cmd_handle, + position_handle(), command_handle(), limits_bad), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, but exception message should // be descriptive try { - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad); + joint_limits_interface::VelocityJointSaturationHandle(position_handle(), command_handle(), limits_bad); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -172,13 +182,13 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) EXPECT_NO_THROW( joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); + position_handle(), command_handle(), limits, soft_limits)); EXPECT_NO_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); + position_handle(), command_handle(), limits, soft_limits)); EXPECT_NO_THROW( joint_limits_interface::VelocityJointSaturationHandle( - pos_handle, cmd_handle, limits)); + position_handle(), command_handle(), limits)); } class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; @@ -194,55 +204,55 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) // Move slower than maximum velocity { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = max_increment / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -max_increment / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } // Move at maximum velocity { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } // Try to move faster than the maximum velocity, enforce velocity limits { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = 2.0 * max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, cmd_handle.get_value(), EPS); + EXPECT_NEAR(max_increment, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -2.0 * max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-max_increment, command_handle().get_value(), EPS); } } @@ -256,79 +266,79 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // Current position == upper soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (zero max velocity) pos = soft_limits.max_position; // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); } // Current position == lower soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (zero min velocity) pos = soft_limits.min_position; // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); + command_handle().set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); } // Current position > upper soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (negative max velocity) // Halfway between soft and hard limit pos = (soft_limits.max_position + limits.max_position) / 2.0; // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); } // Current position < lower soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (positive min velocity) // Halfway between soft and hard limit pos = (soft_limits.min_position + limits.min_position) / 2.0; // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); + command_handle().set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); } } @@ -343,29 +353,29 @@ TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) // Current position == higher hard limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Hit hard limit // On hard limit pos = limits.max_position; // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.max_position); + command_handle().set_value(2.0 * limits.max_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_position, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_position, command_handle().get_value(), EPS); } // Current position == lower hard limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Hit hard limit // On hard limit pos = limits.min_position; // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.min_position); + command_handle().set_value(2.0 * limits.min_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.min_position, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.min_position, command_handle().get_value(), EPS); } } @@ -375,43 +385,43 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) { // Test setup joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); + position_handle(), command_handle(), limits); pos = 0.0; double cmd; // Velocity within bounds cmd = limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); cmd = -limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); // Velocity at bounds cmd = limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); cmd = -limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); // Velocity beyond bounds cmd = 2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); cmd = -2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); } TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) @@ -420,7 +430,7 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) limits.has_acceleration_limits = true; limits.max_acceleration = limits.max_velocity / period.seconds(); joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); + position_handle(), command_handle(), limits); pos = 0.0; double cmd; @@ -429,57 +439,57 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) // Positive velocity // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); + command_handle().set_value(limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond +max velocity cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by velocity limit - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); + command_handle().set_value(limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond -max velocity cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by acceleration limit - EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity / 2.0, command_handle().get_value(), EPS); // Negative velocity // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); + command_handle().set_value(-limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond +max velocity cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by acceleration limit - EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity / 2.0, command_handle().get_value(), EPS); // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); + command_handle().set_value(-limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond -max velocity cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by velocity limit - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); } class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test @@ -488,35 +498,37 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test JointLimitsInterfaceTest() : JointLimitsTest(), pos2(0.0), vel2(0.0), eff2(0.0), cmd2(0.0), - name2("joint2_name"), - cmd2_handle(std::make_shared( - name2, "position_command", - &cmd2)), - pos2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_POSITION, &pos2)), - vel2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_VELOCITY, &vel2)), - eff2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_EFFORT, &eff2)) + name2("joint2_name") {} protected: double pos2, vel2, eff2, cmd2; std::string name2; - std::shared_ptr cmd2_handle; - std::shared_ptr pos2_handle, vel2_handle, eff2_handle; + + inline hardware_interface::CommandInterface command2_handle() { + return hardware_interface::CommandInterface(name2, "position_command", &cmd2); + } + + inline hardware_interface::StateInterface position2_handle() { + return hardware_interface::StateInterface(name2, "position", &pos2); + } + + inline hardware_interface::StateInterface velocity2_handle() { + return hardware_interface::StateInterface(name2, "velocity", &vel2); + } + + inline hardware_interface::StateInterface effort2_handle() { + return hardware_interface::StateInterface(name2, "effort", &eff2); + } }; // TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) // { // // Populate interface // joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( -// pos_handle, cmd_handle, limits, soft_limits); +// position_handle(), command_handle(), limits, soft_limits); // joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( -// pos_handle2, cmd_handle2, limits, soft_limits); +// position2_handle(), command2_handle(), limits, soft_limits); // // joint_limits_interface::PositionJointSoftLimitsInterface iface; // iface.registerHandle(limits_handle1); @@ -545,11 +557,11 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test // // Halfway between soft and hard limit // pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; // // Try to get closer to the hard limit -// cmd_handle.set_value(limits.max_position); -// cmd_handle2.set_cmd(limits.max_position); +// command_handle().set_value(limits.max_position); +// command2_handle().set_cmd(limits.max_position); // iface.enforce_limits(period); -// EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); -// EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); +// EXPECT_GT(position_handle().get_value(), command_handle().get_value()); +// EXPECT_GT(command2_handle().getPosition(), command2_handle().get_cmd()); // } // #if 0 // todo: implement the interfaces @@ -557,7 +569,7 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) { // Populate interface joint_limits_interface::PositionJointSaturationHandle limits_handle1 - (pos_handle, cmd_handle, limits); + (position_handle(), command_handle(), limits); PositionJointSaturationInterface iface; iface.registerHandle(limits_handle1); @@ -566,24 +578,24 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) const double max_increment = period.seconds() * limits.max_velocity; - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); + EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); iface.reset(); pos = limits.max_position; - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle.get_value(), limits.max_position, EPS); + EXPECT_NEAR(command_handle().get_value(), limits.max_position, EPS); } #endif // TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) // { // // Populate interface -// PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits); +// PositionJointSoftLimitsHandle limits_handle1(command_handle(), limits, soft_limits); // // PositionJointSoftLimitsInterface iface; // iface.registerHandle(limits_handle1); @@ -592,17 +604,17 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) // // const double max_increment = period.seconds() * limits.max_velocity; // -// cmd_handle.set_value(limits.max_position); +// command_handle().set_value(limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); +// EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); // // iface.reset(); // pos = limits.max_position; -// cmd_handle.set_value(soft_limits.max_position); +// command_handle().set_value(soft_limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle.get_value(), soft_limits.max_position, EPS); +// EXPECT_NEAR(command_handle().get_value(), soft_limits.max_position, EPS); // // } From b8b4d423f29b832c39be8b0756b6c937426f26f3 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Tue, 15 Dec 2020 23:42:07 -0500 Subject: [PATCH 03/14] cpplint and uncrustify fixes --- .../joint_limits_interface.hpp | 1 + .../test/joint_limits_interface_test.cpp | 33 ++++++++++++------- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp index 0bb042387d..b5c6fab54b 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include "joint_limits_interface/joint_limits.hpp" #include "joint_limits_interface/joint_limits_interface_exception.hpp" diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp index b8eeffe680..a5d9b7551c 100644 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ b/joint_limits_interface/test/joint_limits_interface_test.cpp @@ -88,20 +88,24 @@ class JointLimitsTest rclcpp::Duration period; joint_limits_interface::JointLimits limits; joint_limits_interface::SoftJointLimits soft_limits; - - inline hardware_interface::CommandInterface command_handle() { + + inline hardware_interface::CommandInterface command_handle() + { return hardware_interface::CommandInterface(name, "position_command", &cmd); } - inline hardware_interface::StateInterface position_handle() { + inline hardware_interface::StateInterface position_handle() + { return hardware_interface::StateInterface(name, "position", &pos); } - - inline hardware_interface::StateInterface velocity_handle() { + + inline hardware_interface::StateInterface velocity_handle() + { return hardware_interface::StateInterface(name, "velocity", &vel); } - - inline hardware_interface::StateInterface effort_handle() { + + inline hardware_interface::StateInterface effort_handle() + { return hardware_interface::StateInterface(name, "effort", &eff); } }; @@ -174,7 +178,8 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) // Print error messages. Requires manual output inspection, but exception message should // be descriptive try { - joint_limits_interface::VelocityJointSaturationHandle(position_handle(), command_handle(), limits_bad); + joint_limits_interface::VelocityJointSaturationHandle( + position_handle(), command_handle(), limits_bad); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -505,19 +510,23 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test double pos2, vel2, eff2, cmd2; std::string name2; - inline hardware_interface::CommandInterface command2_handle() { + inline hardware_interface::CommandInterface command2_handle() + { return hardware_interface::CommandInterface(name2, "position_command", &cmd2); } - inline hardware_interface::StateInterface position2_handle() { + inline hardware_interface::StateInterface position2_handle() + { return hardware_interface::StateInterface(name2, "position", &pos2); } - inline hardware_interface::StateInterface velocity2_handle() { + inline hardware_interface::StateInterface velocity2_handle() + { return hardware_interface::StateInterface(name2, "velocity", &vel2); } - inline hardware_interface::StateInterface effort2_handle() { + inline hardware_interface::StateInterface effort2_handle() + { return hardware_interface::StateInterface(name2, "effort", &eff2); } }; From 7223ea7dd680e20ed588d525ce8c63151180d299 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Fri, 18 Dec 2020 11:42:20 -0500 Subject: [PATCH 04/14] Moved joint_limits_interface into hardware_interface module Signed-off-by: Colin MacKenzie --- hardware_interface/CMakeLists.txt | 32 ++++++++++++ .../JointLimits-README.md | 0 .../joint_limits_interface/joint_limits.hpp | 0 .../joint_limits_interface.hpp | 8 +-- .../joint_limits_interface_exception.hpp | 0 .../joint_limits_rosparam.hpp | 0 .../joint_limits_urdf.hpp | 0 .../joint_limits.dox | 0 hardware_interface/package.xml | 1 + .../joint_limits_interface_test.cpp | 0 .../joint_limits_rosparam.launch.py | 0 .../joint_limits}/joint_limits_rosparam.yaml | 0 .../joint_limits_rosparam_test.cpp | 3 +- .../joint_limits}/joint_limits_urdf_test.cpp | 0 joint_limits_interface/CMakeLists.txt | 50 ------------------- joint_limits_interface/package.xml | 38 -------------- 16 files changed, 39 insertions(+), 93 deletions(-) rename joint_limits_interface/README.md => hardware_interface/JointLimits-README.md (100%) rename {joint_limits_interface => hardware_interface}/include/joint_limits_interface/joint_limits.hpp (100%) rename {joint_limits_interface => hardware_interface}/include/joint_limits_interface/joint_limits_interface.hpp (99%) rename {joint_limits_interface => hardware_interface}/include/joint_limits_interface/joint_limits_interface_exception.hpp (100%) rename {joint_limits_interface => hardware_interface}/include/joint_limits_interface/joint_limits_rosparam.hpp (100%) rename {joint_limits_interface => hardware_interface}/include/joint_limits_interface/joint_limits_urdf.hpp (100%) rename joint_limits_interface/mainpage.dox => hardware_interface/joint_limits.dox (100%) rename {joint_limits_interface/test => hardware_interface/test/joint_limits}/joint_limits_interface_test.cpp (100%) rename {joint_limits_interface/test => hardware_interface/test/joint_limits}/joint_limits_rosparam.launch.py (100%) rename {joint_limits_interface/test => hardware_interface/test/joint_limits}/joint_limits_rosparam.yaml (100%) rename {joint_limits_interface/test => hardware_interface/test/joint_limits}/joint_limits_rosparam_test.cpp (99%) rename {joint_limits_interface/test => hardware_interface/test/joint_limits}/joint_limits_urdf_test.cpp (100%) delete mode 100644 joint_limits_interface/CMakeLists.txt delete mode 100644 joint_limits_interface/package.xml diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 1c57c6e4f3..af4bd52133 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) +find_package(urdf REQUIRED) add_library( hardware_interface @@ -38,6 +39,7 @@ ament_target_dependencies( pluginlib rcutils rcpputils + urdf ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -89,11 +91,15 @@ install( ) if(BUILD_TESTING) + # todo: remove me! + find_package(rclcpp REQUIRED) + find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) ament_add_gmock(test_macros test/test_macros.cpp) target_include_directories(test_macros PRIVATE include) @@ -150,6 +156,31 @@ if(BUILD_TESTING) pluginlib ros2_control_test_assets ) + + # joint_limit_interface tests + ament_add_gmock(joint_limits_interface_test test/joint_limits/joint_limits_interface_test.cpp) + target_include_directories(joint_limits_interface_test PUBLIC include) + ament_target_dependencies(joint_limits_interface_test rclcpp) + + add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) + target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) + ament_target_dependencies(joint_limits_rosparam_test rclcpp) + add_launch_test(test/joint_limits/joint_limits_rosparam.launch.py) + install( + TARGETS + joint_limits_rosparam_test + DESTINATION lib/${PROJECT_NAME} + ) + install( + FILES + test/joint_limits/joint_limits_rosparam.yaml + DESTINATION share/${PROJECT_NAME}/test + ) + + ament_add_gmock(joint_limits_urdf_test test/joint_limits/joint_limits_urdf_test.cpp) + target_include_directories(joint_limits_urdf_test PUBLIC include) + ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) endif() ament_export_include_directories( @@ -165,5 +196,6 @@ ament_export_dependencies( rcpputils tinyxml2_vendor TinyXML2 + urdf ) ament_package() diff --git a/joint_limits_interface/README.md b/hardware_interface/JointLimits-README.md similarity index 100% rename from joint_limits_interface/README.md rename to hardware_interface/JointLimits-README.md diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp b/hardware_interface/include/joint_limits_interface/joint_limits.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp similarity index 99% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index b5c6fab54b..9425f4dd35 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -48,11 +48,11 @@ class JointLimitHandle { public: JointLimitHandle() - : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0), - jposh_(hardware_interface::HW_IF_POSITION), + : jposh_(hardware_interface::HW_IF_POSITION), jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command") + jcmdh_("position_command"), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) {} JointLimitHandle( diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_rosparam.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_rosparam.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_urdf.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_urdf.hpp diff --git a/joint_limits_interface/mainpage.dox b/hardware_interface/joint_limits.dox similarity index 100% rename from joint_limits_interface/mainpage.dox rename to hardware_interface/joint_limits.dox diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 35bdd8c1af..73b4fdf60e 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -13,6 +13,7 @@ pluginlib rcpputils tinyxml2_vendor + urdf rcutils rcutils diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp similarity index 100% rename from joint_limits_interface/test/joint_limits_interface_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_interface_test.cpp diff --git a/joint_limits_interface/test/joint_limits_rosparam.launch.py b/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam.launch.py rename to hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py diff --git a/joint_limits_interface/test/joint_limits_rosparam.yaml b/hardware_interface/test/joint_limits/joint_limits_rosparam.yaml similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam.yaml rename to hardware_interface/test/joint_limits/joint_limits_rosparam.yaml diff --git a/joint_limits_interface/test/joint_limits_rosparam_test.cpp b/hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp similarity index 99% rename from joint_limits_interface/test/joint_limits_rosparam_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp index ce2191f073..90195736d9 100644 --- a/joint_limits_interface/test/joint_limits_rosparam_test.cpp +++ b/hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp @@ -28,7 +28,8 @@ class JointLimitsRosParamTest : public ::testing::Test void SetUp() { rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true) + node_options + .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/hardware_interface/test/joint_limits/joint_limits_urdf_test.cpp similarity index 100% rename from joint_limits_interface/test/joint_limits_urdf_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_urdf_test.cpp diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt deleted file mode 100644 index 5af32d44b8..0000000000 --- a/joint_limits_interface/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) -project(joint_limits_interface) - -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -# Install headers -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rclcpp urdf) - -ament_package() diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml deleted file mode 100644 index 2170268c12..0000000000 --- a/joint_limits_interface/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - joint_limits_interface - 0.0.0 - Interface for enforcing joint limits. - - Bence Magyar - Jordan Palacios - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - Adolfo Rodriguez Tsouroukdissian - - ament_cmake - - rclcpp - urdf - - hardware_interface - - hardware_interface - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - hardware_interface - launch - launch_ros - launch_testing - launch_testing_ament_cmake - - - ament_cmake - - From 10efb4c168a266fc3cd2f077d543647a12965063 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Fri, 18 Dec 2020 12:56:42 -0500 Subject: [PATCH 05/14] fixed python test using old package name --- .../test/joint_limits/joint_limits_rosparam.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py b/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py index 68670574a3..ffbc311d66 100644 --- a/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py +++ b/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py @@ -27,10 +27,10 @@ def generate_test_description(): - joint_limits_interface_path = get_package_share_directory('joint_limits_interface') + joint_limits_interface_path = get_package_share_directory('hardware_interface') node_under_test = Node( - package='joint_limits_interface', + package='hardware_interface', executable='joint_limits_rosparam_test', output='screen', parameters=[os.path.join(joint_limits_interface_path, From 9b23de652507e2305042ead762627aa6d9960a1d Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Fri, 18 Dec 2020 23:50:33 -0500 Subject: [PATCH 06/14] removed todo msg --- hardware_interface/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index af4bd52133..93f469e280 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -91,7 +91,6 @@ install( ) if(BUILD_TESTING) - # todo: remove me! find_package(rclcpp REQUIRED) find_package(ament_lint_auto REQUIRED) From 097b247070ef5d9adf1566f207465fe6194ab554 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Sat, 19 Dec 2020 18:10:10 -0500 Subject: [PATCH 07/14] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Changes with interface names using constants. Use of quiet_NaN. Some find_package reordering. Co-authored-by: Denis Štogl --- hardware_interface/CMakeLists.txt | 399 +++-- .../joint_limits_interface.hpp | 1439 +++++++++-------- 2 files changed, 922 insertions(+), 916 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 93f469e280..97770f6d21 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -1,200 +1,199 @@ -cmake_minimum_required(VERSION 3.5) -project(hardware_interface) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - -find_package(ament_cmake REQUIRED) -find_package(control_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rcutils REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) -find_package(urdf REQUIRED) - -add_library( - hardware_interface - SHARED - src/actuator.cpp - src/component_parser.cpp - src/resource_manager.cpp - src/sensor.cpp - src/system.cpp -) -target_include_directories( - hardware_interface - PUBLIC - include -) -ament_target_dependencies( - hardware_interface - control_msgs - pluginlib - rcutils - rcpputils - urdf -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -# Fake components -add_library( - fake_components - SHARED - src/fake_components/generic_system.cpp -) -target_include_directories( - fake_components - PUBLIC - include -) -target_link_libraries( - fake_components - hardware_interface -) -ament_target_dependencies( - fake_components - pluginlib - rcpputils -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(fake_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file( - hardware_interface fake_components_plugin_description.xml) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - fake_components - hardware_interface - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -if(BUILD_TESTING) - find_package(rclcpp REQUIRED) - - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gmock REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_add_gmock(test_macros test/test_macros.cpp) - target_include_directories(test_macros PRIVATE include) - ament_target_dependencies(test_macros rcpputils) - - ament_add_gmock(test_joint_handle test/test_handle.cpp) - target_link_libraries(test_joint_handle hardware_interface) - ament_target_dependencies(test_joint_handle rcpputils) - - ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) - target_link_libraries(test_component_interfaces hardware_interface) - - ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser hardware_interface) - ament_target_dependencies(test_component_parser ros2_control_test_assets) - - add_library(test_components SHARED - test/test_components/test_actuator.cpp - test/test_components/test_sensor.cpp - test/test_components/test_system.cpp) - target_link_libraries(test_components hardware_interface) - ament_target_dependencies(test_components - pluginlib) - install(TARGETS test_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_components/test_components.xml) - - add_library(test_hardware_components SHARED - test/test_hardware_components/test_single_joint_actuator.cpp - test/test_hardware_components/test_force_torque_sensor.cpp - test/test_hardware_components/test_two_joint_system.cpp - test/test_hardware_components/test_system_with_command_modes.cpp - ) - target_link_libraries(test_hardware_components hardware_interface) - ament_target_dependencies(test_hardware_components - pluginlib) - install(TARGETS test_hardware_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_hardware_components/test_hardware_components.xml - ) - - ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) - target_link_libraries(test_resource_manager hardware_interface) - ament_target_dependencies(test_resource_manager ros2_control_test_assets) - - ament_add_gmock(test_generic_system test/fake_components/test_generic_system.cpp) - target_include_directories(test_generic_system PRIVATE include) - target_link_libraries(test_generic_system hardware_interface) - ament_target_dependencies(test_generic_system - pluginlib - ros2_control_test_assets - ) - - # joint_limit_interface tests - ament_add_gmock(joint_limits_interface_test test/joint_limits/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gmock(joint_limits_urdf_test test/joint_limits/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - fake_components - hardware_interface -) -ament_export_dependencies( - control_msgs - pluginlib - rcpputils - tinyxml2_vendor - TinyXML2 - urdf -) -ament_package() +cmake_minimum_required(VERSION 3.5) +project(hardware_interface) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) +find_package(urdf REQUIRED) + +add_library( + hardware_interface + SHARED + src/actuator.cpp + src/component_parser.cpp + src/resource_manager.cpp + src/sensor.cpp + src/system.cpp +) +target_include_directories( + hardware_interface + PUBLIC + include +) +ament_target_dependencies( + hardware_interface + control_msgs + pluginlib + rcutils + rcpputils + urdf +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +# Fake components +add_library( + fake_components + SHARED + src/fake_components/generic_system.cpp +) +target_include_directories( + fake_components + PUBLIC + include +) +target_link_libraries( + fake_components + hardware_interface +) +ament_target_dependencies( + fake_components + pluginlib + rcpputils +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(fake_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file( + hardware_interface fake_components_plugin_description.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + fake_components + hardware_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_macros test/test_macros.cpp) + target_include_directories(test_macros PRIVATE include) + ament_target_dependencies(test_macros rcpputils) + + ament_add_gmock(test_joint_handle test/test_handle.cpp) + target_link_libraries(test_joint_handle hardware_interface) + ament_target_dependencies(test_joint_handle rcpputils) + + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) + target_link_libraries(test_component_interfaces hardware_interface) + + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_link_libraries(test_component_parser hardware_interface) + ament_target_dependencies(test_component_parser ros2_control_test_assets) + + add_library(test_components SHARED + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp) + target_link_libraries(test_components hardware_interface) + ament_target_dependencies(test_components + pluginlib) + install(TARGETS test_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_components/test_components.xml) + + add_library(test_hardware_components SHARED + test/test_hardware_components/test_single_joint_actuator.cpp + test/test_hardware_components/test_force_torque_sensor.cpp + test/test_hardware_components/test_two_joint_system.cpp + ) + target_link_libraries(test_hardware_components hardware_interface) + ament_target_dependencies(test_hardware_components + pluginlib) + install(TARGETS test_hardware_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_components/test_hardware_components.xml + ) + + ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) + target_link_libraries(test_resource_manager hardware_interface) + ament_target_dependencies(test_resource_manager ros2_control_test_assets) + + ament_add_gmock(test_generic_system test/fake_components/test_generic_system.cpp) + target_include_directories(test_generic_system PRIVATE include) + target_link_libraries(test_generic_system hardware_interface) + ament_target_dependencies(test_generic_system + pluginlib + ros2_control_test_assets + ) + + # joint_limit_interface tests + ament_add_gmock(joint_limits_interface_test test/joint_limits/joint_limits_interface_test.cpp) + target_include_directories(joint_limits_interface_test PUBLIC include) + ament_target_dependencies(joint_limits_interface_test rclcpp) + + add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) + target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) + ament_target_dependencies(joint_limits_rosparam_test rclcpp) + add_launch_test(test/joint_limits/joint_limits_rosparam.launch.py) + install( + TARGETS + joint_limits_rosparam_test + DESTINATION lib/${PROJECT_NAME} + ) + install( + FILES + test/joint_limits/joint_limits_rosparam.yaml + DESTINATION share/${PROJECT_NAME}/test + ) + + ament_add_gmock(joint_limits_urdf_test test/joint_limits/joint_limits_urdf_test.cpp) + target_include_directories(joint_limits_urdf_test PUBLIC include) + ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + fake_components + hardware_interface +) +ament_export_dependencies( + control_msgs + pluginlib + rcpputils + tinyxml2_vendor + TinyXML2 + urdf +) +ament_package() diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index 9425f4dd35..550ad86c4b 100644 --- a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -1,716 +1,723 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - - -namespace joint_limits_interface -{ - -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: - JointLimitHandle() - : jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command"), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - JointLimitHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(std::move(jcmdh)), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - JointLimitHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(std::move(jcmdh)), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - /// \return Joint name. - std::string get_name() const - { - return jposh_ ? jposh_.get_name() : - jvelh_ ? jvelh_.get_name() : - jcmdh_ ? jcmdh_.get_name() : - std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::StateInterface jposh_; - hardware_interface::StateInterface jvelh_; - hardware_interface::CommandInterface jcmdh_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? - jvelh_.get_value() : - (jposh_.get_value() - prev_pos_) / period.seconds(); - } -}; - - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: - JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, std::move(jcmdh), limits), - soft_limits_(soft_limits) - {} - - JointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits), - soft_limits_(soft_limits) - {} - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: - PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const JointLimits & limits) - : JointLimitHandle(jposh, std::move(jcmdh), limits) - { - if (limits_.has_position_limits) { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } else { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - -/// Enforce position and velocity limits for a joint that is not subject to soft limits. -/** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) { - prev_pos_ = jposh_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } else { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = rcppmath::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - PositionJointSoftLimitsHandle() - {} - - PositionJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, std::move(jcmdh), limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) { - // Happens only once at initialization - prev_pos_ = jposh_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = rcppmath::clamp( - jcmdh_.get_value(), - pos_low, - pos_high); - jcmdh_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: - EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle(jposh, - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) { - const double pos = jposh_.get_value(); - if (pos < limits_.min_position) { - min_eff = 0.0; - } else if (pos > limits_.max_position) { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) { - min_eff = 0.0; - } else if (vel > limits_.max_velocity) { - max_eff = 0.0; - } - - double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle(jposh, - hardware_interface::JointHandle( - hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits, soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = jposh_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), - -limits_.max_effort, - limits_.max_effort); - - const double soft_max_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), - -limits_.max_effort, - limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = rcppmath::clamp( - jcmdh_.get_value(), - soft_min_eff, - soft_max_eff); - jcmdh_.set_value(eff_cmd); - } -}; - - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: - VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::StateInterface & jvelh, // currently unused - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::JointHandle( - hardware_interface::HW_IF_POSITION), jvelh, std::move(jcmdh), limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } else { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = rcppmath::clamp( - jcmdh_.get_value(), - vel_low, - vel_high); - jcmdh_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) - { - if (limits.has_velocity_limits) { - max_vel_limit_ = limits.max_velocity; - } else { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); - min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -max_vel_limit_, max_vel_limit_); - max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -max_vel_limit_, max_vel_limit_); - } else { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ +#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "joint_limits_interface/joint_limits.hpp" +#include "joint_limits_interface/joint_limits_interface_exception.hpp" + + +namespace joint_limits_interface +{ + +/** + * The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint. + */ +class JointLimitHandle +{ +public: + JointLimitHandle() + : jposh_(hardware_interface::HW_IF_POSITION), + jvelh_(hardware_interface::HW_IF_VELOCITY), + jcmdh_("position_command"), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(std::numeric_limits::quiet_NaN()) + {} + + JointLimitHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const JointLimits & limits) + : jposh_(jposh), + jvelh_(hardware_interface::HW_IF_VELOCITY), + jcmdh_(std::move(jcmdh)), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + JointLimitHandle( + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, + const JointLimits & limits) + : jposh_(jposh), + jvelh_(jvelh), + jcmdh_(std::move(jcmdh)), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + /// \return Joint name. + std::string get_name() const + { + return jposh_ ? jposh_.get_name() : + jvelh_ ? jvelh_.get_name() : + jcmdh_ ? jcmdh_.get_name() : + std::string(); + } + + /// Sub-class implementation of limit enforcing policy. + virtual void enforce_limits(const rclcpp::Duration & period) = 0; + + /// Clear stored state, causing it to reset next iteration. + virtual void reset() + { + prev_pos_ = std::numeric_limits::quiet_NaN(); + prev_vel_ = 0.0; + } + +protected: + hardware_interface::StateInterface jposh_; + hardware_interface::StateInterface jvelh_; + hardware_interface::CommandInterface jcmdh_; + joint_limits_interface::JointLimits limits_; + + // stored state - track position and velocity of last update + double prev_pos_; + double prev_vel_; + + /// Return velocity for limit calculations. + /** + * @param period Time since last measurement + * @return the velocity, from state if available, otherwise from previous position history. + */ + double get_velocity(const rclcpp::Duration & period) const + { + // if we have a handle to a velocity state we can directly return state velocity + // otherwise we will estimate velocity from previous position (command or state) + return jvelh_ ? + jvelh_.get_value() : + (jposh_.get_value() - prev_pos_) / period.seconds(); + } +}; + + +/** The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint that has soft-limits. + */ +class JointSoftLimitsHandle : public JointLimitHandle +{ +public: + JointSoftLimitsHandle() {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(jposh, std::move(jcmdh), limits), + soft_limits_(soft_limits) + {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits), + soft_limits_(soft_limits) + {} + +protected: + joint_limits_interface::SoftJointLimits soft_limits_; +}; + + +/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have + soft limits. */ +class PositionJointSaturationHandle : public JointLimitHandle +{ +public: + PositionJointSaturationHandle() {} + + PositionJointSaturationHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const JointLimits & limits) + : JointLimitHandle(jposh, std::move(jcmdh), limits) + { + if (limits_.has_position_limits) { + min_pos_limit_ = limits_.min_position; + max_pos_limit_ = limits_.max_position; + } else { + min_pos_limit_ = -std::numeric_limits::max(); + max_pos_limit_ = std::numeric_limits::max(); + } + } + +/// Enforce position and velocity limits for a joint that is not subject to soft limits. +/** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + if (std::isnan(prev_pos_)) { + prev_pos_ = jposh_.get_value(); + } + + double min_pos, max_pos; + if (limits_.has_velocity_limits) { + // enforce velocity limits + // set constraints on where the position can be based on the + // max velocity times seconds since last update + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); + max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); + } else { + // no velocity limit, so position is simply limited to set extents (our imposed soft limits) + min_pos = min_pos_limit_; + max_pos = max_pos_limit_; + } + + // clamp command position to our computed min/max position + const double cmd = rcppmath::clamp(jcmdh_.get_value(), min_pos, max_pos); + jcmdh_.set_value(cmd); + + prev_pos_ = cmd; + } + +private: + double min_pos_limit_, max_pos_limit_; +}; + +/// A handle used to enforce position and velocity limits of a position-controlled joint. +/** + * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least + * amount of requisites on the underlying hardware platform. + * This lowers considerably the entry barrier to use it, but also implies some limitations. + * + * Requisites + * - Position (for non-continuous joints) and velocity limits specification. + * - Soft limits specification. The \c k_velocity parameter is \e not used. + * + * Open loop nature + * + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for + * validity without relying on the actual position/velocity values. + * + * - Actual position values are \e not used because in some platforms there might be a substantial lag + * between sending a command and executing it (propagate command to hardware, reach control objective, + * read from hardware). + * + * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose + * trustworthy velocity measurements, or none at all. + * + * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large + * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command + * velocity. + */ + +// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? +class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: + PositionJointSoftLimitsHandle() + {} + + PositionJointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(jposh, std::move(jcmdh), limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce position and velocity limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity limits will be + * enforced. + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + assert(period.seconds() > 0.0); + + // Current position + if (std::isnan(prev_pos_)) { + // Happens only once at initialization + prev_pos_ = jposh_.get_value(); + } + const double pos = prev_pos_; + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Position bounds + const double dt = period.seconds(); + double pos_low = pos + soft_min_vel * dt; + double pos_high = pos + soft_max_vel * dt; + + if (limits_.has_position_limits) { + // This extra measure safeguards against pathological cases, like when the soft limit lies + // beyond the hard limit + pos_low = std::max(pos_low, limits_.min_position); + pos_high = std::min(pos_high, limits_.max_position); + } + + // Saturate position command according to bounds + const double pos_cmd = rcppmath::clamp( + jcmdh_.get_value(), + pos_low, + pos_high); + jcmdh_.set_value(pos_cmd); + + // Cache variables + // todo: shouldn't this just be pos_cmd? why call into the command handle to + // get what we have in the above line? + prev_pos_ = jcmdh_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and effort limits of an effort-controlled + * joint that does not have soft limits. + */ +class EffortJointSaturationHandle : public JointLimitHandle +{ +public: + EffortJointSaturationHandle() {} + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no efforts limits specification."); + } + } + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits) + : EffortJointSaturationHandle( + jposh, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + std::move(jcmdh), + limits) + { + } + + /** + * Enforce position, velocity, and effort limits for a joint that is not subject + * to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits) { + const double pos = jposh_.get_value(); + if (pos < limits_.min_position) { + min_eff = 0.0; + } else if (pos > limits_.max_position) { + max_eff = 0.0; + } + } + + const double vel = get_velocity(period); + if (vel < -limits_.max_velocity) { + min_eff = 0.0; + } else if (vel > limits_.max_velocity) { + max_eff = 0.0; + } + + double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); + jcmdh_.set_value(clamped); + } +}; + +/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. +// TODO(anyone): This class is untested!. Update unit tests accordingly. +class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: + EffortJointSoftLimitsHandle() {} + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no effort limits specification."); + } + } + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : EffortJointSoftLimitsHandle( + jposh, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + std::move(jcmdh), + limits, + soft_limits) + { + } + + /// Enforce position, velocity and effort limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits + * will be enforced. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Current state + const double pos = jposh_.get_value(); + const double vel = get_velocity(period); + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Effort bounds depend on the velocity and effort bounds + const double soft_min_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_min_vel), + -limits_.max_effort, + limits_.max_effort); + + const double soft_max_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_max_vel), + -limits_.max_effort, + limits_.max_effort); + + // Saturate effort command according to bounds + const double eff_cmd = rcppmath::clamp( + jcmdh_.get_value(), + soft_min_eff, + soft_max_eff); + jcmdh_.set_value(eff_cmd); + } +}; + + +/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. +class VelocityJointSaturationHandle : public JointLimitHandle +{ +public: + VelocityJointSaturationHandle() {} + + VelocityJointSaturationHandle( + const hardware_interface::StateInterface & jvelh, // currently unused + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(hardware_interface::JointHandle( + hardware_interface::HW_IF_POSITION), jvelh, std::move(jcmdh), limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + VelocityJointSaturationHandle( + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce joint velocity and acceleration limits. + /** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Velocity bounds + double vel_low; + double vel_high; + + if (limits_.has_acceleration_limits) { + assert(period.seconds() > 0.0); + const double dt = period.seconds(); + + vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); + } else { + vel_low = -limits_.max_velocity; + vel_high = limits_.max_velocity; + } + + // Saturate velocity command according to limits + const double vel_cmd = rcppmath::clamp( + jcmdh_.get_value(), + vel_low, + vel_high); + jcmdh_.set_value(vel_cmd); + + // Cache variables + prev_vel_ = jcmdh_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and acceleration limits of a + * velocity-controlled joint. + */ +class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: + VelocityJointSoftLimitsHandle() {} + + VelocityJointSoftLimitsHandle( + const hardware_interface::StateInterface & jposh, + const hardware_interface::StateInterface & jvelh, + hardware_interface::CommandInterface && jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) + { + if (limits.has_velocity_limits) { + max_vel_limit_ = limits.max_velocity; + } else { + max_vel_limit_ = std::numeric_limits::max(); + } + } + + /** + * Enforce position, velocity, and acceleration limits for a velocity-controlled joint + * subject to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + double min_vel, max_vel; + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit. + const double pos = jposh_.get_value(); + min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -max_vel_limit_, max_vel_limit_); + max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -max_vel_limit_, max_vel_limit_); + } else { + min_vel = -max_vel_limit_; + max_vel = max_vel_limit_; + } + + if (limits_.has_acceleration_limits) { + const double vel = get_velocity(period); + const double delta_t = period.seconds(); + min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); + max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); + } + + jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); + } + +private: + double max_vel_limit_; +}; + +// TODO(anyone): Port this to ROS 2 +// //** +// * Interface for enforcing joint limits. +// * +// * \tparam HandleType %Handle type. Must implement the following methods: +// * \code +// * void enforce_limits(); +// * std::string get_name() const; +// * \endcode +// */ +// template +// class joint_limits_interface::JointLimitsInterface +// : public hardware_interface::ResourceManager +// { +// public: +// HandleType getHandle(const std::string & name) +// { +// // Rethrow exception with a meaningful type +// try { +// return this->hardware_interface::ResourceManager::getHandle(name); +// } catch (const std::logic_error & e) { +// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); +// } +// } +// +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Enforce limits for all managed handles. */ +// void enforce_limits(const rclcpp::Duration & period) +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.enforce_limits(period); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint through saturation. */ +// class PositionJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ +// class PositionJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ +// class EffortJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ +// class EffortJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ +// class VelocityJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ +// class VelocityJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +} // namespace joint_limits_interface + +#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ From 61018d6be86b95e6994430d8071d44705e2b9aa3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 13 Mar 2021 18:55:03 +0100 Subject: [PATCH 08/14] Apply suggestions from code review --- .../joint_limits_interface.hpp | 16 +++++++++------- .../joint_limits/joint_limits_interface_test.cpp | 16 ++++++++-------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index 550ad86c4b..ec86486387 100644 --- a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -17,7 +17,6 @@ #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#include #include #include @@ -51,7 +50,7 @@ class JointLimitHandle JointLimitHandle() : jposh_(hardware_interface::HW_IF_POSITION), jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command"), + jcmdh_(hardware_interface::HW_IF_POSITION), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(std::numeric_limits::quiet_NaN()) {} @@ -69,9 +68,9 @@ class JointLimitHandle {} JointLimitHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_pos_interface, + const hardware_interface::StateInterface & joint_vel_interface, + hardware_interface::CommandInterface && joint_cmd_interface, const JointLimits & limits) : jposh_(jposh), jvelh_(jvelh), @@ -507,8 +506,11 @@ class VelocityJointSaturationHandle : public JointLimitHandle const hardware_interface::StateInterface & jvelh, // currently unused hardware_interface::CommandInterface && jcmdh, const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::JointHandle( - hardware_interface::HW_IF_POSITION), jvelh, std::move(jcmdh), limits) + : JointLimitHandle( + hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), + jvelh, + std::move(jcmdh), + limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( diff --git a/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp index a5d9b7551c..6263692ac4 100644 --- a/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp +++ b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp @@ -91,22 +91,22 @@ class JointLimitsTest inline hardware_interface::CommandInterface command_handle() { - return hardware_interface::CommandInterface(name, "position_command", &cmd); + return hardware_interface::CommandInterface(name, hardware_interface::HW_IF_POSITION, &cmd); } inline hardware_interface::StateInterface position_handle() { - return hardware_interface::StateInterface(name, "position", &pos); + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_POSITION, &pos); } inline hardware_interface::StateInterface velocity_handle() { - return hardware_interface::StateInterface(name, "velocity", &vel); + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_VELOCITY, &vel); } inline hardware_interface::StateInterface effort_handle() { - return hardware_interface::StateInterface(name, "effort", &eff); + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_EFFORT, &eff); } }; @@ -512,22 +512,22 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test inline hardware_interface::CommandInterface command2_handle() { - return hardware_interface::CommandInterface(name2, "position_command", &cmd2); + return hardware_interface::CommandInterface(name2, hardware_interface::HW_IF_POSITION, &cmd2); } inline hardware_interface::StateInterface position2_handle() { - return hardware_interface::StateInterface(name2, "position", &pos2); + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_POSITION, &pos2); } inline hardware_interface::StateInterface velocity2_handle() { - return hardware_interface::StateInterface(name2, "velocity", &vel2); + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_VELOCITY, &vel2); } inline hardware_interface::StateInterface effort2_handle() { - return hardware_interface::StateInterface(name2, "effort", &eff2); + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_EFFORT, &eff2); } }; From d882097998453a2c3e15fb00f2ba395650648162 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sun, 14 Mar 2021 09:09:32 +0100 Subject: [PATCH 09/14] Update names in joint_limit_interface --- .../joint_limits_interface.hpp | 171 +++++++++--------- 1 file changed, 90 insertions(+), 81 deletions(-) diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index ec86486387..d645fc2741 100644 --- a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -48,33 +48,33 @@ class JointLimitHandle { public: JointLimitHandle() - : jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(hardware_interface::HW_IF_POSITION), + : joint_position_interface_(hardware_interface::HW_IF_POSITION), + joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), + joint_command_interface_(hardware_interface::HW_IF_POSITION), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(std::numeric_limits::quiet_NaN()) {} JointLimitHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(std::move(jcmdh)), + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), + joint_command_interface_(std::move(joint_command_interface)), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) {} JointLimitHandle( - const hardware_interface::StateInterface & joint_pos_interface, - const hardware_interface::StateInterface & joint_vel_interface, - hardware_interface::CommandInterface && joint_cmd_interface, + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface && joint_command_interface, const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(std::move(jcmdh)), + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(joint_velocity_interface), + joint_command_interface_(std::move(joint_command_interface)), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) @@ -83,9 +83,9 @@ class JointLimitHandle /// \return Joint name. std::string get_name() const { - return jposh_ ? jposh_.get_name() : - jvelh_ ? jvelh_.get_name() : - jcmdh_ ? jcmdh_.get_name() : + return joint_position_interface_ ? joint_position_interface_.get_name() : + joint_velocity_interface_ ? joint_velocity_interface_.get_name() : + joint_command_interface_ ? joint_command_interface_.get_name() : std::string(); } @@ -100,9 +100,9 @@ class JointLimitHandle } protected: - hardware_interface::StateInterface jposh_; - hardware_interface::StateInterface jvelh_; - hardware_interface::CommandInterface jcmdh_; + hardware_interface::StateInterface joint_position_interface_; + hardware_interface::StateInterface joint_velocity_interface_; + hardware_interface::CommandInterface joint_command_interface_; joint_limits_interface::JointLimits limits_; // stored state - track position and velocity of last update @@ -118,9 +118,9 @@ class JointLimitHandle { // if we have a handle to a velocity state we can directly return state velocity // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? - jvelh_.get_value() : - (jposh_.get_value() - prev_pos_) / period.seconds(); + return joint_velocity_interface_ ? + joint_velocity_interface_.get_value() : + (joint_position_interface_.get_value() - prev_pos_) / period.seconds(); } }; @@ -134,21 +134,22 @@ class JointSoftLimitsHandle : public JointLimitHandle JointSoftLimitsHandle() {} JointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, std::move(jcmdh), limits), + : JointLimitHandle(joint_position_interface, std::move(joint_command_interface), limits), soft_limits_(soft_limits) {} JointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface && joint_command_interface, const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits), + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + std::move(joint_command_interface), limits), soft_limits_(soft_limits) {} @@ -165,10 +166,10 @@ class PositionJointSaturationHandle : public JointLimitHandle PositionJointSaturationHandle() {} PositionJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const JointLimits & limits) - : JointLimitHandle(jposh, std::move(jcmdh), limits) + : JointLimitHandle(joint_position_interface, std::move(joint_command_interface), limits) { if (limits_.has_position_limits) { min_pos_limit_ = limits_.min_position; @@ -186,7 +187,7 @@ class PositionJointSaturationHandle : public JointLimitHandle void enforce_limits(const rclcpp::Duration & period) { if (std::isnan(prev_pos_)) { - prev_pos_ = jposh_.get_value(); + prev_pos_ = joint_position_interface_.get_value(); } double min_pos, max_pos; @@ -204,8 +205,8 @@ class PositionJointSaturationHandle : public JointLimitHandle } // clamp command position to our computed min/max position - const double cmd = rcppmath::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); + const double cmd = rcppmath::clamp(joint_command_interface_.get_value(), min_pos, max_pos); + joint_command_interface_.set_value(cmd); prev_pos_ = cmd; } @@ -251,11 +252,12 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle {} PositionJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, std::move(jcmdh), limits, soft_limits) + : JointSoftLimitsHandle(joint_position_interface, std::move( + joint_command_interface), limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -277,7 +279,7 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle // Current position if (std::isnan(prev_pos_)) { // Happens only once at initialization - prev_pos_ = jposh_.get_value(); + prev_pos_ = joint_position_interface_.get_value(); } const double pos = prev_pos_; @@ -316,15 +318,15 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle // Saturate position command according to bounds const double pos_cmd = rcppmath::clamp( - jcmdh_.get_value(), + joint_command_interface_.get_value(), pos_low, pos_high); - jcmdh_.set_value(pos_cmd); + joint_command_interface_.set_value(pos_cmd); // Cache variables // todo: shouldn't this just be pos_cmd? why call into the command handle to // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); + prev_pos_ = joint_command_interface_.get_value(); } }; @@ -338,11 +340,12 @@ class EffortJointSaturationHandle : public JointLimitHandle EffortJointSaturationHandle() {} EffortJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, std::move(jcmdh), limits) + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + std::move(joint_command_interface), limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -357,13 +360,13 @@ class EffortJointSaturationHandle : public JointLimitHandle } EffortJointSaturationHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits) : EffortJointSaturationHandle( - jposh, + joint_position_interface, hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - std::move(jcmdh), + std::move(joint_command_interface), limits) { } @@ -380,7 +383,7 @@ class EffortJointSaturationHandle : public JointLimitHandle double max_eff = limits_.max_effort; if (limits_.has_position_limits) { - const double pos = jposh_.get_value(); + const double pos = joint_position_interface_.get_value(); if (pos < limits_.min_position) { min_eff = 0.0; } else if (pos > limits_.max_position) { @@ -395,8 +398,8 @@ class EffortJointSaturationHandle : public JointLimitHandle max_eff = 0.0; } - double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); + double clamped = rcppmath::clamp(joint_command_interface_.get_value(), min_eff, max_eff); + joint_command_interface_.set_value(clamped); } }; @@ -408,12 +411,13 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle EffortJointSoftLimitsHandle() {} EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + std::move(joint_command_interface), limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -428,14 +432,14 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle } EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : EffortJointSoftLimitsHandle( - jposh, + joint_position_interface, hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - std::move(jcmdh), + std::move(joint_command_interface), limits, soft_limits) { @@ -451,7 +455,7 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle void enforce_limits(const rclcpp::Duration & period) override { // Current state - const double pos = jposh_.get_value(); + const double pos = joint_position_interface_.get_value(); const double vel = get_velocity(period); // Velocity bounds @@ -488,10 +492,10 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle // Saturate effort command according to bounds const double eff_cmd = rcppmath::clamp( - jcmdh_.get_value(), + joint_command_interface_.get_value(), soft_min_eff, soft_max_eff); - jcmdh_.set_value(eff_cmd); + joint_command_interface_.set_value(eff_cmd); } }; @@ -503,13 +507,13 @@ class VelocityJointSaturationHandle : public JointLimitHandle VelocityJointSaturationHandle() {} VelocityJointSaturationHandle( - const hardware_interface::StateInterface & jvelh, // currently unused - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_velocity_interface, // currently unused + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits) : JointLimitHandle( - hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), - jvelh, - std::move(jcmdh), + hardware_interface::StateInterface("position"), + joint_velocity_interface, + std::move(joint_command_interface), limits) { if (!limits.has_velocity_limits) { @@ -520,10 +524,11 @@ class VelocityJointSaturationHandle : public JointLimitHandle } VelocityJointSaturationHandle( - hardware_interface::CommandInterface && jcmdh, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits) : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), - hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), std::move(jcmdh), limits) + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + std::move(joint_command_interface), limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -555,13 +560,13 @@ class VelocityJointSaturationHandle : public JointLimitHandle // Saturate velocity command according to limits const double vel_cmd = rcppmath::clamp( - jcmdh_.get_value(), + joint_command_interface_.get_value(), vel_low, vel_high); - jcmdh_.set_value(vel_cmd); + joint_command_interface_.set_value(vel_cmd); // Cache variables - prev_vel_ = jcmdh_.get_value(); + prev_vel_ = joint_command_interface_.get_value(); } }; @@ -575,12 +580,13 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle VelocityJointSoftLimitsHandle() {} VelocityJointSoftLimitsHandle( - const hardware_interface::StateInterface & jposh, - const hardware_interface::StateInterface & jvelh, - hardware_interface::CommandInterface && jcmdh, + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface && joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, std::move(jcmdh), limits, soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + std::move(joint_command_interface), limits, soft_limits) { if (limits.has_velocity_limits) { max_vel_limit_ = limits.max_velocity; @@ -600,7 +606,7 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle double min_vel, max_vel; if (limits_.has_position_limits) { // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); + const double pos = joint_position_interface_.get_value(); min_vel = rcppmath::clamp( -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, max_vel_limit_); @@ -619,7 +625,10 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); } - jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); + joint_command_interface_.set_value( + rcppmath::clamp( + joint_command_interface_.get_value(), + min_vel, max_vel)); } private: From ef87dfa3d7c51f7138dc358988f7b2678cf13917 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Mon, 15 Mar 2021 11:28:22 +0100 Subject: [PATCH 10/14] Remove copy of CommandInterface in JointLimitInterface --- .../joint_limits_interface.hpp | 86 +++++++++---------- hardware_interface/src/resource_manager.cpp | 2 +- .../joint_limits_interface_test.cpp | 9 +- 3 files changed, 50 insertions(+), 47 deletions(-) diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index d645fc2741..33f7d05284 100644 --- a/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -47,21 +47,22 @@ namespace joint_limits_interface class JointLimitHandle { public: - JointLimitHandle() - : joint_position_interface_(hardware_interface::HW_IF_POSITION), - joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), - joint_command_interface_(hardware_interface::HW_IF_POSITION), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(std::numeric_limits::quiet_NaN()) - {} +// JointLimitHandle() +// : joint_position_interface_(hardware_interface::HW_IF_POSITION), +// joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), +// joint_command_interface_(hardware_interface::CommandInterface( +// hardware_interface::HW_IF_POSITION)), +// prev_pos_(std::numeric_limits::quiet_NaN()), +// prev_vel_(std::numeric_limits::quiet_NaN()) +// {} JointLimitHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const JointLimits & limits) : joint_position_interface_(joint_position_interface), joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), - joint_command_interface_(std::move(joint_command_interface)), + joint_command_interface_(joint_command_interface), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) @@ -70,11 +71,11 @@ class JointLimitHandle JointLimitHandle( const hardware_interface::StateInterface & joint_position_interface, const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const JointLimits & limits) : joint_position_interface_(joint_position_interface), joint_velocity_interface_(joint_velocity_interface), - joint_command_interface_(std::move(joint_command_interface)), + joint_command_interface_(joint_command_interface), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), prev_vel_(0.0) @@ -102,7 +103,7 @@ class JointLimitHandle protected: hardware_interface::StateInterface joint_position_interface_; hardware_interface::StateInterface joint_velocity_interface_; - hardware_interface::CommandInterface joint_command_interface_; + hardware_interface::CommandInterface & joint_command_interface_; joint_limits_interface::JointLimits limits_; // stored state - track position and velocity of last update @@ -131,25 +132,25 @@ class JointLimitHandle class JointSoftLimitsHandle : public JointLimitHandle { public: - JointSoftLimitsHandle() {} +// JointSoftLimitsHandle() {} JointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(joint_position_interface, std::move(joint_command_interface), limits), + : JointLimitHandle(joint_position_interface, joint_command_interface, limits), soft_limits_(soft_limits) {} JointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const JointLimits & limits, const SoftJointLimits & soft_limits) : JointLimitHandle(joint_position_interface, joint_velocity_interface, - std::move(joint_command_interface), limits), + joint_command_interface, limits), soft_limits_(soft_limits) {} @@ -163,13 +164,13 @@ class JointSoftLimitsHandle : public JointLimitHandle class PositionJointSaturationHandle : public JointLimitHandle { public: - PositionJointSaturationHandle() {} +// PositionJointSaturationHandle() {} PositionJointSaturationHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const JointLimits & limits) - : JointLimitHandle(joint_position_interface, std::move(joint_command_interface), limits) + : JointLimitHandle(joint_position_interface, joint_command_interface, limits) { if (limits_.has_position_limits) { min_pos_limit_ = limits_.min_position; @@ -248,16 +249,15 @@ class PositionJointSaturationHandle : public JointLimitHandle class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle { public: - PositionJointSoftLimitsHandle() - {} +// PositionJointSoftLimitsHandle() +// {} PositionJointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(joint_position_interface, std::move( - joint_command_interface), limits, soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_command_interface, limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -337,15 +337,15 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle class EffortJointSaturationHandle : public JointLimitHandle { public: - EffortJointSaturationHandle() {} +// EffortJointSaturationHandle() {} EffortJointSaturationHandle( const hardware_interface::StateInterface & joint_position_interface, const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits) : JointLimitHandle(joint_position_interface, joint_velocity_interface, - std::move(joint_command_interface), limits) + joint_command_interface, limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -361,12 +361,12 @@ class EffortJointSaturationHandle : public JointLimitHandle EffortJointSaturationHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits) : EffortJointSaturationHandle( joint_position_interface, hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - std::move(joint_command_interface), + joint_command_interface, limits) { } @@ -408,16 +408,16 @@ class EffortJointSaturationHandle : public JointLimitHandle class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle { public: - EffortJointSoftLimitsHandle() {} +// EffortJointSoftLimitsHandle() {} EffortJointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, - std::move(joint_command_interface), limits, soft_limits) + joint_command_interface, limits, soft_limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -433,13 +433,13 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle EffortJointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : EffortJointSoftLimitsHandle( joint_position_interface, hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - std::move(joint_command_interface), + joint_command_interface, limits, soft_limits) { @@ -504,16 +504,16 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle class VelocityJointSaturationHandle : public JointLimitHandle { public: - VelocityJointSaturationHandle() {} +// VelocityJointSaturationHandle() {} VelocityJointSaturationHandle( const hardware_interface::StateInterface & joint_velocity_interface, // currently unused - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits) : JointLimitHandle( hardware_interface::StateInterface("position"), joint_velocity_interface, - std::move(joint_command_interface), + joint_command_interface, limits) { if (!limits.has_velocity_limits) { @@ -524,11 +524,11 @@ class VelocityJointSaturationHandle : public JointLimitHandle } VelocityJointSaturationHandle( - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits) : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - std::move(joint_command_interface), limits) + joint_command_interface, limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -577,16 +577,16 @@ class VelocityJointSaturationHandle : public JointLimitHandle class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle { public: - VelocityJointSoftLimitsHandle() {} +// VelocityJointSoftLimitsHandle() {} VelocityJointSoftLimitsHandle( const hardware_interface::StateInterface & joint_position_interface, const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface && joint_command_interface, + hardware_interface::CommandInterface & joint_command_interface, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, - std::move(joint_command_interface), limits, soft_limits) + joint_command_interface, limits, soft_limits) { if (limits.has_velocity_limits) { max_vel_limit_ = limits.max_velocity; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 3b208cc395..3af2f388a9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -231,12 +231,12 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin std::string("Command interface with '") + key + "' does not exist"); } - std::lock_guard lg(resource_lock_); if (command_interface_is_claimed(key)) { throw std::runtime_error( std::string("Command interface with '") + key + "' is already claimed"); } + std::lock_guard lg(resource_lock_); claimed_command_interface_map_[key] = true; return LoanedCommandInterface( resource_storage_->command_interface_map_.at(key), diff --git a/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp index 6263692ac4..295603621f 100644 --- a/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp +++ b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp @@ -63,7 +63,9 @@ class JointLimitsTest JointLimitsTest() : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), name("joint_name"), - period(0, 100000000) + period(0, 100000000), + position_command_handle( + name, hardware_interface::HW_IF_POSITION, &cmd) { limits.has_position_limits = true; limits.min_position = -1.0; @@ -88,10 +90,11 @@ class JointLimitsTest rclcpp::Duration period; joint_limits_interface::JointLimits limits; joint_limits_interface::SoftJointLimits soft_limits; + hardware_interface::CommandInterface position_command_handle; - inline hardware_interface::CommandInterface command_handle() + inline hardware_interface::CommandInterface & command_handle() { - return hardware_interface::CommandInterface(name, hardware_interface::HW_IF_POSITION, &cmd); + return position_command_handle; } inline hardware_interface::StateInterface position_handle() From e20df035e8a525edbc154efc9cb07f6f054aefb5 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Mon, 15 Mar 2021 11:32:44 +0100 Subject: [PATCH 11/14] Added files for ros2 development --- hardware_interface/CMakeLists.txt | 4 + .../joint_limits_interface_ros2.hpp | 734 ++++++++++++++++++ .../joint_limits_interface_test_ros2.cpp | 636 +++++++++++++++ 3 files changed, 1374 insertions(+) create mode 100644 hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp create mode 100644 hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 97770f6d21..a2070cbab2 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -160,6 +160,10 @@ if(BUILD_TESTING) target_include_directories(joint_limits_interface_test PUBLIC include) ament_target_dependencies(joint_limits_interface_test rclcpp) + ament_add_gmock(joint_limits_interface_test_ros2 test/joint_limits/joint_limits_interface_test_ros2.cpp) + target_include_directories(joint_limits_interface_test_ros2 PUBLIC include) + ament_target_dependencies(joint_limits_interface_test_ros2 rclcpp) + add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp new file mode 100644 index 0000000000..e6cf11e2f2 --- /dev/null +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp @@ -0,0 +1,734 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ +#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "joint_limits_interface/joint_limits.hpp" +#include "joint_limits_interface/joint_limits_interface_exception.hpp" + + +namespace joint_limits_interface +{ + +/** + * The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint. + */ +class JointLimitHandle +{ +public: +// JointLimitHandle() +// : joint_position_interface_(hardware_interface::HW_IF_POSITION), +// joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), +// joint_command_interface_(hardware_interface::CommandInterface( +// hardware_interface::HW_IF_POSITION)), +// prev_pos_(std::numeric_limits::quiet_NaN()), +// prev_vel_(std::numeric_limits::quiet_NaN()) +// {} + + JointLimitHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), + joint_command_interface_(joint_command_interface), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + JointLimitHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(joint_velocity_interface), + joint_command_interface_(joint_command_interface), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + /// \return Joint name. + std::string get_name() const + { + return joint_position_interface_ ? joint_position_interface_.get_name() : + joint_velocity_interface_ ? joint_velocity_interface_.get_name() : + joint_command_interface_ ? joint_command_interface_.get_name() : + std::string(); + } + + /// Sub-class implementation of limit enforcing policy. + virtual void enforce_limits(const rclcpp::Duration & period) = 0; + + /// Clear stored state, causing it to reset next iteration. + virtual void reset() + { + prev_pos_ = std::numeric_limits::quiet_NaN(); + prev_vel_ = 0.0; + } + +protected: + hardware_interface::StateInterface joint_position_interface_; + hardware_interface::StateInterface joint_velocity_interface_; + hardware_interface::CommandInterface & joint_command_interface_; + joint_limits_interface::JointLimits limits_; + + // stored state - track position and velocity of last update + double prev_pos_; + double prev_vel_; + + /// Return velocity for limit calculations. + /** + * @param period Time since last measurement + * @return the velocity, from state if available, otherwise from previous position history. + */ + double get_velocity(const rclcpp::Duration & period) const + { + // if we have a handle to a velocity state we can directly return state velocity + // otherwise we will estimate velocity from previous position (command or state) + return joint_velocity_interface_ ? + joint_velocity_interface_.get_value() : + (joint_position_interface_.get_value() - prev_pos_) / period.seconds(); + } +}; + + +/** The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint that has soft-limits. + */ +class JointSoftLimitsHandle : public JointLimitHandle +{ +public: +// JointSoftLimitsHandle() {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(joint_position_interface, joint_command_interface, limits), + soft_limits_(soft_limits) + {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits), + soft_limits_(soft_limits) + {} + +protected: + joint_limits_interface::SoftJointLimits soft_limits_; +}; + + +/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have + soft limits. */ +class PositionJointSaturationHandle : public JointLimitHandle +{ +public: +// PositionJointSaturationHandle() {} + + PositionJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : JointLimitHandle(joint_position_interface, joint_command_interface, limits) + { + if (limits_.has_position_limits) { + min_pos_limit_ = limits_.min_position; + max_pos_limit_ = limits_.max_position; + } else { + min_pos_limit_ = -std::numeric_limits::max(); + max_pos_limit_ = std::numeric_limits::max(); + } + } + +/// Enforce position and velocity limits for a joint that is not subject to soft limits. +/** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + if (std::isnan(prev_pos_)) { + prev_pos_ = joint_position_interface_.get_value(); + } + + double min_pos, max_pos; + if (limits_.has_velocity_limits) { + // enforce velocity limits + // set constraints on where the position can be based on the + // max velocity times seconds since last update + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); + max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); + } else { + // no velocity limit, so position is simply limited to set extents (our imposed soft limits) + min_pos = min_pos_limit_; + max_pos = max_pos_limit_; + } + + // clamp command position to our computed min/max position + const double cmd = rcppmath::clamp(joint_command_interface_.get_value(), min_pos, max_pos); + joint_command_interface_.set_value(cmd); + + prev_pos_ = cmd; + } + +private: + double min_pos_limit_, max_pos_limit_; +}; + +/// A handle used to enforce position and velocity limits of a position-controlled joint. +/** + * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least + * amount of requisites on the underlying hardware platform. + * This lowers considerably the entry barrier to use it, but also implies some limitations. + * + * Requisites + * - Position (for non-continuous joints) and velocity limits specification. + * - Soft limits specification. The \c k_velocity parameter is \e not used. + * + * Open loop nature + * + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for + * validity without relying on the actual position/velocity values. + * + * - Actual position values are \e not used because in some platforms there might be a substantial lag + * between sending a command and executing it (propagate command to hardware, reach control objective, + * read from hardware). + * + * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose + * trustworthy velocity measurements, or none at all. + * + * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large + * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command + * velocity. + */ + +// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? +class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// PositionJointSoftLimitsHandle() +// {} + + PositionJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_command_interface, limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce position and velocity limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity limits will be + * enforced. + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + assert(period.seconds() > 0.0); + + // Current position + if (std::isnan(prev_pos_)) { + // Happens only once at initialization + prev_pos_ = joint_position_interface_.get_value(); + } + const double pos = prev_pos_; + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Position bounds + const double dt = period.seconds(); + double pos_low = pos + soft_min_vel * dt; + double pos_high = pos + soft_max_vel * dt; + + if (limits_.has_position_limits) { + // This extra measure safeguards against pathological cases, like when the soft limit lies + // beyond the hard limit + pos_low = std::max(pos_low, limits_.min_position); + pos_high = std::min(pos_high, limits_.max_position); + } + + // Saturate position command according to bounds + const double pos_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + pos_low, + pos_high); + joint_command_interface_.set_value(pos_cmd); + + // Cache variables + // todo: shouldn't this just be pos_cmd? why call into the command handle to + // get what we have in the above line? + prev_pos_ = joint_command_interface_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and effort limits of an effort-controlled + * joint that does not have soft limits. + */ +class EffortJointSaturationHandle : public JointLimitHandle +{ +public: +// EffortJointSaturationHandle() {} + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no efforts limits specification."); + } + } + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : EffortJointSaturationHandle( + joint_position_interface, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, + limits) + { + } + + /** + * Enforce position, velocity, and effort limits for a joint that is not subject + * to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits) { + const double pos = joint_position_interface_.get_value(); + if (pos < limits_.min_position) { + min_eff = 0.0; + } else if (pos > limits_.max_position) { + max_eff = 0.0; + } + } + + const double vel = get_velocity(period); + if (vel < -limits_.max_velocity) { + min_eff = 0.0; + } else if (vel > limits_.max_velocity) { + max_eff = 0.0; + } + + double clamped = rcppmath::clamp(joint_command_interface_.get_value(), min_eff, max_eff); + joint_command_interface_.set_value(clamped); + } +}; + +/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. +// TODO(anyone): This class is untested!. Update unit tests accordingly. +class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// EffortJointSoftLimitsHandle() {} + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no effort limits specification."); + } + } + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : EffortJointSoftLimitsHandle( + joint_position_interface, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, + limits, + soft_limits) + { + } + + /// Enforce position, velocity and effort limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits + * will be enforced. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Current state + const double pos = joint_position_interface_.get_value(); + const double vel = get_velocity(period); + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Effort bounds depend on the velocity and effort bounds + const double soft_min_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_min_vel), + -limits_.max_effort, + limits_.max_effort); + + const double soft_max_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_max_vel), + -limits_.max_effort, + limits_.max_effort); + + // Saturate effort command according to bounds + const double eff_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + soft_min_eff, + soft_max_eff); + joint_command_interface_.set_value(eff_cmd); + } +}; + + +/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. +class VelocityJointSaturationHandle : public JointLimitHandle +{ +public: +// VelocityJointSaturationHandle() {} + + VelocityJointSaturationHandle( + const hardware_interface::StateInterface & joint_velocity_interface, // currently unused + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle( + hardware_interface::StateInterface("position"), + joint_velocity_interface, + joint_command_interface, + limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + VelocityJointSaturationHandle( + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce joint velocity and acceleration limits. + /** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Velocity bounds + double vel_low; + double vel_high; + + if (limits_.has_acceleration_limits) { + assert(period.seconds() > 0.0); + const double dt = period.seconds(); + + vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); + } else { + vel_low = -limits_.max_velocity; + vel_high = limits_.max_velocity; + } + + // Saturate velocity command according to limits + const double vel_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + vel_low, + vel_high); + joint_command_interface_.set_value(vel_cmd); + + // Cache variables + prev_vel_ = joint_command_interface_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and acceleration limits of a + * velocity-controlled joint. + */ +class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// VelocityJointSoftLimitsHandle() {} + + VelocityJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits, soft_limits) + { + if (limits.has_velocity_limits) { + max_vel_limit_ = limits.max_velocity; + } else { + max_vel_limit_ = std::numeric_limits::max(); + } + } + + /** + * Enforce position, velocity, and acceleration limits for a velocity-controlled joint + * subject to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + double min_vel, max_vel; + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit. + const double pos = joint_position_interface_.get_value(); + min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -max_vel_limit_, max_vel_limit_); + max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -max_vel_limit_, max_vel_limit_); + } else { + min_vel = -max_vel_limit_; + max_vel = max_vel_limit_; + } + + if (limits_.has_acceleration_limits) { + const double vel = get_velocity(period); + const double delta_t = period.seconds(); + min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); + max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); + } + + joint_command_interface_.set_value( + rcppmath::clamp( + joint_command_interface_.get_value(), + min_vel, max_vel)); + } + +private: + double max_vel_limit_; +}; + +// TODO(anyone): Port this to ROS 2 +// //** +// * Interface for enforcing joint limits. +// * +// * \tparam HandleType %Handle type. Must implement the following methods: +// * \code +// * void enforce_limits(); +// * std::string get_name() const; +// * \endcode +// */ +// template +// class joint_limits_interface::JointLimitsInterface +// : public hardware_interface::ResourceManager +// { +// public: +// HandleType getHandle(const std::string & name) +// { +// // Rethrow exception with a meaningful type +// try { +// return this->hardware_interface::ResourceManager::getHandle(name); +// } catch (const std::logic_error & e) { +// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); +// } +// } +// +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Enforce limits for all managed handles. */ +// void enforce_limits(const rclcpp::Duration & period) +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.enforce_limits(period); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint through saturation. */ +// class PositionJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ +// class PositionJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ +// class EffortJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ +// class EffortJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ +// class VelocityJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ +// class VelocityJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +} // namespace joint_limits_interface + +#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ diff --git a/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp new file mode 100644 index 0000000000..cea695fc91 --- /dev/null +++ b/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp @@ -0,0 +1,636 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include + +#include + +#include +#include + +#include + +#include +#include + + +// Floating-point value comparison threshold +const double EPS = 1e-12; + +TEST(SaturateTest, Saturate) +{ + const double min = -1.0; + const double max = 2.0; + double val; + + val = -0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = -1.0; + EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); + + val = -2.0; + EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); + + val = 2.0; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 3.0; + EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); +} + + +class JointLimitsTest +{ +public: + JointLimitsTest() + : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), + name("joint_name"), + period(0, 100000000), + position_command_handle( + name, hardware_interface::HW_IF_POSITION, &cmd) + { + limits.has_position_limits = true; + limits.min_position = -1.0; + limits.max_position = 1.0; + + limits.has_velocity_limits = true; + limits.max_velocity = 2.0; + + limits.has_effort_limits = true; + limits.max_effort = 8.0; + + soft_limits.min_position = -0.8; + soft_limits.max_position = 0.8; + soft_limits.k_position = 20.0; + // TODO(anyone): Tune value + soft_limits.k_velocity = 40.0; + } + +protected: + double pos, vel, eff, cmd; + std::string name; + rclcpp::Duration period; + joint_limits_interface::JointLimits limits; + joint_limits_interface::SoftJointLimits soft_limits; + hardware_interface::CommandInterface position_command_handle; + + inline hardware_interface::CommandInterface & command_handle() + { + return position_command_handle; + } + + inline hardware_interface::StateInterface position_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_POSITION, &pos); + } + + inline hardware_interface::StateInterface velocity_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_VELOCITY, &vel); + } + + inline hardware_interface::StateInterface effort_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_EFFORT, &eff); + } +}; + +class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(JointLimitsHandleTest, HandleConstruction) +{ + { + joint_limits_interface::JointLimits limits_bad; + EXPECT_THROW( + joint_limits_interface::PositionJointSoftLimitsHandle( + position_handle(), command_handle(), + limits_bad, soft_limits), + joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should be + // descriptive + try { + joint_limits_interface::PositionJointSoftLimitsHandle( + position_handle(), command_handle(), limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + limits_bad.has_effort_limits = true; + EXPECT_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + position_handle(), command_handle(), limits_bad, + soft_limits), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, + // but exception message should be descriptive + try { + joint_limits_interface::EffortJointSoftLimitsHandle( + position_handle(), command_handle(), limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + limits_bad.has_velocity_limits = true; + EXPECT_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + position_handle(), command_handle(), limits_bad, + soft_limits), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should + // be descriptive + try { + joint_limits_interface::EffortJointSoftLimitsHandle( + position_handle(), command_handle(), limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + EXPECT_THROW( + joint_limits_interface::VelocityJointSaturationHandle( + position_handle(), command_handle(), + limits_bad), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should + // be descriptive + try { + joint_limits_interface::VelocityJointSaturationHandle( + position_handle(), command_handle(), limits_bad); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + EXPECT_NO_THROW( + joint_limits_interface::PositionJointSoftLimitsHandle( + position_handle(), command_handle(), limits, soft_limits)); + EXPECT_NO_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + position_handle(), command_handle(), limits, soft_limits)); + EXPECT_NO_THROW( + joint_limits_interface::VelocityJointSaturationHandle( + position_handle(), command_handle(), limits)); +} + +class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) +{ + // Test setup + const double max_increment = period.seconds() * limits.max_velocity; + pos = 0.0; + + double cmd; + + // Move slower than maximum velocity + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = max_increment / 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = -max_increment / 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + } + + // Move at maximum velocity + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = max_increment; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = -max_increment; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + } + + // Try to move faster than the maximum velocity, enforce velocity limits + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = 2.0 * max_increment; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(max_increment, command_handle().get_value(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + cmd = -2.0 * max_increment; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(-max_increment, command_handle().get_value(), EPS); + } +} + +// This is a black box test and does not verify against random precomputed values, but rather that +// the expected qualitative behavior is honored +TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) +{ + // Test setup + const double workspace_center = (limits.min_position + limits.max_position) / 2.0; + + // Current position == upper soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Can't get any closer to hard limit (zero max velocity) + pos = soft_limits.max_position; + // Try to get closer to the hard limit + command_handle().set_value(limits.max_position); + limits_handle.enforce_limits(period); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); + + // OK to move away from hard limit + // Try to go to workspace center + command_handle().set_value(workspace_center); + limits_handle.enforce_limits(period); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); + } + + // Current position == lower soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Can't get any closer to hard limit (zero min velocity) + pos = soft_limits.min_position; + // Try to get closer to the hard limit + command_handle().set_value(limits.min_position); + limits_handle.enforce_limits(period); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); + + // OK to move away from hard limit + // Try to go to workspace center + command_handle().set_value(workspace_center); + limits_handle.enforce_limits(period); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); + } + + // Current position > upper soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Can't get any closer to hard limit (negative max velocity) + // Halfway between soft and hard limit + pos = (soft_limits.max_position + limits.max_position) / 2.0; + // Try to get closer to the hard limit + command_handle().set_value(limits.max_position); + limits_handle.enforce_limits(period); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); + + // OK to move away from hard limit + // Try to go to workspace center + command_handle().set_value(workspace_center); + limits_handle.enforce_limits(period); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); + } + + // Current position < lower soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Can't get any closer to hard limit (positive min velocity) + // Halfway between soft and hard limit + pos = (soft_limits.min_position + limits.min_position) / 2.0; + // Try to get closer to the hard limit + command_handle().set_value(limits.min_position); + limits_handle.enforce_limits(period); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); + + // OK to move away from hard limit + // Try to go to workspace center + command_handle().set_value(workspace_center); + limits_handle.enforce_limits(period); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); + } +} + +TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) +{ + // Safety limits are past the hard limits + soft_limits.min_position = limits.min_position * + (1.0 - 0.5 * limits.min_position / std::abs(limits.min_position)); + soft_limits.max_position = limits.max_position * + (1.0 + 0.5 * limits.max_position / std::abs(limits.max_position)); + + // Current position == higher hard limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Hit hard limit + // On hard limit + pos = limits.max_position; + // Way beyond hard limit + command_handle().set_value(2.0 * limits.max_position); + limits_handle.enforce_limits(period); + EXPECT_NEAR(limits.max_position, command_handle().get_value(), EPS); + } + + // Current position == lower hard limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + position_handle(), command_handle(), limits, soft_limits); + + // Hit hard limit + // On hard limit + pos = limits.min_position; + // Way beyond hard limit + command_handle().set_value(2.0 * limits.min_position); + limits_handle.enforce_limits(period); + EXPECT_NEAR(limits.min_position, command_handle().get_value(), EPS); + } +} + +class VelocityJointSaturationHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) +{ + // Test setup + joint_limits_interface::VelocityJointSaturationHandle limits_handle( + position_handle(), command_handle(), limits); + + pos = 0.0; + double cmd; + + // Velocity within bounds + cmd = limits.max_velocity / 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + + cmd = -limits.max_velocity / 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + + // Velocity at bounds + cmd = limits.max_velocity; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + + cmd = -limits.max_velocity; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); + + // Velocity beyond bounds + cmd = 2.0 * limits.max_velocity; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); + + cmd = -2.0 * limits.max_velocity; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); +} + +TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) +{ + // Test setup + limits.has_acceleration_limits = true; + limits.max_acceleration = limits.max_velocity / period.seconds(); + joint_limits_interface::VelocityJointSaturationHandle limits_handle( + position_handle(), command_handle(), limits); + + pos = 0.0; + double cmd; + // An arbitrarily long time, sufficient to suppress acceleration limits + const rclcpp::Duration long_enough(1000, 0); + + // Positive velocity + // register last command + command_handle().set_value(limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforce_limits(long_enough); + + // Try to go beyond +max velocity + cmd = limits.max_velocity * 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + // Max velocity bounded by velocity limit + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); + + // register last command + command_handle().set_value(limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforce_limits(long_enough); + + // Try to go beyond -max velocity + cmd = -limits.max_velocity * 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + // Max velocity bounded by acceleration limit + EXPECT_NEAR(-limits.max_velocity / 2.0, command_handle().get_value(), EPS); + + // Negative velocity + // register last command + command_handle().set_value(-limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforce_limits(long_enough); + + // Try to go beyond +max velocity + cmd = limits.max_velocity * 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + // Max velocity bounded by acceleration limit + EXPECT_NEAR(limits.max_velocity / 2.0, command_handle().get_value(), EPS); + + // register last command + command_handle().set_value(-limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforce_limits(long_enough); + + // Try to go beyond -max velocity + cmd = -limits.max_velocity * 2.0; + command_handle().set_value(cmd); + limits_handle.enforce_limits(period); + // Max velocity bounded by velocity limit + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); +} + +class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test +{ +public: + JointLimitsInterfaceTest() + : JointLimitsTest(), + pos2(0.0), vel2(0.0), eff2(0.0), cmd2(0.0), + name2("joint2_name") + {} + +protected: + double pos2, vel2, eff2, cmd2; + std::string name2; + + inline hardware_interface::CommandInterface command2_handle() + { + return hardware_interface::CommandInterface(name2, hardware_interface::HW_IF_POSITION, &cmd2); + } + + inline hardware_interface::StateInterface position2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_POSITION, &pos2); + } + + inline hardware_interface::StateInterface velocity2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_VELOCITY, &vel2); + } + + inline hardware_interface::StateInterface effort2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_EFFORT, &eff2); + } +}; + +// TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) +// { +// // Populate interface +// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( +// position_handle(), command_handle(), limits, soft_limits); +// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( +// position2_handle(), command2_handle(), limits, soft_limits); +// +// joint_limits_interface::PositionJointSoftLimitsInterface iface; +// iface.registerHandle(limits_handle1); +// iface.registerHandle(limits_handle2); +// +// // Get handles +// EXPECT_NO_THROW(iface.getHandle(name)); +// EXPECT_NO_THROW(iface.getHandle(name2)); +// +// PositionJointSoftLimitsHandle h1_tmp = iface.getHandle(name); +// EXPECT_EQ(name, h1_tmp.getName()); +// +// PositionJointSoftLimitsHandle h2_tmp = iface.getHandle(name2); +// EXPECT_EQ(name2, h2_tmp.getName()); +// +// // Print error message +// // Requires manual output inspection, but exception message should contain the interface name +// // (not its base class) +// try { +// iface.getHandle("unknown_name"); +// } catch (const JointLimitsInterfaceException & e) { +// ROS_ERROR_STREAM(e.what()); +// } +// +// // Enforce limits of all managed joints +// // Halfway between soft and hard limit +// pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; +// // Try to get closer to the hard limit +// command_handle().set_value(limits.max_position); +// command2_handle().set_cmd(limits.max_position); +// iface.enforce_limits(period); +// EXPECT_GT(position_handle().get_value(), command_handle().get_value()); +// EXPECT_GT(command2_handle().getPosition(), command2_handle().get_cmd()); +// } +// +#if 0 // todo: implement the interfaces +TEST_F(JointLimitsHandleTest, ResetSaturationInterface) +{ + // Populate interface + joint_limits_interface::PositionJointSaturationHandle limits_handle1 + (position_handle(), command_handle(), limits); + + PositionJointSaturationInterface iface; + iface.registerHandle(limits_handle1); + + iface.enforce_limits(period); // initialize limit handles + + const double max_increment = period.seconds() * limits.max_velocity; + + command_handle().set_value(limits.max_position); + iface.enforce_limits(period); + + EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); + + iface.reset(); + pos = limits.max_position; + command_handle().set_value(limits.max_position); + iface.enforce_limits(period); + + EXPECT_NEAR(command_handle().get_value(), limits.max_position, EPS); +} +#endif + +// TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) +// { +// // Populate interface +// PositionJointSoftLimitsHandle limits_handle1(command_handle(), limits, soft_limits); +// +// PositionJointSoftLimitsInterface iface; +// iface.registerHandle(limits_handle1); +// +// iface.enforce_limits(period); // initialize limit handles +// +// const double max_increment = period.seconds() * limits.max_velocity; +// +// command_handle().set_value(limits.max_position); +// iface.enforce_limits(period); +// +// EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); +// +// iface.reset(); +// pos = limits.max_position; +// command_handle().set_value(soft_limits.max_position); +// iface.enforce_limits(period); +// +// EXPECT_NEAR(command_handle().get_value(), soft_limits.max_position, EPS); +// +// } + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From de0b556503d094ce86fb5ed310b5f3230a878755 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 23 Mar 2021 16:32:59 +0100 Subject: [PATCH 12/14] Added joint_limits package with limiters for ResourceManager --- hardware_interface/CMakeLists.txt | 4 - .../joint_limits/joint_limiter_interface.hpp | 168 ++++ .../include/joint_limits/joint_limits.hpp | 67 ++ .../joint_limits/joint_limits_rosparam.hpp | 243 ++++++ .../joint_limits/joint_limits_urdf.hpp | 84 ++ .../joint_limits/simple_joint_limiter.hpp | 49 ++ .../joint_limits_interface_ros2.hpp | 734 ------------------ .../src/joint_limits/simple_joint_limiter.cpp | 159 ++++ .../joint_limits_interface_test_ros2.cpp | 636 --------------- 9 files changed, 770 insertions(+), 1374 deletions(-) create mode 100644 hardware_interface/include/joint_limits/joint_limiter_interface.hpp create mode 100644 hardware_interface/include/joint_limits/joint_limits.hpp create mode 100644 hardware_interface/include/joint_limits/joint_limits_rosparam.hpp create mode 100644 hardware_interface/include/joint_limits/joint_limits_urdf.hpp create mode 100644 hardware_interface/include/joint_limits/simple_joint_limiter.hpp delete mode 100644 hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp create mode 100644 hardware_interface/src/joint_limits/simple_joint_limiter.cpp delete mode 100644 hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index a2070cbab2..97770f6d21 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -160,10 +160,6 @@ if(BUILD_TESTING) target_include_directories(joint_limits_interface_test PUBLIC include) ament_target_dependencies(joint_limits_interface_test rclcpp) - ament_add_gmock(joint_limits_interface_test_ros2 test/joint_limits/joint_limits_interface_test_ros2.cpp) - target_include_directories(joint_limits_interface_test_ros2 PUBLIC include) - ament_target_dependencies(joint_limits_interface_test_ros2 rclcpp) - add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) diff --git a/hardware_interface/include/joint_limits/joint_limiter_interface.hpp b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..3c1ab5729d --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,168 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +// TODO(all): there is possibility to use something like LoanedCommandInterface, still having copy-constructor on this level makes thing way easier and more readable +// class LimiterCommandHandle + +template +class JointLimiterInterface +{ +public: + JointLimiterInterface( + LimitsType & limits, + std::vector & state_interfaces, + // Here could be used "LimiterCommandHandle" with copy-constructor to avid raw-pointer access/storage + std::vector & command_interfaces) + : limits_(limits), + has_effort_command_(false), + has_velocity_state_(false), + previous_position_(std::numeric_limits::quiet_NaN()), + previous_velocity_(0.0) + { + // At least one command interface has to be provided + if (command_interfaces.size() == 0) { + throw std::runtime_error("At least one command interface has to be provided."); + } + + // Check if position-velocity-acceleration or effort commands are used + if ( + std::find_if( + command_interfaces.begin(), command_interfaces.end(), + [](const hardware_interface::CommandInterface * interface) { + return interface->get_name() == hardware_interface::HW_IF_EFFORT; + }) != command_interfaces.end()) { + has_effort_command_ = true; + // Effort command interface has to be used as only interface + if (command_interfaces.size() > 1) { + // TODO(all): Do we need here specific exception type? + throw std::runtime_error("Effort command interface has to be used alone."); + } + } + + for (const auto & interface_name : interface_order_) { + for (const auto & interface : command_interfaces) { + if (interface_name == interface->get_name()) { + command_interfaces_.emplace_back(interface); + } else { + // Do not create 'effort' virtual interfaces if effort command interface is used + if (!has_effort_command_ || interface_name != hardware_interface::HW_IF_EFFORT) { + virtual_command_storage_.push_back(0.0); + auto i = virtual_command_storage_.size() - 1; // last element + virtual_command_interfaces_.emplace_back(hardware_interface::CommandInterface( + "joint_limiter_virtual", interface_name, &virtual_command_storage_[i])); + command_interfaces_.emplace_back(&virtual_command_interfaces_.back()); + } + } + } + for (const auto & interface : state_interfaces) { + if (interface_name == interface.get_name()) { + state_interfaces_.emplace_back(interface); + } else { + virtual_state_storage_.push_back(0.0); + auto i = virtual_state_storage_.size() - 1; // last element + state_interfaces_.emplace_back(hardware_interface::StateInterface( + "joint_limiter_virtual", interface_name, &virtual_state_storage_[i])); + } + } + } + } + + ~JointLimiterInterface() = default; + + /// Implementation of limit enforcing policy. + virtual void enforce_limits(const rclcpp::Duration & period) + { + if (std::isnan(previous_position_)) { + previous_position_ = state_interfaces_[0].get_value(); + } + + if (has_effort_command_) { + enforce_effort_limits(period); + } else { + enforce_pos_vel_acc_limits(period); + } + } + + std::string get_name() const + { + // Hopefully some interface exists. Simply go through all of them and return first name + for (const auto & interface : state_interfaces_) { + return interface.get_name(); + } + for (const auto & interface : command_interfaces_) { + return interface->get_name(); + } + return std::string(); + } + + /// Clear stored state, causing to reset next iteration. + virtual void reset() + { + previous_position_ = std::numeric_limits::quiet_NaN(); + previous_velocity_ = 0.0; + } + +protected: + /// Limit enforcing policy for effort command interface. + virtual void enforce_effort_limits(const rclcpp::Duration & period) = 0; + + /// Limit enforcing policy for position, velocity and acceleration command interfaces. + virtual void enforce_pos_vel_acc_limits(const rclcpp::Duration & period) = 0; + + LimitsType & limits_; + std::vector command_interfaces_; + std::vector state_interfaces_; + std::vector virtual_command_interfaces_; + std::vector virtual_state_interfaces_; + std::vector virtual_command_storage_; + std::vector virtual_state_storage_; + + bool has_effort_command_; + bool has_velocity_state_; + + // stored states + double previous_position_; + double previous_velocity_; + + // min and max values + double min_position_limit_, max_position_limit_; + double max_velocity_limit_; + double max_acceleration_limit_; + + std::vector interface_order_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits.hpp b/hardware_interface/include/joint_limits/joint_limits.hpp new file mode 100644 index 0000000000..c53d0ad2e7 --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HPP_ + +namespace joint_limits +{ +struct JointLimits +{ + JointLimits() + : min_position(0.0), + max_position(0.0), + max_velocity(0.0), + max_acceleration(0.0), + max_jerk(0.0), + max_effort(0.0), + has_position_limits(false), + has_velocity_limits(false), + has_acceleration_limits(false), + has_jerk_limits(false), + has_effort_limits(false), + angle_wraparound(false) + { + } + + double min_position; + double max_position; + double max_velocity; + double max_acceleration; + double max_jerk; + double max_effort; + + bool has_position_limits; + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + bool has_effort_limits; + bool angle_wraparound; +}; + +struct SoftJointLimits +{ + SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} + + double min_position; + double max_position; + double k_position; + double k_velocity; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp new file mode 100644 index 0000000000..96ee386b4e --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp @@ -0,0 +1,243 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ + +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +/// Populate a JointLimits instance from the ROS parameter server. +/** + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters + * are simply not added to the joint limits specification. + * \code + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode + * + * This specification is similar to the one used by MoveIt!, + * but additionally supports jerk and effort limits. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + */ +inline bool getJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if ( + !node->has_parameter(param_base_name + ".has_position_limits") && + !node->has_parameter(param_base_name + ".min_position") && + !node->has_parameter(param_base_name + ".max_position") && + !node->has_parameter(param_base_name + ".has_velocity_limits") && + !node->has_parameter(param_base_name + ".min_velocity") && + !node->has_parameter(param_base_name + ".max_velocity") && + !node->has_parameter(param_base_name + ".has_acceleration_limits") && + !node->has_parameter(param_base_name + ".max_acceleration") && + !node->has_parameter(param_base_name + ".has_jerk_limits") && + !node->has_parameter(param_base_name + ".max_jerk") && + !node->has_parameter(param_base_name + ".has_effort_limits") && + !node->has_parameter(param_base_name + ".max_effort") && + !node->has_parameter(param_base_name + ".angle_wraparound") && + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) { + RCLCPP_ERROR_STREAM( + node->get_logger(), "No joint limits specification found for joint '" + << joint_name << "' in the parameter server (node: " + << std::string(node->get_name()) + " param name: " + param_base_name + << ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Position limits + bool has_position_limits = false; + if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) { + if (!has_position_limits) { + limits.has_position_limits = false; + } + double min_pos, max_pos; + if ( + has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && + node->get_parameter(param_base_name + ".max_position", max_pos)) { + limits.has_position_limits = true; + limits.min_position = min_pos; + limits.max_position = max_pos; + } + + bool angle_wraparound; + if ( + !has_position_limits && + node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) { + limits.angle_wraparound = angle_wraparound; + } + } + + // Velocity limits + bool has_velocity_limits = false; + if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) { + if (!has_velocity_limits) { + limits.has_velocity_limits = false; + } + double max_vel; + if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) { + limits.has_velocity_limits = true; + limits.max_velocity = max_vel; + } + } + + // Acceleration limits + bool has_acceleration_limits = false; + if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) { + if (!has_acceleration_limits) { + limits.has_acceleration_limits = false; + } + double max_acc; + if ( + has_acceleration_limits && + node->get_parameter(param_base_name + ".max_acceleration", max_acc)) { + limits.has_acceleration_limits = true; + limits.max_acceleration = max_acc; + } + } + + // Jerk limits + bool has_jerk_limits = false; + if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) { + if (!has_jerk_limits) { + limits.has_jerk_limits = false; + } + double max_jerk; + if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) { + limits.has_jerk_limits = true; + limits.max_jerk = max_jerk; + } + } + + // Effort limits + bool has_effort_limits = false; + if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) { + if (!has_effort_limits) { + limits.has_effort_limits = false; + } + double max_effort; + if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) { + limits.has_effort_limits = true; + limits.max_effort = max_effort; + } + } + + return true; +} + +/// Populate a SoftJointLimits instance from the ROS parameter server. +/** + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely specified soft + * joint limits specifications will be considered valid. + * \code + * joint_limits: + * foo_joint: + * soft_lower_limit: 0.0 + * soft_upper_limit: 1.0 + * k_position: 10.0 + * k_velocity: 10.0 + * \endcode + * + * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter server. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and + * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + */ +inline bool getSoftJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + SoftJointLimits & soft_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if ( + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) { + RCLCPP_DEBUG_STREAM( + node->get_logger(), "No soft joint limits specification found for joint '" + << joint_name << "' in the parameter server (node: " + << std::string(node->get_name()) + " param name: " + param_base_name + << ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Override soft limits if complete specification is found + bool has_soft_limits; + if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) { + if ( + has_soft_limits && node->has_parameter(param_base_name + ".k_position") && + node->has_parameter(param_base_name + ".k_velocity") && + node->has_parameter(param_base_name + ".soft_lower_limit") && + node->has_parameter(param_base_name + ".soft_upper_limit")) { + node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); + node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); + node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); + node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); + return true; + } + } + + return false; +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits_urdf.hpp b/hardware_interface/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..a7f27543ad --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,84 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" +#include "urdf/urdfdom_compatibility.h" +#include "urdf_model/joint.h" + +namespace joint_limits +{ +/** + * Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing + * values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = urdf_joint->limits->velocity; + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = urdf_joint->limits->effort; + + return true; +} + +/** + * Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ diff --git a/hardware_interface/include/joint_limits/simple_joint_limiter.hpp b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp new file mode 100644 index 0000000000..34f578df03 --- /dev/null +++ b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp @@ -0,0 +1,49 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include "hardware_interface/handle.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/// Implementation of simple joint limiter enforcing position, velocity, acceleration and effort limits without considering soft limits. +/** + * This class implements a very simple position, velocity, acceleration and effort limits enforcing + * policy, and tries to do this in hardware-agnostic way. + * This enable its general use in the Resource Manager. + */ +class SimpleJointLimiter : public JointLimiterInterface +{ +public: + SimpleJointLimiter( + JointLimits & limits, std::vector & state_interfaces, + std::vector & command_interfaces); + +protected: + void enforce_effort_limits(const rclcpp::Duration & period) override; + void enforce_pos_vel_acc_limits(const rclcpp::Duration & period) override; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp deleted file mode 100644 index e6cf11e2f2..0000000000 --- a/hardware_interface/include/joint_limits_interface/joint_limits_interface_ros2.hpp +++ /dev/null @@ -1,734 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - - -namespace joint_limits_interface -{ - -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: -// JointLimitHandle() -// : joint_position_interface_(hardware_interface::HW_IF_POSITION), -// joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), -// joint_command_interface_(hardware_interface::CommandInterface( -// hardware_interface::HW_IF_POSITION)), -// prev_pos_(std::numeric_limits::quiet_NaN()), -// prev_vel_(std::numeric_limits::quiet_NaN()) -// {} - - JointLimitHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const JointLimits & limits) - : joint_position_interface_(joint_position_interface), - joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), - joint_command_interface_(joint_command_interface), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - JointLimitHandle( - const hardware_interface::StateInterface & joint_position_interface, - const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface & joint_command_interface, - const JointLimits & limits) - : joint_position_interface_(joint_position_interface), - joint_velocity_interface_(joint_velocity_interface), - joint_command_interface_(joint_command_interface), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - /// \return Joint name. - std::string get_name() const - { - return joint_position_interface_ ? joint_position_interface_.get_name() : - joint_velocity_interface_ ? joint_velocity_interface_.get_name() : - joint_command_interface_ ? joint_command_interface_.get_name() : - std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::StateInterface joint_position_interface_; - hardware_interface::StateInterface joint_velocity_interface_; - hardware_interface::CommandInterface & joint_command_interface_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return joint_velocity_interface_ ? - joint_velocity_interface_.get_value() : - (joint_position_interface_.get_value() - prev_pos_) / period.seconds(); - } -}; - - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: -// JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(joint_position_interface, joint_command_interface, limits), - soft_limits_(soft_limits) - {} - - JointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface & joint_command_interface, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(joint_position_interface, joint_velocity_interface, - joint_command_interface, limits), - soft_limits_(soft_limits) - {} - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: -// PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const JointLimits & limits) - : JointLimitHandle(joint_position_interface, joint_command_interface, limits) - { - if (limits_.has_position_limits) { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } else { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - -/// Enforce position and velocity limits for a joint that is not subject to soft limits. -/** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) { - prev_pos_ = joint_position_interface_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } else { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = rcppmath::clamp(joint_command_interface_.get_value(), min_pos, max_pos); - joint_command_interface_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: -// PositionJointSoftLimitsHandle() -// {} - - PositionJointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(joint_position_interface, joint_command_interface, limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) { - // Happens only once at initialization - prev_pos_ = joint_position_interface_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = rcppmath::clamp( - joint_command_interface_.get_value(), - pos_low, - pos_high); - joint_command_interface_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = joint_command_interface_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: -// EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::StateInterface & joint_position_interface, - const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(joint_position_interface, joint_velocity_interface, - joint_command_interface, limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle( - joint_position_interface, - hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - joint_command_interface, - limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) { - const double pos = joint_position_interface_.get_value(); - if (pos < limits_.min_position) { - min_eff = 0.0; - } else if (pos > limits_.max_position) { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) { - min_eff = 0.0; - } else if (vel > limits_.max_velocity) { - max_eff = 0.0; - } - - double clamped = rcppmath::clamp(joint_command_interface_.get_value(), min_eff, max_eff); - joint_command_interface_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: -// EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, - joint_command_interface, limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle( - joint_position_interface, - hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - joint_command_interface, - limits, - soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = joint_position_interface_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), - -limits_.max_effort, - limits_.max_effort); - - const double soft_max_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), - -limits_.max_effort, - limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = rcppmath::clamp( - joint_command_interface_.get_value(), - soft_min_eff, - soft_max_eff); - joint_command_interface_.set_value(eff_cmd); - } -}; - - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: -// VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::StateInterface & joint_velocity_interface, // currently unused - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::StateInterface("position"), - joint_velocity_interface, - joint_command_interface, - limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), - hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), - joint_command_interface, limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } else { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = rcppmath::clamp( - joint_command_interface_.get_value(), - vel_low, - vel_high); - joint_command_interface_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = joint_command_interface_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: -// VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::StateInterface & joint_position_interface, - const hardware_interface::StateInterface & joint_velocity_interface, - hardware_interface::CommandInterface & joint_command_interface, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, - joint_command_interface, limits, soft_limits) - { - if (limits.has_velocity_limits) { - max_vel_limit_ = limits.max_velocity; - } else { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = joint_position_interface_.get_value(); - min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -max_vel_limit_, max_vel_limit_); - max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -max_vel_limit_, max_vel_limit_); - } else { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - joint_command_interface_.set_value( - rcppmath::clamp( - joint_command_interface_.get_value(), - min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_ROS2_HPP_ diff --git a/hardware_interface/src/joint_limits/simple_joint_limiter.cpp b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp new file mode 100644 index 0000000000..cf8c2f8b30 --- /dev/null +++ b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp @@ -0,0 +1,159 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include "joint_limits/simple_joint_limiter.hpp" + +#include +#include + +#include "rcppmath/clamp.hpp" + +namespace joint_limits +{ +SimpleJointLimiter::SimpleJointLimiter( + JointLimits & limits, std::vector & state_interfaces, + std::vector & command_interfaces) +: JointLimiterInterface(limits, state_interfaces, command_interfaces) +{ + // Position state interface has to be always present and not virtual + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & interface) { + return interface.get_name() == hardware_interface::HW_IF_POSITION; + }) == state_interfaces.end()) { + throw std::runtime_error( + "Simple joint limiter requires position state interface for joint '" + get_name() + "'."); + } + + if (has_effort_command_) { + if (!limits.has_velocity_limits) { + throw std::runtime_error( + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw std::runtime_error( + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no efforts limits specification."); + } + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & interface) { + return interface.get_name() == hardware_interface::HW_IF_VELOCITY; + }) != state_interfaces.end()) { + has_velocity_state_ = true; + } + } + + if (limits_.has_position_limits) { + min_position_limit_ = limits_.min_position; + max_position_limit_ = limits_.max_position; + } else { + min_position_limit_ = -std::numeric_limits::max(); + max_position_limit_ = std::numeric_limits::max(); + } + if (limits_.has_velocity_limits) { + max_velocity_limit_ = limits_.max_velocity; + } else { + max_velocity_limit_ = std::numeric_limits::max(); + } + if (limits_.has_acceleration_limits) { + max_acceleration_limit_ = limits_.max_velocity; + } else { + max_acceleration_limit_ = std::numeric_limits::max(); + } +} + +void SimpleJointLimiter::enforce_effort_limits(const rclcpp::Duration & period) +{ + double min_effort = -limits_.max_effort; + double max_effort = limits_.max_effort; + + const double current_position = state_interfaces_[0].get_value(); + if (current_position < limits_.min_position) { + min_effort = 0.0; + } else if (current_position > limits_.max_position) { + max_effort = 0.0; + } + + double current_velocity = 0.0; + if (has_velocity_state_) { + current_velocity = state_interfaces_[1].get_value(); + } else { + current_velocity = (current_position - previous_position_) / period.seconds(); + previous_position_ = current_position; + } + + if (current_velocity < -max_velocity_limit_) { + min_effort = 0.0; + } else if (current_velocity > max_velocity_limit_) { + min_effort = 0.0; + } + + // Saturate effort command according to limits + command_interfaces_[0]->set_value( + rcppmath::clamp(command_interfaces_[0]->get_value(), min_effort, max_effort)); +} + +void SimpleJointLimiter::enforce_pos_vel_acc_limits(const rclcpp::Duration & period) +{ + double min_position, max_position; + double min_velocity = -max_velocity_limit_; + double max_velocity = max_velocity_limit_; + double min_acceleration = -max_acceleration_limit_; + double max_acceleration = max_acceleration_limit_; + const double dt = period.seconds(); + + // Make trivial dynamic clamping + if (previous_position_ == min_position_limit_) { + min_velocity = 0.0; + } else if (previous_position_ == max_position_limit_) { + max_velocity = 0.0; + } + if (previous_velocity_ <= min_velocity) { + min_acceleration = 0.0; + } else if (previous_velocity_ >= max_velocity) { + max_acceleration = 0.0; + } + + // enforce acceleration limits - set velocity constants + // the velocity is based on the max acceleration times seconds since last update + min_velocity = std::max(previous_velocity_ + min_acceleration * dt, min_velocity); + max_velocity = std::min(previous_velocity_ + max_acceleration * dt, max_velocity); + + min_position = std::max(previous_position_ + min_velocity * dt, min_position_limit_); + max_position = std::min(previous_position_ + max_velocity * dt, max_position_limit_); + + // Saturate acceleration command according to limits + command_interfaces_[2]->set_value( + rcppmath::clamp(command_interfaces_[2]->get_value(), min_acceleration, max_acceleration)); + + // Saturate velocity command according to limits + command_interfaces_[1]->set_value( + rcppmath::clamp(command_interfaces_[1]->get_value(), min_velocity, max_velocity)); + // Cache variables + previous_velocity_ = command_interfaces_[1]->get_value(); + + // Saturate position command according to limits + command_interfaces_[0]->set_value( + rcppmath::clamp(command_interfaces_[0]->get_value(), min_position_limit_, max_position_limit_)); + // Cache variables + previous_position_ = command_interfaces_[0]->get_value(); +} + +} // namespace joint_limits diff --git a/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp deleted file mode 100644 index cea695fc91..0000000000 --- a/hardware_interface/test/joint_limits/joint_limits_interface_test_ros2.cpp +++ /dev/null @@ -1,636 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#include - -#include - -#include -#include - -#include - -#include -#include - - -// Floating-point value comparison threshold -const double EPS = 1e-12; - -TEST(SaturateTest, Saturate) -{ - const double min = -1.0; - const double max = 2.0; - double val; - - val = -0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = -1.0; - EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); - - val = -2.0; - EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); - - val = 2.0; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 3.0; - EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); -} - - -class JointLimitsTest -{ -public: - JointLimitsTest() - : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), - name("joint_name"), - period(0, 100000000), - position_command_handle( - name, hardware_interface::HW_IF_POSITION, &cmd) - { - limits.has_position_limits = true; - limits.min_position = -1.0; - limits.max_position = 1.0; - - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - limits.has_effort_limits = true; - limits.max_effort = 8.0; - - soft_limits.min_position = -0.8; - soft_limits.max_position = 0.8; - soft_limits.k_position = 20.0; - // TODO(anyone): Tune value - soft_limits.k_velocity = 40.0; - } - -protected: - double pos, vel, eff, cmd; - std::string name; - rclcpp::Duration period; - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; - hardware_interface::CommandInterface position_command_handle; - - inline hardware_interface::CommandInterface & command_handle() - { - return position_command_handle; - } - - inline hardware_interface::StateInterface position_handle() - { - return hardware_interface::StateInterface(name, hardware_interface::HW_IF_POSITION, &pos); - } - - inline hardware_interface::StateInterface velocity_handle() - { - return hardware_interface::StateInterface(name, hardware_interface::HW_IF_VELOCITY, &vel); - } - - inline hardware_interface::StateInterface effort_handle() - { - return hardware_interface::StateInterface(name, hardware_interface::HW_IF_EFFORT, &eff); - } -}; - -class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; - -TEST_F(JointLimitsHandleTest, HandleConstruction) -{ - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::PositionJointSoftLimitsHandle( - position_handle(), command_handle(), - limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should be - // descriptive - try { - joint_limits_interface::PositionJointSoftLimitsHandle( - position_handle(), command_handle(), limits_bad, soft_limits); - } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_effort_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - position_handle(), command_handle(), limits_bad, - soft_limits), joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, - // but exception message should be descriptive - try { - joint_limits_interface::EffortJointSoftLimitsHandle( - position_handle(), command_handle(), limits_bad, soft_limits); - } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_velocity_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - position_handle(), command_handle(), limits_bad, - soft_limits), joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try { - joint_limits_interface::EffortJointSoftLimitsHandle( - position_handle(), command_handle(), limits_bad, soft_limits); - } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::VelocityJointSaturationHandle( - position_handle(), command_handle(), - limits_bad), joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try { - joint_limits_interface::VelocityJointSaturationHandle( - position_handle(), command_handle(), limits_bad); - } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - EXPECT_NO_THROW( - joint_limits_interface::PositionJointSoftLimitsHandle( - position_handle(), command_handle(), limits, soft_limits)); - EXPECT_NO_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - position_handle(), command_handle(), limits, soft_limits)); - EXPECT_NO_THROW( - joint_limits_interface::VelocityJointSaturationHandle( - position_handle(), command_handle(), limits)); -} - -class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; - -TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) -{ - // Test setup - const double max_increment = period.seconds() * limits.max_velocity; - pos = 0.0; - - double cmd; - - // Move slower than maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = max_increment / 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = -max_increment / 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - } - - // Move at maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = max_increment; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = -max_increment; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - } - - // Try to move faster than the maximum velocity, enforce velocity limits - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = 2.0 * max_increment; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, command_handle().get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - cmd = -2.0 * max_increment; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, command_handle().get_value(), EPS); - } -} - -// This is a black box test and does not verify against random precomputed values, but rather that -// the expected qualitative behavior is honored -TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) -{ - // Test setup - const double workspace_center = (limits.min_position + limits.max_position) / 2.0; - - // Current position == upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Can't get any closer to hard limit (zero max velocity) - pos = soft_limits.max_position; - // Try to get closer to the hard limit - command_handle().set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - command_handle().set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(position_handle().get_value(), command_handle().get_value()); - } - - // Current position == lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Can't get any closer to hard limit (zero min velocity) - pos = soft_limits.min_position; - // Try to get closer to the hard limit - command_handle().set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - command_handle().set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(position_handle().get_value(), command_handle().get_value()); - } - - // Current position > upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Can't get any closer to hard limit (negative max velocity) - // Halfway between soft and hard limit - pos = (soft_limits.max_position + limits.max_position) / 2.0; - // Try to get closer to the hard limit - command_handle().set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_GT(position_handle().get_value(), command_handle().get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - command_handle().set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(position_handle().get_value(), command_handle().get_value()); - } - - // Current position < lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Can't get any closer to hard limit (positive min velocity) - // Halfway between soft and hard limit - pos = (soft_limits.min_position + limits.min_position) / 2.0; - // Try to get closer to the hard limit - command_handle().set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_LT(position_handle().get_value(), command_handle().get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - command_handle().set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(position_handle().get_value(), command_handle().get_value()); - } -} - -TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) -{ - // Safety limits are past the hard limits - soft_limits.min_position = limits.min_position * - (1.0 - 0.5 * limits.min_position / std::abs(limits.min_position)); - soft_limits.max_position = limits.max_position * - (1.0 + 0.5 * limits.max_position / std::abs(limits.max_position)); - - // Current position == higher hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.max_position; - // Way beyond hard limit - command_handle().set_value(2.0 * limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_position, command_handle().get_value(), EPS); - } - - // Current position == lower hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - position_handle(), command_handle(), limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.min_position; - // Way beyond hard limit - command_handle().set_value(2.0 * limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.min_position, command_handle().get_value(), EPS); - } -} - -class VelocityJointSaturationHandleTest : public JointLimitsTest, public ::testing::Test {}; - -TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) -{ - // Test setup - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - position_handle(), command_handle(), limits); - - pos = 0.0; - double cmd; - - // Velocity within bounds - cmd = limits.max_velocity / 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - - cmd = -limits.max_velocity / 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - - // Velocity at bounds - cmd = limits.max_velocity; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - - cmd = -limits.max_velocity; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, command_handle().get_value(), EPS); - - // Velocity beyond bounds - cmd = 2.0 * limits.max_velocity; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); - - cmd = -2.0 * limits.max_velocity; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); -} - -TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) -{ - // Test setup - limits.has_acceleration_limits = true; - limits.max_acceleration = limits.max_velocity / period.seconds(); - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - position_handle(), command_handle(), limits); - - pos = 0.0; - double cmd; - // An arbitrarily long time, sufficient to suppress acceleration limits - const rclcpp::Duration long_enough(1000, 0); - - // Positive velocity - // register last command - command_handle().set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); - - // register last command - command_handle().set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(-limits.max_velocity / 2.0, command_handle().get_value(), EPS); - - // Negative velocity - // register last command - command_handle().set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(limits.max_velocity / 2.0, command_handle().get_value(), EPS); - - // register last command - command_handle().set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - command_handle().set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); -} - -class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test -{ -public: - JointLimitsInterfaceTest() - : JointLimitsTest(), - pos2(0.0), vel2(0.0), eff2(0.0), cmd2(0.0), - name2("joint2_name") - {} - -protected: - double pos2, vel2, eff2, cmd2; - std::string name2; - - inline hardware_interface::CommandInterface command2_handle() - { - return hardware_interface::CommandInterface(name2, hardware_interface::HW_IF_POSITION, &cmd2); - } - - inline hardware_interface::StateInterface position2_handle() - { - return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_POSITION, &pos2); - } - - inline hardware_interface::StateInterface velocity2_handle() - { - return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_VELOCITY, &vel2); - } - - inline hardware_interface::StateInterface effort2_handle() - { - return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_EFFORT, &eff2); - } -}; - -// TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) -// { -// // Populate interface -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( -// position_handle(), command_handle(), limits, soft_limits); -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( -// position2_handle(), command2_handle(), limits, soft_limits); -// -// joint_limits_interface::PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// iface.registerHandle(limits_handle2); -// -// // Get handles -// EXPECT_NO_THROW(iface.getHandle(name)); -// EXPECT_NO_THROW(iface.getHandle(name2)); -// -// PositionJointSoftLimitsHandle h1_tmp = iface.getHandle(name); -// EXPECT_EQ(name, h1_tmp.getName()); -// -// PositionJointSoftLimitsHandle h2_tmp = iface.getHandle(name2); -// EXPECT_EQ(name2, h2_tmp.getName()); -// -// // Print error message -// // Requires manual output inspection, but exception message should contain the interface name -// // (not its base class) -// try { -// iface.getHandle("unknown_name"); -// } catch (const JointLimitsInterfaceException & e) { -// ROS_ERROR_STREAM(e.what()); -// } -// -// // Enforce limits of all managed joints -// // Halfway between soft and hard limit -// pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; -// // Try to get closer to the hard limit -// command_handle().set_value(limits.max_position); -// command2_handle().set_cmd(limits.max_position); -// iface.enforce_limits(period); -// EXPECT_GT(position_handle().get_value(), command_handle().get_value()); -// EXPECT_GT(command2_handle().getPosition(), command2_handle().get_cmd()); -// } -// -#if 0 // todo: implement the interfaces -TEST_F(JointLimitsHandleTest, ResetSaturationInterface) -{ - // Populate interface - joint_limits_interface::PositionJointSaturationHandle limits_handle1 - (position_handle(), command_handle(), limits); - - PositionJointSaturationInterface iface; - iface.registerHandle(limits_handle1); - - iface.enforce_limits(period); // initialize limit handles - - const double max_increment = period.seconds() * limits.max_velocity; - - command_handle().set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); - - iface.reset(); - pos = limits.max_position; - command_handle().set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(command_handle().get_value(), limits.max_position, EPS); -} -#endif - -// TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) -// { -// // Populate interface -// PositionJointSoftLimitsHandle limits_handle1(command_handle(), limits, soft_limits); -// -// PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// -// iface.enforce_limits(period); // initialize limit handles -// -// const double max_increment = period.seconds() * limits.max_velocity; -// -// command_handle().set_value(limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); -// -// iface.reset(); -// pos = limits.max_position; -// command_handle().set_value(soft_limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(command_handle().get_value(), soft_limits.max_position, EPS); -// -// } - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From b975f82b6b99b7340ff6547e10cff069752d98b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 28 Mar 2021 17:08:28 +0200 Subject: [PATCH 13/14] Started working on tests --- hardware_interface/CMakeLists.txt | 5 + .../joint_limits/joint_limiter_interface.hpp | 30 +++-- .../include/joint_limits/joint_limits.hpp | 3 +- .../joint_limits/joint_limits_rosparam.hpp | 34 +++-- .../joint_limits/simple_joint_limiter.hpp | 3 +- .../src/joint_limits/simple_joint_limiter.cpp | 19 ++- .../test_joint_limiter_interface.cpp | 126 ++++++++++++++++++ 7 files changed, 183 insertions(+), 37 deletions(-) create mode 100644 hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 97770f6d21..2685a701e7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -155,6 +155,11 @@ if(BUILD_TESTING) ros2_control_test_assets ) + # joint_limiter tests + ament_add_gmock(test_joint_limiter_interface test/joint_limits/test_joint_limiter_interface.cpp) + target_include_directories(test_joint_limiter_interface PUBLIC include) + ament_target_dependencies(test_joint_limiter_interface rcutils) + # joint_limit_interface tests ament_add_gmock(joint_limits_interface_test test/joint_limits/joint_limits_interface_test.cpp) target_include_directories(joint_limits_interface_test PUBLIC include) diff --git a/hardware_interface/include/joint_limits/joint_limiter_interface.hpp b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp index 3c1ab5729d..931e12dc64 100644 --- a/hardware_interface/include/joint_limits/joint_limiter_interface.hpp +++ b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp @@ -30,17 +30,19 @@ namespace joint_limits { -// TODO(all): there is possibility to use something like LoanedCommandInterface, still having copy-constructor on this level makes thing way easier and more readable +// TODO(all): there is possibility to use something like LoanedCommandInterface. +// Still, having copy-constructor on this level makes thing way easier and more readable // class LimiterCommandHandle -template +template class JointLimiterInterface { public: JointLimiterInterface( - LimitsType & limits, + LimitsType & limits, std::vector & state_interfaces, - // Here could be used "LimiterCommandHandle" with copy-constructor to avid raw-pointer access/storage + // Here could be used "LimiterCommandHandle" with copy-constructor + // to avoid raw-pointer access/storage std::vector & command_interfaces) : limits_(limits), has_effort_command_(false), @@ -52,14 +54,15 @@ class JointLimiterInterface if (command_interfaces.size() == 0) { throw std::runtime_error("At least one command interface has to be provided."); } - + // Check if position-velocity-acceleration or effort commands are used if ( std::find_if( command_interfaces.begin(), command_interfaces.end(), [](const hardware_interface::CommandInterface * interface) { return interface->get_name() == hardware_interface::HW_IF_EFFORT; - }) != command_interfaces.end()) { + }) != command_interfaces.end()) + { has_effort_command_ = true; // Effort command interface has to be used as only interface if (command_interfaces.size() > 1) { @@ -77,8 +80,9 @@ class JointLimiterInterface if (!has_effort_command_ || interface_name != hardware_interface::HW_IF_EFFORT) { virtual_command_storage_.push_back(0.0); auto i = virtual_command_storage_.size() - 1; // last element - virtual_command_interfaces_.emplace_back(hardware_interface::CommandInterface( - "joint_limiter_virtual", interface_name, &virtual_command_storage_[i])); + virtual_command_interfaces_.emplace_back( + hardware_interface::CommandInterface( + "joint_limiter_virtual", interface_name, &virtual_command_storage_[i])); command_interfaces_.emplace_back(&virtual_command_interfaces_.back()); } } @@ -89,8 +93,9 @@ class JointLimiterInterface } else { virtual_state_storage_.push_back(0.0); auto i = virtual_state_storage_.size() - 1; // last element - state_interfaces_.emplace_back(hardware_interface::StateInterface( - "joint_limiter_virtual", interface_name, &virtual_state_storage_[i])); + state_interfaces_.emplace_back( + hardware_interface::StateInterface( + "joint_limiter_virtual", interface_name, &virtual_state_storage_[i])); } } } @@ -114,10 +119,7 @@ class JointLimiterInterface std::string get_name() const { - // Hopefully some interface exists. Simply go through all of them and return first name - for (const auto & interface : state_interfaces_) { - return interface.get_name(); - } + // At least one command interface exists - return its name for (const auto & interface : command_interfaces_) { return interface->get_name(); } diff --git a/hardware_interface/include/joint_limits/joint_limits.hpp b/hardware_interface/include/joint_limits/joint_limits.hpp index c53d0ad2e7..e3c0e1c0c2 100644 --- a/hardware_interface/include/joint_limits/joint_limits.hpp +++ b/hardware_interface/include/joint_limits/joint_limits.hpp @@ -54,7 +54,8 @@ struct JointLimits struct SoftJointLimits { - SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} + SoftJointLimits() + : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} double min_position; double max_position; diff --git a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp index 96ee386b4e..7d36bb3843 100644 --- a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp +++ b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp @@ -80,12 +80,13 @@ inline bool getJointLimits( !node->has_parameter(param_base_name + ".k_position") && !node->has_parameter(param_base_name + ".k_velocity") && !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) { + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { RCLCPP_ERROR_STREAM( - node->get_logger(), "No joint limits specification found for joint '" - << joint_name << "' in the parameter server (node: " - << std::string(node->get_name()) + " param name: " + param_base_name - << ")."); + node->get_logger(), "No joint limits specification found for joint '" << + joint_name << "' in the parameter server (node: " << + std::string(node->get_name()) + " param name: " + param_base_name << + ")."); return false; } } catch (const std::exception & ex) { @@ -102,7 +103,8 @@ inline bool getJointLimits( double min_pos, max_pos; if ( has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && - node->get_parameter(param_base_name + ".max_position", max_pos)) { + node->get_parameter(param_base_name + ".max_position", max_pos)) + { limits.has_position_limits = true; limits.min_position = min_pos; limits.max_position = max_pos; @@ -111,7 +113,8 @@ inline bool getJointLimits( bool angle_wraparound; if ( !has_position_limits && - node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) { + node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) + { limits.angle_wraparound = angle_wraparound; } } @@ -138,7 +141,8 @@ inline bool getJointLimits( double max_acc; if ( has_acceleration_limits && - node->get_parameter(param_base_name + ".max_acceleration", max_acc)) { + node->get_parameter(param_base_name + ".max_acceleration", max_acc)) + { limits.has_acceleration_limits = true; limits.max_acceleration = max_acc; } @@ -206,12 +210,13 @@ inline bool getSoftJointLimits( !node->has_parameter(param_base_name + ".k_velocity") && !node->has_parameter(param_base_name + ".k_position") && !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) { + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { RCLCPP_DEBUG_STREAM( - node->get_logger(), "No soft joint limits specification found for joint '" - << joint_name << "' in the parameter server (node: " - << std::string(node->get_name()) + " param name: " + param_base_name - << ")."); + node->get_logger(), "No soft joint limits specification found for joint '" << + joint_name << "' in the parameter server (node: " << + std::string(node->get_name()) + " param name: " + param_base_name << + ")."); return false; } } catch (const std::exception & ex) { @@ -226,7 +231,8 @@ inline bool getSoftJointLimits( has_soft_limits && node->has_parameter(param_base_name + ".k_position") && node->has_parameter(param_base_name + ".k_velocity") && node->has_parameter(param_base_name + ".soft_lower_limit") && - node->has_parameter(param_base_name + ".soft_upper_limit")) { + node->has_parameter(param_base_name + ".soft_upper_limit")) + { node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); diff --git a/hardware_interface/include/joint_limits/simple_joint_limiter.hpp b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp index 34f578df03..c7f0f01180 100644 --- a/hardware_interface/include/joint_limits/simple_joint_limiter.hpp +++ b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp @@ -26,7 +26,8 @@ namespace joint_limits { -/// Implementation of simple joint limiter enforcing position, velocity, acceleration and effort limits without considering soft limits. +/// Implementation of simple joint limiter enforcing position, velocity, acceleration and effort +/// limits without considering soft limits. /** * This class implements a very simple position, velocity, acceleration and effort limits enforcing * policy, and tries to do this in hardware-agnostic way. diff --git a/hardware_interface/src/joint_limits/simple_joint_limiter.cpp b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp index cf8c2f8b30..36b2c738b5 100644 --- a/hardware_interface/src/joint_limits/simple_joint_limiter.cpp +++ b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp @@ -16,8 +16,10 @@ #include "joint_limits/simple_joint_limiter.hpp" +#include #include #include +#include #include "rcppmath/clamp.hpp" @@ -34,28 +36,31 @@ SimpleJointLimiter::SimpleJointLimiter( state_interfaces.begin(), state_interfaces.end(), [](const hardware_interface::StateInterface & interface) { return interface.get_name() == hardware_interface::HW_IF_POSITION; - }) == state_interfaces.end()) { + }) == state_interfaces.end()) + { throw std::runtime_error( - "Simple joint limiter requires position state interface for joint '" + get_name() + "'."); + "Simple joint limiter requires position state interface for joint '" + get_name() + + "'."); } if (has_effort_command_) { if (!limits.has_velocity_limits) { throw std::runtime_error( - "Cannot enforce limits on effort command interface for joint '" + get_name() + - "'. It has no velocity limits specification."); + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no velocity limits specification."); } if (!limits.has_effort_limits) { throw std::runtime_error( - "Cannot enforce limits on effort command interface for joint '" + get_name() + - "'. It has no efforts limits specification."); + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no efforts limits specification."); } if ( std::find_if( state_interfaces.begin(), state_interfaces.end(), [](const hardware_interface::StateInterface & interface) { return interface.get_name() == hardware_interface::HW_IF_VELOCITY; - }) != state_interfaces.end()) { + }) != state_interfaces.end()) + { has_velocity_state_ = true; } } diff --git a/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp b/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp new file mode 100644 index 0000000000..ef2c5ca5aa --- /dev/null +++ b/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include + +#include "hardware_interface/handle.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rcppmath/clamp.hpp" + +// Floating-point value comparison threshold +const double EPS = 1e-12; + +TEST(SaturateTest, Saturate) +{ + const double min = -1.0; + const double max = 2.0; + double val; + + val = -0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = -1.0; + EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); + + val = -2.0; + EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); + + val = 2.0; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 3.0; + EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); +} + +class TestJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + TestJointLimiter( + joint_limits::JointLimits & limits, + std::vector & state_interfaces, + std::vector & command_interfaces) + : JointLimiterInterface(limits, state_interfaces, command_interfaces) + {} + +protected: + void enforce_effort_limits(const rclcpp::Duration & /*period*/) override + {} + void enforce_pos_vel_acc_limits(const rclcpp::Duration & /*period*/) override + {} +}; + +class JointLimiterTest : public ::testing::Test +{ +public: + JointLimiterTest() + { + limits.has_position_limits = true; + limits.min_position = -1.0; + limits.max_position = 1.0; + + limits.has_velocity_limits = true; + limits.max_velocity = 2.0; + + limits.has_effort_limits = true; + limits.max_effort = 8.0; + } + + void SetUp() + { + commands_storage_.resize(4, 0.0); + states_storage_.resize(4, 0.0); + } + + joint_limits::JointLimits limits; + + std::vector commands_storage_; + std::vector states_storage_; +}; + +TEST_F(JointLimiterTest, Construction) +{ + // Test throw on missing command interfaces + { + std::vector state_interfaces; + std::vector command_interfaces; + EXPECT_THROW( + TestJointLimiter(limits, state_interfaces, command_interfaces), std::runtime_error); + } + + // Test OK when at least one command interface is provided + { + joint_limits::JointLimits limits_bad; + std::vector state_interfaces; + std::vector command_interfaces; + EXPECT_THROW( + TestJointLimiter(limits_bad, state_interfaces, command_interfaces), std::runtime_error); + } + + // Test bad limits +// { +// joint_limits::JointLimits limits_bad; +// std::vector state_interfaces; +// std::vector command_interfaces; +// EXPECT_THROW( +// TestJointLimiter(limits_bad, state_interfaces, command_interfaces), std::runtime_error); +// } +} From 72ee141986ee6f542974aca326f45d9eeb6c28d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 21 Jun 2021 09:39:35 +0200 Subject: [PATCH 14/14] Add documentation diagrams for initializing JLI. --- .../JointLimitInterface.drawio | 1 + .../JointLimitInterface.png | Bin 0 -> 43445 bytes 2 files changed, 1 insertion(+) create mode 100644 hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio create mode 100644 hardware_interface/doc/joint_limits_interface/JointLimitInterface.png diff --git a/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio new file mode 100644 index 0000000000..cec461741c --- /dev/null +++ b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio @@ -0,0 +1 @@ +7Zrdc9o4EMD/GmbSh+tgGRt4LCRN7655KXPXjxdGsYWtxrZ8sghwf/2tZMkf2FAHCrRXZjLBWn0g7e5Pu5Lp2dN4fc9xGj4wn0Q91PfXPfu2hxDquwg+pGSTSyxrZOeSgFNfy0rBjP5LtLCvpUvqk6zWUDAWCZrWhR5LEuKJmgxzzlb1ZgsW1b81xQFpCGYejprSj9QXYS4doWEpf0doEJpvttxxXhNj01ivJAuxz1YVkX3Xs6ecMZE/xespiaT2jF7yfm931BYT4yQRXTrc3yPi/EnIl+xv92ltWffuw/w3PcozjpZ6wR9IxpbcIzPBuNLNG/ijCRUUR2Cd+Q32xBJDZW84zUiS5Q+bTJD4lV6o2BjtcbZMfCIn0O/Zk1VIBZml2JO1K3AYkIUijqBkweOCRtGURTCgfZuwBBpN4CuDBIo81/DkmXBBwThvtFwwOcaCJUJ7juWasp6EWSB0JOudmrMKe4AnExYTwTfQxLixq02onXigi6vSIQa2loUVZ3CHWoi1EwbF0KWd4EGb6gVmcxuaJj64rS4yLkIWsARHd6V0UrdF2eY9k0pUFvhKhNhoTeKlYHX7kDUVn2T3144ufa7U3K71yKqwMYUEllvpJIufq3VlN1Uy/aomBctM8vXKRe63IehEee8e3ekdSWAeELGnndPuE5xEWNDn+jy+u4FRg8sKgiHm/gpWfnMscFtK3gOgB2omvDDEUTgNrDpOltPkyRq08GSfCqfBFaeDcbI74oQuiZPdwOndx3y9GZgQ9f/6cPtWZikk8zhNBWVJT0ZTF8cSmfx/0WzLVQAEUbdsJjh7IlskdYFrO7zF1PeVs7WBXHfAdgsfxymqczpuwRSdE9PRFdODMXU6Yjq8JKZOM+rFKdhsngksCOSZHotjnPhzKoFZAA7ZTxoD7VGdreK8U4EL9c8Jl5nAla4D6Bp2pGt8SbqGDboy4efnu2c4OoPLm4AXCRXFdCWEM5oERWUg8mComKyQqKSu6voohwpqgxzyDU3aC6t91/gbkUXb4fKy0dd2nPoOMWzZIZyz7hDoukMcvEOMO+4Q1g6vOM8WMW5sESpFBtFXBhTCZ0RjKjJ5k8ZZXGbOgKkENiTek3TRkHqhuoAjKqte0ESm2f+DOG2jjnF6cDIKnSuFB1NomSvkb2I4uCSGZpoVDiOGJWF/SAzfSwbBwwG4CGcKRhla+wQr6jSq26H45hErksUmVbe69cF+NzH2VbNnUwJI46WUyWFmkKRHpD6znxH14RbqLedd1HbePdktL7qm5EegPuiK+kWTcjPNrlm5gnuuo7Bq1Urxdi6tS8ljpq+yKuMQ/kvl1Vu3WoMWzG3knDOijxou0DBGAHpJd65ev/zEj6Z5/6VaKVantTJs0UrrOy7rdMcNt6EW827yASc4kPu+9P8VBy86+hrocu8e9/hE01wXNEfz+gCUIbvB7vLPUr7Fhs0dEhGPmO2pkBcZitp1ZMLCFmW+Asf8FFSeiB/2Lu9FJlrXswINlNuyy5z13NCyqVyTic7JRNfj+65t9kzJRPP8nkEGQcy5XUZ6GEK+3MI9+ROVVHmkkBSal8pVIOE5wTHJp4r6T2TzgwL6rcweva5fpjktOLbm9s7JcGwe8Yq8T9mlmvTtuVc1V6dTZZaTJY7d0sh5jNNfOJV0W34Y1JpKHuBVUCx/KqbqKr+4s+/+Aw== \ No newline at end of file diff --git a/hardware_interface/doc/joint_limits_interface/JointLimitInterface.png b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.png new file mode 100644 index 0000000000000000000000000000000000000000..f58f7c7e7d1707cd8bfe0390df8cd2dd3c10d90f GIT binary patch literal 43445 zcmeFYXH=8H*Dp$wq99VGSpeyZ1VZQtgisPffP^kBw1f}{5C}z@1u23eV4+Ag)X>#i*L>Z(8ltFR9fkc4I zL{m#yMH#FH{D4$70mAxnc;J+I_pm>u# zeE$_|SedA*1>2CcLcA@!aQJXbn3vbTF_R-m-v4;KyuIF$EL+?9|kK;UJPG-pC11%>lcH9m&g2Jp^oGr2-;&IELBz_?kNzL9mf# zXbn4`FtmrI1KCza)yUe&NQ+{GF;|I1`(Pb3H0;9-J-xkcy#fhDHAk$67X}ZZ0HgC& zGedzvTAofuK9&(cHHw$9mk-2-6zuB=ym?!ypeVtiHo+QUI4l@ohXK=V;^$@Q z;U1d4j%Y7?U$tOEU!Yvo$lnYa7EBC6gOS!w5oljB%7ct@R6ztop%||ays;?+=?Eyw z)XbY~>*z%Gjj%8>vRBo@Si-%%)ICUQ8lI|JS~xR4T?J69iH9xPP|F^xWkEn7 z?6JORBG`;#7ioj{4-3`|CYu>~Laod}7AQbefSHWcQ1kMQH1X85MG_G(Pt8Dk1X(Rq z1%nO6d%zw1j1W#pN|-qeV70}Wpll6IQTCP)UmQZy#>-p-@9l^RCIzcGTG~W71Z!Dq zXgaBw;>ji;e|va%Fi-~;NHhgSP{K^{Hh}?wYSxAz3LG055(u?7QuhqD#o0vKhd`ke zJCF$?+*idHL?)VGY@C3_X@m`dVk}VI0zHs>4C8GkFW_hC#%>;s=}-ZHpp;G za)7muIRs*E8cBqEIEI9)AZ=}|G|^`M7AhD!+ellrFrcAGBWrR%q_0*4&fX>n1NINF zv(Sow7#;LsYexoqLZKkEryKs-=XxKm`9 zi775H(j$Nb4W{@5MW&j-mNW6OgORadq<1Jp)r08Zq>A!4HL}-KB|_024qzuOfCGbZ zQVBzus`(-VA}xc}Owd}SPz2eZWDCc7*g(RKp`jrpil>IZHw^0O11AIrK#({UxP>(Z zrLJy{_cv9;2NG4SOhO!e&`?!4+|bYriNd3;0s}MxiC!2rxF1$6%**&dNN+!z!oP_41Iti;DgYz@>10@2|#*TQV4#QP!Eu)Io`-5 z$Oxl?1@vY@#)Yb>n-csiFkYT20hT6aVMrf{H5g4Xi*!N|f;`X`Xbl^pb$CQ5(3me6 z<3l8ph+r>5n1vM_5dcQu{Vi<$EwQ#?LBU=We@l!vh+<3jFa(%`EpT{KOLH;~?%^MV zL-?zpDc%$ZPhiK{fQ{ft4BA8`Y!1j_MKZ@=gTl>2;P!r+#^?aBzd8!)LxLEY z;DWr#{-J2NmY<`ctqI5iN(37lL4#ls<|aW{L?{wy0$3t&7{W9ZXKIf}s%zQ8Li}xr zfcY~)`g&naL8=jOO%<|{9a!Bf(8>YhK{0Yd!GpYg90R?5;6xuxh_?+EX{TZmtZ5Sj z4I>_`nIO1UfVnB&2?4esAt(-(6e~>=gtdQ|jfuUf3Knis)XVpcnZSG+Yg6R_jJ@UHi3BkN zMw9@8B^a(r4AZ~{!L0DX5Ii)<7LNu52y?WuH#Nj~qbvwGPfe(ApqF15E{tNP4gyTK zgTE=#7nlqaBeI4q*u>E~3}KA{8==TnXnUV9^8>@9X&s1z`64tRR)&VEBtKh2dmmFD zG6<_;Y-NN+Y8VD-5Exh6 ziBNZ>5OLv#=759wE1rL7B+eQa2_cw50%02XP+Ri=ED5M?VdkgirxpSXAegA`;R`r! zql30BgF+%<<~E=JgkOj{&H)o*>;O<4;I;$@V-pm?M3odCN~Wm#hl9cP>Lif8B?4q& zVy0yp;pI*6wo)_J)PO-Kp5{hKz$F`nANZUIEYPeWCNLC$6-Ew}0~dMlIZ(=fdB(qP z5_ta)4pD=J+}qk?Vmi%aZfuANcm3JHF{%%X*!TnGdsZFCQ^nUyFzpd@zG3onVpKe= zhgZGGP{vQ{`Nr*M{72lE>O50wX3(BL|1{XwN;)LUC+?gmb)DDw_;azN{rzG`L;H7$ z>(_VlqbT39r_|NHyQ($lw5wd&`Lpb(Z7QI`eDD#uNmrKkItP-Fp5n6zy(8<+a`2%q zy*PF*rO^2f*S$Edd-PmR2k!4*&yD;2C8``|PMMZ>$SeQp&QnjZ@k>u4)j;HA_=v9Gn$rW@L1E3Z$Te{p~rg0qQBF9Q5D0Mdpw+T`BNEPG`@xTF0(7=ZuIUS zYx-dPzV9KdX6E_pScvzTd-Yqt!f^wv-#Y0v*I5p6_f)(q0j=iS$g>+y-5bsfmFV_G zW@Xvos6WjZe2C-F;iXhME#97)V$Q|@(WB#zu<~=oYe{SoFNVZ0UeS?p9iFe(ag4;T zVlI)JOy#9VVD`yp_X@!?4wq_oUxQ@Ie`ntG59c9$m;ZG&(JJ7Wx0|pgO1DFTTlZM2 z{PLHw5rMdTO4(pJJ;3_GYZ@Y3xE((GqJedPxV|%VU6W!=ZQ-O&vF-_l3tOpZA79or z7{M)Pxq&BWgs;8hVP%2+kMy}NHl10WoqGKlJz5j>bt;Of^xC=P>J-TBNs0qc!}(0A z`?v}&TQ=JF4H>gty_|lM1Zi4=T&W!U-2uM>6aT%Ae?i@F8tJq=S`1mwKtgT|$MhQY z``o~NNoii7AtQ({O4{_tn$iN<14MZTHc?{3h@z>-XZK<=7DDw0t0t+<7l&rV=upo` z(AW(6pRPFn))?d1%|rUaVXop^$Cd|0ZYIZcynIQ@KPCr*UVq+pPg5Q)2u9{qbWcgN zdT_ViIy!pRL!eEZMULx5ex;ou0<*5$DdlsAR_FS)rgWo=4s?)M#kMRx#yQWj!_I5XNlqXJsJnPI6dk z(6_Vw*Y#a)jD+l2QR?DlUDPmaB~2pGsD(pn3n~eiHMB-m9r5L&o{r_yOg0TE+DbB%z3u(hqcS zpJM$V9fK>*eDS@!t8Y)lk6hdD6eCz%6KP&4oEwJ>C*9PpeIUPHyZnK%AIUf>ge^O? zt=drU?q@antl-kkRay>Y_{!C>`{6ixxMlCgL_+E=P0mM#N7*lscxjZ{;n!D=9sX8W zm&#Kg@nfS>7hTvF8z?3{);##M3%P*w9HOp=rwXDnC+Q+Cyk1akhWb*^ zX6CspJ6?I|LsmY7Y0viTHtvnR&3?N3JagqJFKI<2cI~%m9DyZS8Ir;i_vW6oH3Z42s+GG$5iY z=d3s0iZX#lz~#@}+MUs1bGuzPa>%Jv_rsu{Hd7T4>pkZ@5kDsqo%V9-!+wBiYg;%%3=uy(2?ZicsQz7}Y3QqW&+3Qage~&kgRBSx#YU1mLEa0j( za*_*Ap4Ld^BSu?qwUl=xty5PMeiXaR%YT5aRjy!qIh|Dd_%rV4oaAn->v-+!vW>sH zn^F{ydu-o#i(RotH63yl-;9_&S9hkk+~MucpTZN$lByOOeexB31vRDTc zdxCkH8IX@=U)LH7g~B5e3!c@QIq3`yw%ukIgk=zQBKj2sBX9+1 z#h+O!W~VD`#!O2wthXSMDHmNgtnOLxY4sT(eXnR{!# z9X5rWmh+aK80 zF-6GZW0wla4T)<*uY^7m`f%{O$z}%P+KpjF^G!*x9B1&t!;*NHBOUN;BU`WXl4~(q z2HvlZ!UU7gW+uPon$>o&EtP+eM!s33tv8X09?{NPLP^)F^)$Li)DEmc$9L?8p@f{s z><_Hh!WtCChNASHbQGV}j&{iZ9>6)zCq>RFIV?1G&g=vtxYdzeN%8*UUN=8Lhnj~w z2h`u_FSJ)L5K?F6S3LPdn^!z?HY)r9eIYO4J)yRHI>fHm4FRRz6C#Bi$hV3_F~V4~ z$8@FS^OClY3Du!PYmaX4)IPts0N?kolJv_x%My=lR>}{~!HvJKoExy?w&xQ)980h@33>mCt%jsZB{;4I>@t(WaQ2j!j`49 zKNwWaYI7M3XM4Crr?@nSUWwk>M1C$Nj2kC-`?Q^Z>rAv7G(~d1-o(&E=L(Ilv{QdK zWt>M6$0*{E)s~ZAGn3W3A^QGq^1K9s9i^saD_AQ+7I6|o?5*Tt5ApFV)FWggu~&x=ioE-}vxg>nXG(2|oEvu8 zk2L(LiTWsWEp6h;=UJHZrck=P-(c^|7)T)=-92C;Kr6(vUZ?72bv~T{fZB`@b_;H##?w57lQQ7gv5W9AnVFeqI{}-ssAVmY8^Q0W!SaA<8PFV>R z*L&6oxX*W8NjT^Xf_t6G{>;L&;jv|Ev(>=+vDi6g>xJSiRh09oW_*op)mqv-HD=&L zLg9FzBe#0@;9o=nSs?xZm8Xg{(~3uNa^=*ybrQ`{MQ?eo5LHfFu+<+-XrR`$Rh*g& zBuYUKiA0L@vr^iwL<9|%z<5X6@{KK)_~CtPjnY9wjLIi2SE?RNlJlfWKD!9GR7Q9q zqx__AERJNP=r2%_>aYPsPGml;zr@%ar8y?)_$F&!dt(0*zG9=p)y#2P5w)N@c|gUi%>leOd}5MX$nnY(JmvAG&h=B-`4g z+xSj^Gk4`Z{M)u;(2fUv35FRWbA{B=M;GntJ}ZHUV@BeJXLr&%-xP1MkMGnXqiEQN z9B+_+N@zULmZ^FXKX}bsk^bk$iO;NNS`<(t>kGxv^A3$Nw6_YJeU_FhD6}}>8fC4= z!FHRe+$r9o#NeUUhQv+oak9Ug8ZS$a&b7$bXNw+u?s3(VnKSIh>#!v^dGQEG;v`m& zfXB<9)IPM>4!z?+u*r9x(pG#u&=L9FmbW^4=9OLt_+)j8=#!_Dj;0BZpe@l0TsSdD zzD6ol=)NDHE1|xp`W5y`XLMSps!YLkp10xt@Z_;3$I30MD^gHcBEd5H8rJen+XrjJ z+3d*=!N`?V6fO0*_~8&*J9WS5Dz(J1W9=}G)XLF-sPGUZ)LkMaQ;=A8)_>FBZ%JVm}SO)un%LDRS)!p`gZ=bmwxQ{Pk?fVXTv~U*yVn|t|y@`KsD?mFblsYBc z-vF~V2@p>}azEhTTR6~8Py474_;0{EY7B^%bSqZl@0)`z(9UOlXSx8K;N`3my=sg3D2t89-nMA ze%0I&ClCijT3Nec=NSR?)VRz2yzx`d)w?+Mc!4nV_(MVrM`C|03yMWwcvEnDSFleV z#$QA5dHF_`mI7$kWiO0~U>i%ER*MT^+PoQ6bL261IL9)5Yn&&(I4;Ep9?l7}I3@Cq zM(_IgRb1g#*BFs*oWP5y}`jMNlU2?M^e)5Fo$Q>B5iaubN;tZruxe%v>-Fh3T8J)F9v3f;WN~N9j3nI?O0` zWv*QDEp5{5J?f<}i*t%@$fg+ZRh#~A3((y&itw9<4Tl>SkH@jb1+wrS4azitms}9> zeifE9yB?;A%s!ka#js}ynVI)01RCnWyvAR_!npMUpE+PUb;! zZ6kX*=W4KIk#CB+Hm$Tnvgq|DQ|YsgP0qp&*sk-4($mcFDUH=O?GVK&n_BL^}m9dvN$-lUKe}w+72`#9nnd@{db;?gyzFVU|Z4%U|vVXNtg2pQ(n~ zkju)f(KTb7zxoo{)P;5?Y##Ark_x4)h*zy`w4A;up)%klXBhnF^o|LdqYD&&)hJ%I z+?21d#q54;ElewxntFfLAiqC-n)BSK<53tLt0`#yT@ArP?6H~;Pd@A>#`kWYU77Q- z4jXD47kw7VuZJvuptR>;oG|tzVsJD(n8;{!7$3|o3=4#FPl@*XidI}TE20KB36~|> zh+|%x6Ec$7Q(ju3OS7M!OPLnM9xpF?Z#0@YG6+50gfC0(_gFbypqe?SsV{lep`S3J z-r%nZi3~chIw9psL)Q$PL7!xvneydb{WfU*(jF^jWmIO*^BTML>X_1kZU^zP-|0GZ zW+~_5ow}H^rsoYTBNfM>FiQO|So$NXuh4zG>HP6{!n3Y*#l#N+GHLc~lSeKpQb}${ zFLoGlXFXgUY^-s8*q2Eya-jvmYu6)8Dq5a#&w}iWu&kc?Q z!burG4c#=;#)}a(WYL^ zeAEKKSoZ><8>Fj26I@kkGzouB6J_0Pev!C3h4{SZ1je5o% zlzY3g7xLF;zq}mRqjE%Jl=Gk0Pyif5X~|N(oJ=iEU9aOtjllWWOIxUThx={5$jD|*(uK{XOctF(T2`l04t*(<9Kc1h4& zFlsy*!L-g77`^LUWAsBP^@#naPcZK2+c4*yPGt%p+$TM+-|RAd z(bKS{ud~j1JC{=|Gy3FSuEj5W9^yI?gfUJ)$~pd!iHt7(CB3pTA8T1;9{uxo-!V*w zTiNmbE{XHN)*O@@Bd2zT6;njlr7mfibFT*lcC?foUkf!Uc3qt^_xh1>mYw+Eoxc&# zH`jFgj|G05_N(b#9yx=dJh8$rch)Zd&iXPBuHiJ)Z6?ndYtMWn-@YcLnv|t~HE*)` z2h#0vzrFWbd#9N+Xy~|i^3BNknaF5q+pbv=x#({+#aZt?x1Qd;*&x(xQ?xWR->Yt7 z~ zq8p@v`MN?CowtlLQ_6lIR`^zV=RxTQv0m)G!x%^IdCU4f>YcWiR(!Z1?Q5Mf6>2;} zJc=y-(g~=PrG&X-9yHNCj)I#v=Ep0vQ>Bsmudrdradut-GmU8u1f=Rk@heon>n7EAt5+zo5$2o z_PhFl(I{Nna99YBS47W;BmXLBc{caypSTyS*X7#HWxTSLvu@4alcB}VX_hx*%j|W? z?8RX>=g3^d-F}@^Mbpn!WtsM6P5 zS#)FpbObW;b!RC)Rx6TrdHDW6)b!>6Z&^;((`2m-4G1Ya`59dI3}S(Wy7%)^DVLWa zV{>JC;p~k%K3NqsPm+0L^#RW}l07Bj zfag~^p)vY^M~#vst0E7yQ=pmm5e}?E`?+xYD6IpYm4^h?699$$)1{t^+XzrQr-^TU zFi&)+S9rOIFxh#-C zuT2~mJGsbbNMQ_P2ALt~j$)i~=XD?1+I)55*V>G1Z5YsNQw!5b>>aiThb35k-f@T( zTRzLEYp6<3g~a0zZ12p{0J7jXGwiw?Q@b1`|L{*9O~s$rhBRooz(mgQ(1-KS$Ah)W z7)`eeW2?G=rhYA7p{c~bV;lIJn2QE&n1z?5}!!^tByv2&1Z_mEgiycS>IZ?dIq-?g~OF0yJogiS)Ux$rBY z{xst}&1*X0qsa7zs<^$kN7-M0kqBYaZEXVRhgky72OMz$Bx&zkZOY(ZWv zsOsEB`qlQ3Y<86&A7SPVaKUXmwH$AWR|aMScpfw73Mb_V?*BYYiMpTHE2}<=)l`T% zK6x#UqeZ##5^X$X+~7DMFz~!k0`!GUjt#rk-tp$0cZQfhC|9uS$dVELit}-&&JPPc z#DXJ$+XJ4%Y*R7=gK0?- z7I&U?@$e>?hdbiQVSoTiZ#mH`!tYxyd`%q;MZe(F}2V9@C1Z z+_Dnm7^8^Zt#fA>Goo<~<$5yo?y*Vjy^$8$Ch>f9@2}`zfX&Kn*|+nKo_cal2+H;9 zMSMGRVB!xb3whzTh@zD4QO%IqrSI&WM=y$LN*V`fFeG`f_q1mV>Cy3(u%Yck3?@bn z{R&hm7B$+%G9o1t&L5iOG0R;$ zeY)@h_m%-^aH*{&4fd$&7i(y!kSA>V_+cH)re6FKTd~`w|4j5a;{E4K<8Od{s1V2tlsSZcpmwKxg zHwD(T!){F{4v`(5uFL*T=3ve$R`|k^Fwv5fBS_QNcxDwkF!b=*?+ag+jP)t=;6 zKC=55v7e``}v7 zJ&16FetBTjEc(vU{R@DF+l_9`7F#>Zu9TfbT<**K?uJABsw?Q#Vm*ynpFs*71hc0oNBTxLxP>` z-^mBR_^-PF{_q3A@_#CK<(E;m1)K1!Dbx$RX?aD8ZhSS6*ErfThvZP3YYw@w{2>7l zE{j-`mZs>1EXHthEP)uG?nPv(JZ6(<74xvBr&6LJ^s_LJ8zSnX)Bxqi~jY2}GUZ&-LbejyzxR_J7*@dJ5ZmHdeU}BK9fQ0x;Pps6V|}h=A)Dw^~(~y#-=6 zH4xQ#L4BJ5Od+JYzEIW*@Tx!Vc>G^r^M8TO|9fDg&$i~8bAY1tm^(IS!RJ}hxWCWT zoUK|vj;beJNqw0a_uG`3_R;-L59vWn&X6Bv!+fQE`3+ZVtk+_J*e?J(u8o)2wKt#u zJnd^q|05m3;mG}cwm^aw=KH>1safGT%i^g=;YB$o(zx4KR)=3*FXZWFvtYy?AcF>5 z;I__ei#foX4`7z}xj<*%j;?}dbx&j#4=?`IitK^R7y*qQN0A%4(>*j?JI4k~0$64H z%mA;pP}hmhfPgtIS&={?6n#1~5l%4wgq$|B45f-gEATNpbS$cZ6AmjsI@H z*kT9D6d!B8)~wrGt;g4@y}W*6>+w9zK8d%;P3!8S8oeV9h4K3OvQ}wAx4K2-+w^ru zN9)C(crXdZ5Mj{h#P})Z%^RgZdRbnu@G^s2AG3vvlLQ!V=vU%4Ig)gkt|!pKg5eICzttFv>ECKNj7Q-`Ij!sUtzbozBE{f{Ba3}$+J(aD=frvAw9 zQ8Pdg*uE{XP;nm^*WR8i={N&T_n==R%uyAG>be|Lm*&P2$EI?mRAVA)(+|**eL%2q zwCaMeR7n&jBdUQ*Zbb7f{#9)K9`otjOLL&3}rJy`^4<0bTDpJQqmAw_k zDpqwfMUKSvuqeKlHQRNNp=^yYE+d?wB4;-wjq4G!){>8>EEg|OjK%M=TQWW1!Np~U zm(Jv#+F~Iw6+GE~636cF2UUIydZMPp(7FFA+l){g+lsA$%b&5;@dlaiZW=@7^78?7 z@yiXyE}d=M%TJcCKXc{fADNd=KyJ6pIBv9T%}K%{5G8^q?6&7w)PJrs*?n(sgA~or z3?5;|7M|IgN(z&Fj;|N7)Ye^B{)8PBpNI_n1YB8JOvWR)K26eDSw}|Vbe^{rvyX>7 zk5zIS#4mhLT|ccCIwiIE@kC!-@kM>+oSK+}o^%DE?g7xkZ7T-hFLW7TBT02PVc-QY%Cr56+Y&rIV{C4?Q-K?qg*I(Fk zHr6KSKLwLPGMz*qN6kKGp})-LTf6_AC)Nw;RV}En`b8HSfl$vPbP$>Q zL88g=p&tozwG^qH44gYZRKz>2|8VHa{Nr{=un@n~)atgscirLl;5W_%mgpk|PnHwE zYG)NcvHMIbS=S*JDZI9%WXw6u6u;b5;v??qQvK)WG)hGLuH0HhlU=Ga_FOem%Omj} z_UV~(zu*jp*G_wN&%<`}oM(2Mr7@LXKAuT{kuTb6Y*(gr6n#>hQg7m`i{vSAsfAA> z+a^H1;tw6UTMQ#Syt({bu~M;=P}aVo^Kfi3D>0%G~|o%vgy zz9UX|TsLaqk$KSbHv*N;X{o+R`qjMqu_;&Lx9Ny>)kY3JzQZI70#(w>r6cDDV`ty0Oy}iUDK^`~@-MI2%=p@sgytsyxCL$*-wHk|Jr74%f<&_-i?83A?|)K8W}Y5pYlf`g77-ihQ3< zN5@0sCT=_Bvg;lR|5-)w`GR!+Z#|Fl{U?3)DEDvr>=U2U)MFb!{4DLs;RhG~rO#qx z+t>#WlBB&;4|5CxS@7f^Wwjb6Q#h8`gKQsEC08YsB02Or*#y>jZ zAIG&OSMXyQ$Cz|g`pMf*$&QmgP~pED+{@td@xO=Pz`=g_+XLnQJ9K&M^8@nlHg|0# zAc+tp9>8X0Wt&-XBS88|Dnr}(xr#?sWNkV-qwXsB&>xNI90!f z{7}j2wqzCS=J%f{D|=df$7kk$9rA|jlk6GN^t`yJ=ld~ikCmE#98aI-jUQ({#ge=q z16k&!i(h#~`?AIc%l_YzORg6w;*6w~?b`PV=k}fIXPnaSKKpVP0B+}5f3uo0=d&sw zy(qWx;!&J)Lf$_6X3Ao z6&XQv*q1;G+a%LttyVkr7tMQ}3d^;}e&ow|cUFBp@|J&SyB_H^ikWKm&ib>5WGK^1 zKYOJe85x3Y*fwcI4?V6<{jh@8(LLHKzPu-z#tL}^>Ao^q-0lgRH|Yy6Q0~uuSc$r~9K1 zoi@Zoh~d0a*qZ^|X1v}K1-Q(^t-1wxb)L`sFs@e%pJQMDUXXm3&uuMHJV^|{vtUum zbDrapE&u^?=1Q+$Be3o)MAIL3^JZ6c8hz$Sy3D?r!*OAvhOkDrE`e<1=}0MN6)$Da zJu#75zI9JQ{!-8$yBj3*a`XCdm2pn}YhBxBwu_I?*d!mVoU<93bRDp&(T>T#XIs-! zry~g@{kb}BEOq!83nW)h3Yh+R`H6fw@-!(!1h+n(%1nC&rim)5{=Rvv)g|r}lRa|) zllHL7;YIV9zI|6F|A_b1Lpk4>gzXxV_&tVj5o#u ztDA#88yyxoHjM6i22Xzmjc+)En#oQ^W6I=Vvwk303!XJqN|Q&7|19#pVw?5X^4(5n zmV$V@E4Qv1UBTpuz(1{J=DQY`#sY^~66)95QJ*e75UCP6;pd)JhD*luQwJQ@Co^m2 z?u|JLU#GDAPDpKQ-fC%`m4xOPWrAIH2aLI8a$8b4cwkq=8r{Tw#i+VK_HA7^sciS+ zrQWp{^i()@^eY0k5n4`+MuYqn5ptvwUE2UY(K*SgYUP{N_`sYw+a46U(sCX!B^TrK zWllO>kJKnAFTy6nFH><=sD60%z|gDNoM+2{P>j%ewA^BUcJhOF>RI-d{F<~Vg?(_A zbT?MfYD7Me$2hq}Q17A9?Qex`K%VZY{1f{>$zXDa#!SxP+-joI(Mb3HwMG$z+5Pof zk{1%^-Q)-fV+QpK!^6&m9;X*W-6@ujZn-sDItq8oxLeGWS+T%UjHW z)T!SDYN}^9Wpph_pOLciI^p%B7~>RvQ`+<;eaKV!;307ShbnL%j&%3j_tYj+*yS;D zvAj9oOz0PdE=%s-Me)i>;;PWyp?A%n3(OYIO>^YI%0@tfZv$7MPMeSoe5y@)+sTuS zF=anitN4;0sI6R!sr)r_3_IFfB?}sw7YYq6Cj`F7m)W1V0L%ZU<+6PBhh@B)go@cs zx;-#25v_89D%g9pTT*S80-G4y8g&W}?;Qpr znu-*57g~9Y^i^veq`KZ1q^Z1RwwZ-+&J|>^vKn5 zv4j0PH-;0qup~kbzY>t zD(}IE^xx$|(GJK6lymG><_(38#>e$QYVgdSd$|%Sr~V_S?)vlJIkWiF zJ+Jt8a`W^zse>j%Cm%?Z=1GX>{~|=?`S-oxudwVHCHr01H!rQPv_zH97va5rpjCyn zZxQR9guBl@o%FFLT7s6#g<^HL3*X{T5W_7a{9qI2JUiTO6F}3~dqV4zCWD5)|6BvH zM^`U!ukeK}oSsnAzt=IV-NETbxotdHej<9TaOXDa)#$gx`rZ1o2}jn0a?sqD{CZC1 zO?_)jgXos(HWe+FBp^%Uf86N&T;L14x`Rz0yLQ|4ZXd;FT{7-pkow?^Bm`L+M%L4N z2)JNQDA(L(iN2|IG>L*7B%nD*&2Tv=#JYn}f{r1!+ELTGdaO=K=6 z|8{!s%gmY+!rhmmJ$vnp=%sd5&-%|*&Z1SFE{kocrcBcf^76RIG7Pj*8^80$y1tC3 z*uE2Y>GYxvuv8((mK8(;q{e|uE1zVEcfDDpkM$|>{ST+&bY({eth9xDC$P+at>>N7 zh`#{Xvo^_l^mo9XpuFqTx<@YoXc?a=(?=nLHFQx)cvhMS z+9v^?aR0mpF8_lYF-rGbUOqkGS#-A#+y;1hH|q$R`T)x9vT(U zS$86tz}|sJJ(eWpO^Z)1XQ^?#lw@w<3pwNr5#&N0Wvl zXtw}mT(g|QXRY6E`J0K2cj5Luh(Q}dKCZFQp>TJEw!CTO#K(}eWa8^*E-~*V?d)l0uw(;UZbgZ zYc~FZdGOyfZv21JxDN=vU5d+o41Vz+KqB$uCBaq`g)SxLnw!CGUv0h6ouw5wsEWGB zTZKp$7$>BgGdn(i<&Hk|z3X=H;-ODGt=73UTjGrC^uRA-P5Jw*y4+i^+3cyFI?O(o zF4vaBr{WN?toJBp7j#Jt^N2)YS~+jK&xMh_{-y_r_DO6j_+~(%e#qiaUkmmbSq!n} zH`i%vGjzmhVvc>bX(Y;46xlq_m;ylmsYmC?eBDQwUtH7kWp+Ju=}5S-R6>usFZ&|@ zlyisH@^mu6q*_e4yN>Jo-L4*$3;AAw*7l&shnM*oL^_g*CybY6@|3zQTd}-hdiVRd zcGegF4edW23kQ)L85Z=rale_m*aFxkPFi0nx_%{YDa-7RbTyE{)!|4CGBkjY@^LrHENS2*KOl@J-R~B7d<0fAm9Fnb^fb*Sf+EUDA-~r@GsQQC-X%1=M#6&SoMq* z`CEOZq`mxlI8LfPWlazY>+Wf+ILS#?{lh99NQ<4|>l4IV$0#O*uAgtY1)Q7r%t@9o zMlilf7UJV;O#P5xN!@^=^im(-a808a=(yjXI%Q7I4vHZB4YtQSI!Af|8$U2HGoURNDTV& zOYwvyXC{~YFtaYEx;B~56!LNTCC|6Zd=vWwp0Sll$i9pQSK#ZUUdh6e`O`7nQ(QmA zkH4aRo+`I`2-AQiWGmWvm6YIJxU!@!${_m{S!{v(Zhtx|_IjX(9#F*ug)XW#qGW9J zj!7ZE0u2RkY~-VwWm7ZrVr?bKZKj0#8ch1)8be8iyT9&-m-Vw6<-D{SQEvi9M1vFENL!*uo7Z056UjVK}{7lZ!0HhCiYlE&u!Q4{Lku}bhEYA$3!f#{Y^ zyrl;uZWT79&m|P!IQ7WK5A}(Nl>36{(_-4*m~IjAm)H%#8HLO3-c>;z!kS7X z+F{TTx}Ck}_@SKdrkRCES+^VZ5`v)@FBH-0HL2J#Gn9CUAeUcpYLO_i#qXAKjpzrM zKysp>C?1J0axIaR?r!8>sI98#k}3rLpM>$ zX{8NSn+=k$>U3$yYNYPqU=gay@n0m}Q8`rqc zKd}JfoAbLVuVgNVobepPxck5JbW~buLpO&~dpL2lx}tL}D748ujE$gQX1?e!1Igt8wJ!Ncp<gFl{DRQjOhH9eFoi&#O`N;^4wdeDdA0>B++zP&)UmIr z0*94U468P2bJC%{Bl`l)&ZfY!o8xE!A*SWXz+#GFMal?4b%so=%VaDZkT4VMlQ_DL4S6}6FGW$1yiZ08Br{g$*1X! zNs5lSy|h9SVmC!4ZL1Q;3JlU*zqm%$@M#@FM2r(uZ-@&*<$eGdg?LG*PL2-$#ogZ$ zC(K<8zx43l=MLi@mRC2!GD^}{d!OQqVO_4+@GfMqiFnd|!EIaV2wvo`qfsDTJwj=2 z*4ZD}n^E_4+Pt&(?%fqzdSL}KVVm%gtks$wzBEA?*Fd%^1^1GQ2c_cpmpPA7srCZJ zJ7zsL--qcs*XipH!^+lcMm#`-{b)1bsOL+?2JKYPXNPm~M6Q(FlBp+8Rb`bDO@V;Y z1PXde;)8}w27+*u)AX2B8LU?SES%fc3X5esmOtnmf9-%8)o(D8hw13_1qOtnux-tc zk+l^_rI5pK*^{XVu(;u-JxcoIr0WiWW%bf*S4M^Btk3j=7hVbpz?$sVHp4=9;<6c( zawnInst)J)sWZ0y9$rkD z#F7r%yi|%Zy*dY{z}Ws9W~$#)5dOs20s(}e0~cEETy))t-1`}!wv=V_MV8AUsEziI z*gU_=iyVX19jn{HwCj_nf40;1Roo8bKnT)+a6w&IAzP#z*j_KWKCr!N1EH@vjG~yXb3Up{;#O? z0AP#&b?T!7ALa%5nt352;?dOX-1kqc_ETRO;F2+X-G1>%68sm3e?9*O0=b+vp!;V5 zOxWLKom|170Nj<@9b4asfNxc++*Pl&f$;UhN_2-Z5;=Y9l|Ck3E6oNgjSd_L*si?Tqkk%noA@3GOed^2v6q-2K!C)Awtsc0<#j z&O8^6yEBWi*+3mu-3)?iK~sV37l$9Cq7r8N^$`C5n8dTIpFI_CHBAYBC5r)?S2`p*X0hNLWX}5eAqj#g5U+$ zY$8N!Us`p2l#uqY%GP^p;&*5gp&6i&jn2Y1r$LGTmnN7S<0l~vm7Xd`PKl7C)Xihd|> z|NXQT&4?rJv8iaYUT|LD3(SF^%GUfyOz#0Oh-V(lFs*%M5)ZFunbSF*o^@XU*F_(F z&?_R%^8ERTVVZHiMudq<|Cuj45*pGRDkY{gtQk_B_znKUNm4Z=Fh?cfcvQ6 z;B`bh66{R*pLSb+kwV@lY(` zYZO6_Ykj3FS)O~9Y@^&y)hHd8WRruQiDAtLUH0QG=^!hsIY&?gq>GEAoRBG-~qVS%?OqX$S&%}+?;0d4k2+H21Q1| z(l2Gk8}mO9o7Yzr>nME8hQ(V~N{<6)f0EF&!dLRd!c7q`CNleq>y&xu<7WcQ(# zzl+fu`oT6>2vzlO*Sp1yV{VM$f~=8wf#;-r8ghn`_l9cdju{B4O@t0J6|WET^+u%A z#3rG!O7I5pKMb6~cM+GTP9=Ju_f-v+i!L!}fJb7UxceOo_DVcuCGe=e-j2|4i3yG` zDEHULMAegj1 zW9+QkbPd%{7QSOYC$aGGq<4x^NN&@^W` zd;G3L2}mvRtZLE>HNT>PD>QcOuEqz&47A*T0@TM~bZ&SitGprrvkEUa^n(6^a8zT| zT~#12zh;b~At-$vYMd0IyRRKLlGEbYJvaW>7ql4EY_MmWY$_<3#xgqB70|Vp-nQX$ zS-6=5xtsrZI0@7EilA|Y6gsN(i7$pv&)Mz12hSvSV_1lQ34LfONx@^pvnGjxGp7dQ9Sg*>dRMZQ6;chG7-u(1`J8^j zDng(y!Q`D(RaC;G%k$QIO0C-a1`)5dCiJG^c)K!!7ER~lmaex$zb>)HbH|%-r)Q0s z>wKc><@hksA*IeRBf7-hVFKF2UtCQq*%WYDw?W;TG5-dA{2lT2FOHrX^3`i(S+WXC zmmIUY7D4aFoQ5Q+;S4o`(5zk=N;mIkLeIsh=86RfzpPh2@TAD=PCMuIZNAaEzPG6? z)){|g^@MCZI;_K1VRlw|yo^5wRYK|7jZcfc9B_`{nRvR%W#l3&539H(G!K|PAhq34 z^p-A2_VPYl*fZmeDa!ieeQ0RqS*umOc4rcQcceOdm}rtavnr)hB_LgI#Ks;bJsu?( zgoCu|E3Lnm$4~fWyYhk1Ay-wB(4RYa<~--~mp{!4uemEzEV>6^wM0raxhtR4>9VrnQlKo>8h@ zGHVrPzthq$|LraV;S8Zh;1+uh!ET|We2#jbY1jiZBHNW!v(Dq~BMyyAS}fKJHq0KJ z`@No7!#TIqj(7RI{d6j(69K>op?`lkC=&OZKutieVE+kgyk+ zj>HZ#j1-PJx?C~pr=W5r*w=_U4&-Qlf2Yoe#uBgpnVvk#F?*RJOa1lC*iy??%94NZ zwa2=EJoOYw4epcYDH_mg1ES2n01{GX!;s?O$4ukfn^-xt><)9>H2iuWcT$CV?Xrlda-V^m4-Bs$`?zCBjN!ZU#=uWHZO2v)^fuZnQ0_O{CqVKFDZ2K)a0H{33rw+40U|0IL@ zzr4l&e`kVk*-|Qc%?=0dTHl?AuVW-8ta&H(-X_-Oy<`+YJeLQy0A1Ef z=Qi=uQSK2@{S65K=?nRSEYY)x!H%)_y~V9I=KI@``i2^3soa-nF9h7U?bmEFKYY(Z z3O1w<{r>e+okKqx3V`Q1&Tj}I$Mv3Ll#at2L(#U8JRrplY_=@6(0SDPPpvlYld8={ z4}uT%&fEaE(0O$=kxpWJshn3{Q684nDUP4hq2=<>oFrO=3^GxhJbAs@@#F;58?m=avd(p1w7~ z7Y!KApc1|-W4iezG}@MmRL8hs|Jp93s01fn8l8wA4P~ddhO3Ia6@i z{LhjEN4r%WLhi-sxu+7``2b;~ve(aipd+yI#s>!_4ztW1YTJa{c5p`@2^*;yLy$Q9 z@v65hepBy9QAYmgc}9MBCwW6zOTa5Z^Fo1H_EX|erY}Q#Wuc$;aK&60+0a*ACr@%- z4nNfw6;!rXpz;(aBXlKr1EOVv0%^yW-;)NsJi*{PT=F0d=gqTDd=W#;lvrGTWZeo< zQ$h~k$D-w?%!aBR=NJ++6no9`HNq}Rj%ut`q3oC z9g+-O0jS{nCEZKSKjZ&P5& z2hX#Ppx9?ZNy|mjD(~Yb&--!pbs4DHuV;9kf;q;3j1#EtvmuON&i}!;G&yJYmOI;u zs13T-&$6QRcMe;I&UN%)wwE`CCi9X6m96G1IDtI{l zK8?nF%S#lNCSfVXVnr=_f0j5|2}O_6LMsSf znR?t2uw6;-aupCB^tA~Pe(9x~?#23`ZdBo^>prM8ke4y>|;KKL*z+qu^Q?igX%Basw8*{DVRb)A$Xd+EEsqgM@azvX;)yUstZ z6rB!b%gWSjQ&H^=eGKwsd3IS#u3BcZ$7A%QTaVS5+Lbp9NIeM8JQ!SecKBNHpdXnc zeokiWd%F}S#5h|t3tW3OOl-ZLRmguz=jZ*vG^&fO%;;zbwqDG_VTJm9I4g*0j+}-L zI;PlNyE4%7rB+}lQNS%z+(seeFam8z1O~E`L_u_R z_1NG)fa>9d0II98T?*v{{Us-~0Y-%p5LKzpSOhMx$EO0B07kN%IZ7J=0N@L%uGCV3 zDIc9qKGS{d>*@0+Q|}E_-do#XRF{r`7(wNwoZSBdY|8v*YmUz!pDrXc$-yoZr8~!W zkI@uoQ)Sq?QQsnVKzsa`ErCxCO4vQcyI%OB$0$x{sn~*J;b2GqgZ)PdB?wyyeNT=x zns<9EmqRJ%S3V06ONB#l$Wz~A5Mrf7*(9m~NcvtCLwBF%AylN`^dw(URv^lU;F+-Z zpS=8M;9bGs6^evIt_}qucJEkS5l|Of3NI(tM~b3@hV{Ok0@9DRZ8dHFQ3@eO5D#yC zkU8-on{oC_VrwB7(_~AS%;uIyjsq2F3GTxb|G#{6|3CY&X0aIo?|-rZN-cfmEW5D@ zRZZmd35xGG)=N-K>WzMg@d(<4BdkO&@gu{ajvmgPfPx zxc*Ti=WaR{RxbP83Q7&iHdFH-!lDagR9WhUG7e zAW#Qj74dv(V{+n^SGZ}Cv-w*$rKG3YY7uiaxMEAgW3%6i>7kpEb3czj_}23*6C<%whkaeI|#@SH|%K%x5IE@<#*Rc(&KWG^U$q+6*;Jf8%uS!!4qS z1;vdLZl-GWt^9o5hcFjAw6yBJo1}lleIMPUB*M`XxwDP^a4R#vDGg=h2yez+Y96aK zDf7mX6uSp+F8rGC#H!|qt-0B-?=;&ho%^%b!LL+pbsMZ+zUaI>e5_k5ygD0c@I(# z7^c)*KeCd(62>y!9hu|mU;y}w({Azq z$5@g~+AqAe0zN+=OPW??fJ@Bow_UujJqd0cK)QVFvJ(|hn{k}+gI~lemjyg5nO1jtRF)e;h=1pMd|BsV5?S+q$s z6rMor+&O^oQOE*d^@v~I;{J1V_YMk>c#sZ_-GZO2So+)x75Z(am7=2pkX72-WG#Zz#Ph2 zga|Ny2)fV`M@{Tx+LWmqIV-bL1#9&px?yVed~~ESPjxeXZrKFs$Iaf~<9M6-NZC^{bxkcqFj($m(K1Z5-fE=u2`K zgz{ty5B<)R=UHS%_G$y}6JjvxV!5aX_b~)N0q&8fVN6fvHxB8T@4U`~AUYA#$Iy1>D zY?}oE))pNu>oJEkaoxsfF04XE3Pj*}uHf>O;IyUr(~rdFq{X2&R7AS%whTu$=9+G~ z(o-K_fw1o16^cn5;xip3sQy7-0hJ0qQsb2;+2veD8U7p!Up9Ot20zn)+DfgQLRmSB z4PDQ&D6g`-Ir*!G4qFJ0v>?Q8=N^~3s{B`(pAWIt88u{76Q!zg+gh+x)JINIYNhVf zr-nZ3P(>6e_&@nuq!1*HC{j2L8he=j<&JiuKPa{o!^lLv2@cF%HLGJ&k|4ClYs+ZP zqe}agQS@-IYJ2#x03_Hd&``BOmP+eBxo$_P#Wh$I-vZv=V;3#K_BCHI*sJl`q@E!g z@03E4F${zC<9(Lz;n!B?dPYL0jzkq?2?WfglR89xfo%G8(C$`{mmb@bUQIBko!ZR*d~sfEOfGOYF>}Fc8!#9EOd( z=K5SP+#7q)IhjfG+^T+X%uynGJS1n6AId^2hlVcUrZrI5Q_-lX@kk<>vrOfKo-V1K zk96+{JcLPvi78KZGTOoI2fC@)x2SI|ViiB>^SDbiq)Y21;bvC@Ge3&~>xbe#d%JRp zrsLF2Lp&(^kSmYjsa4(>ef)lNfQMf#*}0qKt4$f&5g1R zNSP5?0&-to&4Hp;>!Qr{l%D*oaoWnF*>2QW(N-%@H+Fy-Kk;S#TZb1P4{Q?{TCK}p zTEjJ9eEiq^(f2tzIc43$8xiU3{&rN5qlVSuOEo0^2O%W)J%=v~Xk7s~E#eu0c_mL_Qlan}n5?VZ|EVPOzqF-&TS)q!*zW#&i~lb~ z;Xm*m_`nCa#f1E`#uY%&5v8Wrz#6~$b*4!0JqS28%+3?>3BaG||6#~Ij`|Yi0;#Ku zss|Q-{ni}79{NCAYib0ao(Y=pU2-(Rd%PU$?^t%Z4D=1W=NA*k2HqyMasw5rk(qjv z(Ao;f+XP8@2~TAfn;w-4>^jVs-HBV-lsCu&{m2~^ZdBd>hsn*%@@{%cWr1nZhnWu( z><9;WN-&1Jr0C!Rb*r`ESF_d%aze#roJWF6z#VWs@6$+XO+$~)6qEXBO-}za1!U6& zIOhyee&IdI5F=U6-Fi zk0gb#wWK|Dum~pT!LZzB16eFhjaTgn6Xu)>1{kQ<*iVELOa0D&lwpIwikp>j($(vd z2xI|w0%-((VUYY5Il;_4%~xUPAM%x- ze*n;lQ|kW-Q-po*KU0|}r5)YAHQtdl@9I(9%v9Iz3mN^I?5eJI{JvGspZyn9@pZm> z+_A`cQloxW;P^=FxTefX*Y29Ajn2KsWCp&O%x~*dlImnxUnJO?f_J)=og~|mbMnc| zf@+FigD$DJAikQg4$(tlM=5jJk**B_%)~QRTR^;huaUTx#g=|v4!kI6<&N`RVU%f% zS|VFh^f7i2vedtKj24O_jHW|gi)~xMJGbTBs)S7O>Y^!f$uVL9ASrx+p?o*GOUReO zKLpULFiJ!LD>}ciBVEVq*BIH@i0%>dOHx+CYQ~JFJGQqBC`^rZBJ+k{H$ZUJ7od}> ziVCa?dUu#w<$P+8d64_bn_a#0SoT_kcSCk_;!_At32o7BRp!6Q_@zGed~teuSEdPp znoxtcD?5$oYHIrDb0603UWC2hX);>=tRO@=+gf1bUbNw&dok6Cz4^cm9LD2s)tgE6 zPW=GAKtcS9@~-LQD{fmI%;HD)5Z3V6+&1Bz?NJ0dO2$YlyNaO5?tjq7g`4FQh+*YY zp&1Wcz?d9G<|kVbp!?6YN>6Q>-B>NHm|83?bZ10VXyG5NobSxyl0y#5Srz`lOo!~k z{n;No&?Rwl>}VY8EHKkWCVB4RiySn6x!9h_pG{;ge7gfa+X8@bZbYI*t}u9QJvwjm zVU&Or$q5n!e&g-Fg3LOg8oL&kY#;mSajEn}2pZn=Zaz85{cn*f%BUL3L%(H}!;>K< zsK(EUa3h`=%FvK>%Y=Vg88lb2;hY)ftXY}e&dcdv*u9urE52UOE7>S2$*&IzRjLv6 z9LIvLjrF=lHGYZq?SGh3)f?wVrHZ}1^{1t%g$aIO%|qDcIzABbswR(Eubc}m?sjF&ePwcSQg zSA6*po>`8Qmu=P_L(j$Qz-{mF#Y!1serx;zhM-1^$QGr2KS)dX;=&i&uiueULWQ9o z+OGG`j$+7!jf!c*oJbNCpZ^gXc~)4g`l-dPEbeRxx^y~*RI**sd0qf>f%N@a^=E=- zs*Kt%WW&%&XdoFxq;ha1GY+MRplV9x401SgQoAxu1WlVACINn4L;wrP?{j%Hz7R4o zJq1#HG592LHLVIIsmwkdluOQ7Qft%}M~F(rY6`3?TdZ|Q7i!54Sxcf7%cY80gvsF2 zr(P^UG}k%I;DqBbgwV}ioKSWJyJN+fU1xr5h2JFN6_{9@BOXzZFeSS%$biC&=s7kE zo~Puh+iYRS-Z;>OdZ(s&5%KCzqT%zWjrb&?j9v5tq#GqE`4=TO)61S4y}d0qN}|x| z)S1~?*YLQ5=r4nYydkc+Z&xF}hwGh>K6sWPi24CNFgJHbnMUpKJjvohbVnA`KUJi% zHBf!D^Zno;>d5-uiRfAl`h5Qiq@Gv@%t9k`6lMo(Gyzf+Cx zyUW8p%9QqshxLJ|B=_iE7*npfz4`K_?Tg2CEBWICewOgpgjR!5InU{#O5bS`PaN_M zWTB9a>LQa>SFiE0^l~b3%)lPvb-MT04v`|#`$11x&H{-1%V6JO+0B=@d5`E_UDuL1 z8vE{RE^pTdu14Q|@`JD(H&F4ueK|U}ee28)CRz3gYp_q^4(0{TX1d`JHGOo^1qH1W zUKMnwn`3)MTWkf)eyyVOv;fAt!>tBQcD^DHbW93bh#+1QW1!_&vDu|(Ui`$l@0ldE z^aOs83kb3ndie_0C+eU<3;lRF!mTG;re{1#nwTppI2GmJ;_Nj>)1mH)ttVELgoVWY znUrted}LoK%oj5{PDa`xv)}TrU0F(Y-+S0rH~tfEB8Qj~*@5acEAm{8t6CF`H_RSio>WGiII}WE9ZQ`VsxRHyDE@i zv}zo@y++!{NVK8Ywkoh?$a06=;fD;3&iF}T^RU7sgGXgYc#7@BMr_gCwekGfj{LWp zICe%Gyd=)u5T+bC7HZZbPi>Sb1xqzdo^={mn$+lPbSayl+BftNw=uF-nBx+-bO$p&0 zH41pt#ad=X(d8GxdmSjGPE{9vGHN*FsuukZA;~=y<3?h5x@i2#Ny}NQGoFA7_kFV} zMRC7x zO?Aw9^NDZfeP`{C8XBo(Kl!FWNT!WQzaxXu;S3K1?s-xu;A@C^=h&YDEUkW+TO-AK zNO<86)WhOedDzy<1b0Z~wsC-FT*$&Nr_hi#_J+^S_JKQ7?{`DF)uH4e`Kb@$PVHzU zlT{{!kX&ZK-uWRxHw8w0O&vDP$hSOsc=$*%J?Fq%9JW#6P)yuQC_~^UA*;+CKO8_< z(BnaMYIRv2pZkngda`b9G|w22;K+ZH;4-F&GN}r@E8ga}pUZL>T(Os! z7B#?dE)UyMQ0SN|v^p0n#et*pwc`Z@J=Dp3=%>(zq5*N6Pn2Y5GUVxDAr$Dy3T6VD z@g5!;GL?Pb!LOf}nKHc5aR)?s7hkgInr-9^8v;{;7U(OP>YVb}85oRy_P!;g!J-|c z3859U_XJYJLut2NRr623^bciS7g#fpp0NFT4AfGF5{+-m5c7j`Os@Nk42@WhP$_=( zjziS)=(?BIUOa^5<@>$*DC+M?ig*W~ot(R2x!Yi8|R zQqt*X_b@mkHL{Qgt=5+nw&XwC_|IK7#$NIeSUhs^(SwzNo;H~XR=fX z; z-|gux{ZFSGJLH!=O0&&>wDjOJQKulSSV1MHPm#LI{ND_ZxOx81^(p!9T7ud9>n?)v z7Oek_7h;&d;10X5@%PqxA6M~9&_x4o_6hYk8fhn)%#q3pz!jx>=W^r>o}x$+N2POR z-p6abl2@NA_dP{ur|wPV%NMSa*-zdkjj zC?^U{j?Jl)ng5Rd1TF=iMcY9JZE=_eX{3=*g3`Sn`)+}Lrz#ES>RUx1Dc8GZsi^1Z z$&haMNDsE|t4N(Hozp~!ck-gV3|r))08O+H>KN#2oMs;d<40U;Thuf|H%_ESRlMsu zq)Tr9I|L`bWKE0yU#q0`?L)_<@cI&FeQoK3J-{X3NJpG!$|F6M4Jo zm+@5euAfvEgw=}P@QMSuP=7mC!JoM6YwcjFR{Nl5xo!t6&9lU?SM#nfJ%3iI|!bDMsQj)G0(V(AB+;tYN5AiPVn0@nr1Y zh@7q`B)OK0++c9Eh+LktEIK-bl*rr7KtxziL@EV?+jMqb_PlF=ILqxvLFVN5rqN+sR>Me##{N7TuA;OhXgqJ)Eq^E?q@@F-C6J$>N)+MlnNVOsBeuL2a4+fmuDJ{)PXuwQK(|oO zE@Ms#Cr@{>AuU`y4l^(^eps9Xucf4h*nr0#j!p~f7tEBo)j*&R426anO-2l^vfWW1 zIe#p?tHd6Bghyi!w~>EuOZ{R%(QYrWJ=}vBU*d;i#Nu5U#puPCbSTEyyX!J~+C;wv zAs7b{^cX_Y9$O9>1`XySSR@{NYjWDHQ~O)BYaxey+SmgV0SWxyP7|m zRd}C*Q@-Z~MX|v1NF|1|50Db+{Zoi{%Hl^Ez0^fJiTIuHMJs~XxkALz0I^kA?MUL( zPwN(wY25fYIZ+gKaCVsUtUF30gR0|!P&B~VEGl)4x;;vp1Yi@=%!kYWYW;ur+itkI zy=IQa;gb6xWI*KoFo%w>I2 z5g6o(Sh(A}MzHy$O>$=QMH>%1IsUnu@`l5MYPeBv?c0qSbr&afMQ);-%yJptfhMQO zi#Yqozw-R8R(Z=lVD;OiEmm~!Hcu@k_56upU7TRK8W3RkSh|Pg`YsV|&a<2VcWOcx z(ygjJ%{I(rdx9E<ZU~RU zekue<-YzzE*u*wM{kmWHyXOvigL0WO^n(5%1-hdY1@}rSHPZp_23znOtt?Pw?p$=g ze!AGXr@*znC7H>0^u9Cf(R*sPc0X12(~u|*-nI8}evd9uEZ~}56BT`Hbo;x`#y{0%+{Tt3-?UnpVlj|xBVaIPiK@Wk8} zbgji0Tq+Z=m-u2m-S3=G^K25dmE|Oql86sO%|$9oDCeH-rb=te@YNCWRUkU@H2&J`%1vvrZH_dr8kv>zOlfcqTAKf80bn5n4-|u_@Z&{z9O& zp~}LpV`30@u=yyWH1?o?c2X4 zIUZZu^P>$ge0yvzR!aPo&TYv7d;dZ{r`Xib4D|_4h z4Ci9aI%*jAaJnGguWI7sva}bNU6#XWaq6&%70tduqYb?fNlX&+jXw=o!3yoWkC&+S znM0*5E+jNDyGGWs&v;{`yCZRfVJ{Di_8Gn~1K_4GW`GIZ<%v z^W-Y`^r$g>_X@_ls5x_DMh2bcx{<_mrMgf|TRS*OH`$v~&;pmc0XP?A3-{a|17W;| zj@#=aLC`ZhnCEV%hweYI01YZsWF&US)Jd+xrez#HlF8}a)KGGx0qz`oWy0tWt}RiJ zvm*!#6Q3@=-zxqawx#=YTH@phVrQZWCy(unPgj^_On)Xtz2I$)ualyAPBepimq4Z5 z@GM`lWPol`KCt{^pS(BqLo68?pIrevRv(EqDM?jYH|{~csz!jU1|1RIRc-Qkw=I88 z>O7BXg+atfyb^P6!-wd!DBbf*b8V>Q!3<(;XN?&p&fDi%O3j!n)Snc7 z@0>~qtk0@pPAzrW;Kr$YRTU9aLcn4Y2DSw};e>xYuZ0goS(`^MjlMPycS$CnH7W>v0H9l=AsYiY7 z&lk?}R+w2iDv^XJR7cIxtnlvx^F4ULKFD8YL`FQxgK0h!E56b-avq^_r^pWZh3sve zLn^!E&pq~Y2U47(vL7BGqxl>RJRugr273m^S{zJ zoFDu`PBKa>@z^<8fLPY{RhxL4ZRS0g9^kREd@<|6@SVtD*Wd#ZH{)^xi?G^T_h-#% z7R|wnQ?=X9kpvob2&)R=K_r*boNTFmP;i==TSoOO%fa5ovzueP7VogkmGAoV=1u0i zP`cl3wg>~jw+t8qWm%FsH8we4OU4fCEz=KJ?w<7%`CZjJ{)J$4?A&qmx*1%nPSIDNOyu#mypqm}zXw zK}hPY0Ywb_1Y%j*k~K*M+7dhw-b<&N7C_p9sacKWtBRq?Hrs+Nt5@ z1!6LGjZ2Ls#UX4(guUeJDKKgfrF(LrSiUNNjJ#>o5l*mnJJ_6VsI!|DpJG5rT=x{% ze`y02AbFNNym3d(D7GtvD2}N3yEAZRUO_KMTG_72wL7l!*x4@%%J+D4tQ(62M@F+~ z$;ah@K)|-}`+_}0w>Q?9v0-VbdAHs<7rXk-I987y8AA=Yhsn7^LZb#W;bd&%SGua;rmyl==g4-c?(Fsr2ks{>%eg3EWB1a2CT1(?uY3ji_Qv-!De2S`=jzh@`+78Tryw6-ByT z$gN+F5ScP??@Qet;r{LA3HA{o7*@aP$9H*Z)f(KnebrhWs~2<}qA^ z;KS_^lr}I#rN>$(g(hAV#NIJ2x@~v45&*cS82+BhQE)2r z3+BGvUV=!d55=w!HzuX2K|m*pU+LYC~}aWT>D)7`pkEE z^&;)*`Me^%{e|-&V*lNhx7>cWhF;JD+@MJZu(NUwS` zob6mFPuz)4UpQQXh)CPTYUtr8+1=n%4B0Zn(U6*!$J`lQjU#W$(uwuByR~KgcjRT7 z^)IhtWhmnoDT5Zanfj9HN>NcfVF6np;kgAFkh2?p8E-^aryn`>@cHRWGd0TOMTVus zVFp*z$N~tT0*RfcR-{yA@xss?TL`Yu0o9|ut4v7A2ABNh%Eze2lFJm2(uI=9>DtRJ zX1e#MRr{MD;`L_jY_3V%&P48gyh`z$(tZ(E&xoy?1U_7bJ${j@_iDoN7(&GoAw>(% zdD3P({)AjO$uD;wS|o-^+Q6>mGELKs^I&++VvjUxygNz|iZNh8>JD#krF_%8eQ#;& z&#!>NhA0EhysMgE!cK?#0l$ScdqUGnJ4n2c-SNF7u4Wq1$xa+LN`&OkTOln&u6c?t zCE3T$AlJ?Xp}V^L-P%zenG+MIB@qkwT5ML);0b%)zWYfcSy4M9RfKJTPPWm4g!_wc zom(Vx?7b7crNp-2+gV|#6F*BaR3O#cr&7&^4|~cQ?>EQ47b) zuYVn);H?WkHr)kb8g|(;(uTQcKhw)33FWf{Y_v}@M*Ht+Q;5W`>pq(gO%DwIB0}$@j^gu7u zUx~U&_Ul(RBB19Oe^5X`Und=TQti>-B*S2B3Zh~545jA%yR|a?>L7=sLux{b`FOO ze^wbYNiu5n6QxKn#8j~5m8Dpveu|&;dvq*3@%Az!-GuK23*tsf9~8wr5+F4S`_WHv zB>X1oc}Xe`D1vO2B#aRX5Co(9{OK0>a0j=yX>z9fOFD5T88 zBftOcr8gBbY#8{|{8^0!E}=(n;2e0l^!JUMclu>kKRy0*dM72VB(9FXy=cdE<}MeD z{Q1!s$6EY8d4;(5#K121(OedY$r|T%Kl z8}U4_I}I5T*XJ5RcjqbUDd-Eu?s4u_SUeLb`fh9Ks=23Q*wZ6|mr!9I#ow$Ic@n2M zlok_ur~2g%ra!?MKJtl>O$u=+BhK%WJ=hvXkEys3Z`&*;Gv|+3uMHF#ovj0i}TO4(~yyhx=kLI2T~@r+dgTbE#;Bh-=ulZ6{G;$ z4$lp5H`mDMR_F+q4hjewnF!{1j9V}?@wBRAm;MW9YbhH{%vc7}tE{h$8R&**l3|%K zPhUklr=EuW_|uQ8J+lXG#aWK7(8|k5np5#e1#@^{5@JbY4U>ZIxYyHZ37XloZL-Ge z51lakgsCx@c}c35Nrf#8D0fI$p_{^g*M-L^_W1_}K3@%iSAa;hNSQKb*I4Daj6oQB zA-)(XM>8t~#c=Shy(q-(Ox%r&2{V``NNjDIt~`CEPX}w9)28ir?07609tNqx%L{_L zM4vdkJ1a^73`06_gavLo@S*5`YwtRvn##g0Dh@aVNGMVyKw^wQ7?36i*hna$${@X& z&=dwKp#?$35)+ybS^@$CqT+xQ6%Yvl3{?Vxh*UwGP?R2;2m|j%=EwW_)?4qb_vigR z_pa}J-?``Bv%kIfO2jgd{-5RHXot9nK3;7p1vZ6HeWAgA_TB0haNVvA#c1L8!lr2Aj7B{-zCZhM8=IkMD@t&l-;|M^(ak!g!R*CnAR!aP&K?-v z{C*u4Bn-cW(Z>j^euNm6;VK0v#}+X6=QHBFjFNqueigbUO7=QQz?t^kzdN{?As~r9ql1fspaPZ8GFiXcn?Th(^^sx8P8^IZ ziUg|hR6c|Xq(rxUM{M?0!ovXpck59ZZ7aMx_-$qj15t4TICI*2KQrI)sJOIPRUW0FnedVN9; zrGhCq`>EmY1_aBP_vh5}TafCPpPiczuvBm-sW`jGH)`d#^r(MfH_%Pp9FcP~@i>Ag zD^9x|b1P8ekLGNcE_{fP4%2tCJuFK&3`&Ba4>v3N0wqV$HZ7*2=ndXF%GCg^Y6%sg z^wIi@M&Gr~W4aNCt$QKx~Ue%kOMbi!u5Pb|-9*Y&2pp z)PpU2h<;K~?=pmYe^&<&=#9#PpjbaM8yU#oM>P27Jrnfar)UkJk6LKI3CHZ~iga=7 zlmkJ;g>bL(qIsKx^vhzl_~gX?%$Tiy#KMi=si-WIylJqlVmBrlNir5DoTakhbL#=H3elozi{9& zMkNf?N``fAJLlG_so%Mu@vPMMQR;3dyfMshMsUe}dXT#wuGSh4GeXOJm9xD;mZWrB zuGA~#%KahtS3?<;tdgs!A0P&g9Q75D^RApkTyVXn1DLR|mU&-E8F~Yq zlbQqBy=nlKxA?Pq-Ji=To z4jiAdP0MoBxCp8V!7Qc%N~Q6o&=8vA^7ZcQj?NNk6_VBlu`BH+zX_P;CTC)}qNGk| zFyagFQRyP{FODh3T@kcY6X~N>y>LUSm*b127uc>{0HL9$y+191Rp8A0bbD|7>U8|r zv3-Zwlt#{%6HneetMK9F7(c;|^;iM6DTE&>br15+@U6w(?edS%TYmt%UX~X|+jm|y zYcCsv6$dSVW)VpO!USK=4BGf}Z+xV-FcKhR2=EiNdhDmc-7@llfgh&aOf5#90IiV# zY;U=eKuI7`?*8aU#NCvxP?*0EDwR9$4P1r?>x zh|5pmxL~hlajFE`rxk0eJBfgTr#`8WIk-jk zaFd81)sJt^BK%|tA;$*7W|z)mtsih`SY|G#Ks*>&Ok~BCEN4p(kCtF(eAGa)w8u2x z*m@O0%7#lhzzOMbtt-4yia0xTqHq1Qr&Z{;xUGTZoLfI1wdVFA#?3;v5cyW2O$ZzH-YH?~1H%p| zm7&(5l{Tsk*dz>3baYTh8wOWDAtKGWsR$`7_XA>=#*xFSr4^OC#YTYYC1O)?UR$U% zx0>|%%tr#TOhmbIYKXy`XordAb%bt^`?UuM1-0#6?8!x!I+37s&hlh#Ohn~D7$ahH{5-#RXowzJ<6iJ0 z@8L%tCHW~Cp%)h|uz7a;slVJZn=`~Rshmn|va9U62CK+Dz$Tw_8+%=+<9OO=Eu(_* za%n#RfB_g2n;tdwKICrOQ)Yd2C9?(AU> z$9lN%XT-=b=THMF(6h39JyJ67m0%uSDaVC}cou5T+A;z|1nHECu`G}U2W}RNr-D=O zW0RczcvEBp3NmPv#0AYgE{$FLaMi3W{mKk|{2PR2^{s5aJeGNmIObq-S9m#S0 zAGA-SLfiL)!jrD?!rM8$aSDi2f%M}F+zsZxWj>TmWQR@L0R(`#55HGpe%Wi^(2&88 zDd^WhdsVWw(u)(Mn7Nl3JbUxxWKrC1C=pT(LBsRLt+@1V!ew?xU42S{K~|skHmFbR zX?38_OcG2v`Z{FKSc-@>|E35@0tLOA-Rdt0JJ9Dy$iB;h?pGO5YK|GbYqnm?^7f9Px z@1#C%y4#Wd%~ui)H`9j}AvH0y;#q89Hs~?sX=U2=VBv_xwuoakslF$Y6 zaGukYAcaaEE~DSsq)8!8*-|Y6l}YTRo)QjWfAo%{Qm^=%>M~n@+3SK8JahG3T~*p8 zvzbs7cLpVuu>Z%Lo~nj71YB~=#_6yC)S$!eEi3=xckm?WQ6O!V0z>0s-RcOW*xvTN z_oXZzbT5Y7{`#HgR?NP9dD+y;9ZsKG6D;YtM*N!^mmegE{~?PhcTDNU@`Z1wE!2s0qble@4~wqq~m zK&QG{z0M6&A&rELd@Jj~1^L|7{f*KnnPym4`o^95Y0EcY{0WwzbR5;-OixNeFtxnf zX=dw+Pk(|4S(){^aZZKkm#*bmox!aQLvd3$@jw51 zR-gS77w9!z8_YTA+POTj`&aWcB*p;`s9Yk}0I1M=g*E8H?VbA8r$4d*@PLK^$F*F1 z|HxVIz47&hnOvURhzBYN%1-y8d)UNpVI>>TOcE7=+Y_UvY&|rIL`ihZk`i>-4>;cY zc-Z8zMu1-}?>N1nF-ot=26UR>%REqlei2hAV6`{iEm>~?=t1HD^ubx*b;Wh=K_dDV zV+sHdM|raz%_F8~?S(NkzqdXFWTVXsbA>(ZthSd;?R&)84eE@chx3mR@(shTEu7%n90h>%E)sLu6h9IMxGa$C{Z#H&qFaH zz>h%xa~?oL`wneO?HgWkq2iq^tX!>s62T`l0W&3V1)zOn`4d5F!aqlg0c)gFy^!D@ z=_|o=Wd66=U%sCaf7&I1NyPq#L%W`h)Z^phqhm4WF9Om3x2E|og|5)jD9tO~Bf&4( f@+WINs@fH4Gw&QfGGrkQB*}+0w#JkhUApxjoZEFm literal 0 HcmV?d00001