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 42271aec8a..769aeb3444 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 @@ -65,14 +65,18 @@ class JointSaturationLimitHandle public: JointSaturationLimitHandle() : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) + prev_vel_(0.0), + jposh_(std::string(), "position"), + jvelh_(std::string(), "velocity"), + jcmdh_(std::string(), "position_command") {} JointSaturationLimitHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) : jposh_(jposh), + jvelh_(std::string(), "velocity"), jcmdh_(jcmdh), limits_(limits), prev_pos_(std::numeric_limits::quiet_NaN()), @@ -80,9 +84,9 @@ class JointSaturationLimitHandle {} JointSaturationLimitHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jvelh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jvelh, + const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) : jposh_(jposh), jvelh_(jvelh), @@ -95,9 +99,9 @@ class JointSaturationLimitHandle /** \return Joint name. */ std::string get_name() const { - return jposh_ ? jposh_->get_name() : - jvelh_ ? jvelh_->get_name() : - jcmdh_ ? jcmdh_->get_name() : + return jposh_ ? jposh_.get_name() : + jvelh_ ? jvelh_.get_name() : + jcmdh_ ? jcmdh_.get_name() : std::string(); } @@ -114,9 +118,9 @@ class JointSaturationLimitHandle } protected: - std::shared_ptr jposh_; - std::shared_ptr jvelh_; - std::shared_ptr jcmdh_; + hardware_interface::JointHandle jposh_; + hardware_interface::JointHandle jvelh_; + hardware_interface::JointHandle jcmdh_; joint_limits_interface::JointLimits limits_; // stored state - track position and velocity of last update @@ -133,8 +137,8 @@ class JointSaturationLimitHandle // 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(); + jvelh_.get_value() : + (jposh_.get_value() - prev_pos_) / period.seconds(); } }; @@ -148,8 +152,8 @@ class JointSoftLimitsHandle : public JointSaturationLimitHandle JointSoftLimitsHandle() {} JointSoftLimitsHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const JointLimits & limits, const SoftJointLimits & soft_limits) : JointSaturationLimitHandle(jposh, jcmdh, limits), @@ -157,9 +161,9 @@ class JointSoftLimitsHandle : public JointSaturationLimitHandle {} JointSoftLimitsHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jvelh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jvelh, + const hardware_interface::JointHandle & jcmdh, const JointLimits & limits, const SoftJointLimits & soft_limits) : JointSaturationLimitHandle(jposh, jvelh, jcmdh, limits), @@ -179,8 +183,8 @@ class PositionJointSaturationHandle : public JointSaturationLimitHandle PositionJointSaturationHandle() {} PositionJointSaturationHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) : JointSaturationLimitHandle(jposh, jcmdh, limits) { @@ -201,7 +205,7 @@ class PositionJointSaturationHandle : public JointSaturationLimitHandle void enforce_limits(const rclcpp::Duration & period) { if (std::isnan(prev_pos_)) { - prev_pos_ = jposh_->get_value(); + prev_pos_ = jposh_.get_value(); } double min_pos, max_pos; @@ -219,8 +223,8 @@ class PositionJointSaturationHandle : public JointSaturationLimitHandle } // 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(jcmdh_.get_value(), min_pos, max_pos); + jcmdh_.set_value(cmd); prev_pos_ = cmd; } @@ -267,8 +271,8 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle {} PositionJointSoftLimitsHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) @@ -294,7 +298,7 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle // Current position if (std::isnan(prev_pos_)) { // Happens only once at initialization - prev_pos_ = jposh_->get_value(); + prev_pos_ = jposh_.get_value(); } const double pos = prev_pos_; @@ -333,15 +337,15 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle // Saturate position command according to bounds const double pos_cmd = rcppmath::clamp( - jcmdh_->get_value(), + jcmdh_.get_value(), pos_low, pos_high); - jcmdh_->set_value(pos_cmd); + 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(); + prev_pos_ = jcmdh_.get_value(); } }; @@ -354,9 +358,9 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle EffortJointSaturationHandle() {} EffortJointSaturationHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jvelh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jvelh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits) : JointSaturationLimitHandle(jposh, jvelh, jcmdh, limits) { @@ -373,10 +377,10 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle } EffortJointSaturationHandle( - const std::shared_ptr & jh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle(jh, nullptr, jcmdh, limits) + : EffortJointSaturationHandle(jposh, {std::string(), "velocity"}, jcmdh, limits) { } @@ -390,7 +394,7 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle double max_eff = limits_.max_effort; if (limits_.has_position_limits) { - const double pos = jposh_->get_value(); + const double pos = jposh_.get_value(); if (pos < limits_.min_position) { min_eff = 0.0; } else if (pos > limits_.max_position) { @@ -405,8 +409,8 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle max_eff = 0.0; } - double clamped = rcppmath::clamp(jcmdh_->get_value(), min_eff, max_eff); - jcmdh_->set_value(clamped); + double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); + jcmdh_.set_value(clamped); } }; @@ -421,9 +425,9 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle EffortJointSoftLimitsHandle() {} EffortJointSoftLimitsHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jvelh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jvelh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) @@ -441,11 +445,11 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle } EffortJointSoftLimitsHandle( - const std::shared_ptr & jh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle(jh, nullptr, jcmdh, limits, soft_limits) + : EffortJointSoftLimitsHandle(jposh, {std::string(), "velocity"}, jcmdh, limits, soft_limits) { } @@ -458,7 +462,7 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle void enforce_limits(const rclcpp::Duration & period) override { // Current state - const double pos = jposh_->get_value(); + const double pos = jposh_.get_value(); const double vel = get_velocity(period); // Velocity bounds @@ -495,10 +499,10 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle // Saturate effort command according to bounds const double eff_cmd = rcppmath::clamp( - jcmdh_->get_value(), + jcmdh_.get_value(), soft_min_eff, soft_max_eff); - jcmdh_->set_value(eff_cmd); + jcmdh_.set_value(eff_cmd); } }; @@ -511,10 +515,10 @@ class VelocityJointSaturationHandle : public JointSaturationLimitHandle VelocityJointSaturationHandle() {} VelocityJointSaturationHandle( - const std::shared_ptr & jvelh, // currently unused - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jvelh, // currently unused + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits) - : JointSaturationLimitHandle(nullptr, jvelh, jcmdh, limits) + : JointSaturationLimitHandle({std::string(), "position"}, jvelh, jcmdh, limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -524,9 +528,12 @@ class VelocityJointSaturationHandle : public JointSaturationLimitHandle } VelocityJointSaturationHandle( - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits) - : JointSaturationLimitHandle(nullptr, nullptr, jcmdh, limits) + : JointSaturationLimitHandle( + {std::string(), "position"}, + {std::string(), "velocity"}, + jcmdh, limits) { if (!limits.has_velocity_limits) { throw joint_limits_interface::JointLimitsInterfaceException( @@ -558,13 +565,13 @@ class VelocityJointSaturationHandle : public JointSaturationLimitHandle // Saturate velocity command according to limits const double vel_cmd = rcppmath::clamp( - jcmdh_->get_value(), + jcmdh_.get_value(), vel_low, vel_high); - jcmdh_->set_value(vel_cmd); + jcmdh_.set_value(vel_cmd); // Cache variables - prev_vel_ = jcmdh_->get_value(); + prev_vel_ = jcmdh_.get_value(); } }; @@ -577,9 +584,9 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle VelocityJointSoftLimitsHandle() {} VelocityJointSoftLimitsHandle( - const std::shared_ptr & jposh, - const std::shared_ptr & jvelh, - const std::shared_ptr & jcmdh, + const hardware_interface::JointHandle & jposh, + const hardware_interface::JointHandle & jvelh, + const hardware_interface::JointHandle & jcmdh, const joint_limits_interface::JointLimits & limits, const joint_limits_interface::SoftJointLimits & soft_limits) : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) @@ -602,7 +609,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 = jposh_.get_value(); min_vel = rcppmath::clamp( -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, max_vel_limit_); @@ -621,7 +628,7 @@ 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)); + jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); } private: diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp index 43b4c1504f..456b43b427 100644 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ b/joint_limits_interface/test/joint_limits_interface_test.cpp @@ -81,10 +81,10 @@ class JointLimitsTest : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), name("joint_name"), period(0, 100000000), - cmd_handle(std::make_shared(name, "position_command", &cmd)), - pos_handle(std::make_shared(name, "position", &pos)), - vel_handle(std::make_shared(name, "velocity", &vel)), - eff_handle(std::make_shared(name, "effort", &eff)) + cmd_handle(hardware_interface::JointHandle(name, "position_command", &cmd)), + pos_handle(hardware_interface::JointHandle(name, "position", &pos)), + vel_handle(hardware_interface::JointHandle(name, "velocity", &vel)), + eff_handle(hardware_interface::JointHandle(name, "effort", &eff)) { limits.has_position_limits = true; limits.min_position = -1.0; @@ -107,8 +107,8 @@ class JointLimitsTest double pos, vel, eff, cmd; std::string name; rclcpp::Duration period; - std::shared_ptr cmd_handle; - std::shared_ptr pos_handle, vel_handle, eff_handle; + 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; }; @@ -213,17 +213,17 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = max_increment / 2.0; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = -max_increment / 2.0; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); } // Move at maximum velocity @@ -231,17 +231,17 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = max_increment; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = -max_increment; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); } // Try to move faster than the maximum velocity, enforce velocity limits @@ -249,17 +249,17 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = 2.0 * max_increment; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, cmd_handle->get_value(), EPS); + EXPECT_NEAR(max_increment, cmd_handle.get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( pos_handle, cmd_handle, limits, soft_limits); cmd = -2.0 * max_increment; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, cmd_handle->get_value(), EPS); + EXPECT_NEAR(-max_increment, cmd_handle.get_value(), EPS); } } @@ -278,15 +278,15 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // 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); + cmd_handle.set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle->get_value(), cmd_handle->get_value(), EPS); + EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle->set_value(workspace_center); + cmd_handle.set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); } // Current position == lower soft limit @@ -297,15 +297,15 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // 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); + cmd_handle.set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle->get_value(), cmd_handle->get_value(), EPS); + EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle->set_value(workspace_center); + cmd_handle.set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); } // Current position > upper soft limit @@ -317,15 +317,15 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // 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); + cmd_handle.set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle->set_value(workspace_center); + cmd_handle.set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); } // Current position < lower soft limit @@ -337,15 +337,15 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // 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); + cmd_handle.set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle->set_value(workspace_center); + cmd_handle.set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle->get_value(), cmd_handle->get_value()); + EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); } } @@ -366,9 +366,9 @@ TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) // On hard limit pos = limits.max_position; // Way beyond hard limit - cmd_handle->set_value(2.0 * limits.max_position); + cmd_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, cmd_handle.get_value(), EPS); } // Current position == lower hard limit @@ -380,9 +380,9 @@ TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) // On hard limit pos = limits.min_position; // Way beyond hard limit - cmd_handle->set_value(2.0 * limits.min_position); + cmd_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, cmd_handle.get_value(), EPS); } } @@ -399,36 +399,36 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) // Velocity within bounds cmd = limits.max_velocity / 2.0; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); cmd = -limits.max_velocity / 2.0; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); // Velocity at bounds cmd = limits.max_velocity; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); cmd = -limits.max_velocity; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle->get_value(), EPS); + EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); // Velocity beyond bounds cmd = 2.0 * limits.max_velocity; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, cmd_handle->get_value(), EPS); + EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); cmd = -2.0 * limits.max_velocity; - cmd_handle->set_value(cmd); + cmd_handle.set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, cmd_handle->get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); } TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) @@ -446,57 +446,57 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) // Positive velocity // register last command - cmd_handle->set_value(limits.max_velocity / 2.0); + cmd_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); + cmd_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, cmd_handle.get_value(), EPS); // register last command - cmd_handle->set_value(limits.max_velocity / 2.0); + cmd_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); + cmd_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, cmd_handle.get_value(), EPS); // Negative velocity // register last command - cmd_handle->set_value(-limits.max_velocity / 2.0); + cmd_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); + cmd_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, cmd_handle.get_value(), EPS); // register last command - cmd_handle->set_value(-limits.max_velocity / 2.0); + cmd_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); + cmd_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, cmd_handle.get_value(), EPS); } class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test @@ -556,10 +556,10 @@ 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_handle.set_value(limits.max_position); // cmd_handle2.set_cmd(limits.max_position); // iface.enforce_limits(period); -// EXPECT_GT(pos_handle->get_value(), cmd_handle->get_value()); +// EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); // EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); // } // @@ -577,17 +577,17 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) const double max_increment = period.seconds() * limits.max_velocity; - cmd_handle->set_value(limits.max_position); + cmd_handle.set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle->get_value(), max_increment, EPS); + EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); iface.reset(); pos = limits.max_position; - cmd_handle->set_value(limits.max_position); + cmd_handle.set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle->get_value(), limits.max_position, EPS); + EXPECT_NEAR(cmd_handle.get_value(), limits.max_position, EPS); } #endif @@ -603,17 +603,17 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) // // const double max_increment = period.seconds() * limits.max_velocity; // -// cmd_handle->set_value(limits.max_position); +// cmd_handle.set_value(limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle->get_value(), max_increment, EPS); +// EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); // // iface.reset(); // pos = limits.max_position; -// cmd_handle->set_value(soft_limits.max_position); +// cmd_handle.set_value(soft_limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle->get_value(), soft_limits.max_position, EPS); +// EXPECT_NEAR(cmd_handle.get_value(), soft_limits.max_position, EPS); // // }