Skip to content

Commit

Permalink
ros-controls#191 removed std::shared_ptr from Joint Limits handles
Browse files Browse the repository at this point in the history
  • Loading branch information
@guru-florida
guru-florida committed Oct 11, 2020
1 parent 459fecd commit 7f8c299
Show file tree
Hide file tree
Showing 2 changed files with 140 additions and 133 deletions.
129 changes: 68 additions & 61 deletions joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,24 +65,28 @@ class JointSaturationLimitHandle
public:
JointSaturationLimitHandle()
: prev_pos_(std::numeric_limits<double>::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<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & 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<double>::quiet_NaN()),
prev_vel_(0.0)
{}

JointSaturationLimitHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jvelh,
const std::shared_ptr<hardware_interface::JointHandle> & jcmdh,
const hardware_interface::JointHandle & jposh,
const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits)
: jposh_(jposh),
jvelh_(jvelh),
Expand All @@ -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();
}

Expand All @@ -114,9 +118,9 @@ class JointSaturationLimitHandle
}

protected:
std::shared_ptr<hardware_interface::JointHandle> jposh_;
std::shared_ptr<hardware_interface::JointHandle> jvelh_;
std::shared_ptr<hardware_interface::JointHandle> 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
Expand All @@ -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();
}
};

Expand All @@ -148,18 +152,18 @@ class JointSoftLimitsHandle : public JointSaturationLimitHandle
JointSoftLimitsHandle() {}

JointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jcmdh,
const hardware_interface::JointHandle & jposh,
const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits,
const SoftJointLimits & soft_limits)
: JointSaturationLimitHandle(jposh, jcmdh, limits),
soft_limits_(soft_limits)
{}

JointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jvelh,
const std::shared_ptr<hardware_interface::JointHandle> & 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),
Expand All @@ -179,8 +183,8 @@ class PositionJointSaturationHandle : public JointSaturationLimitHandle
PositionJointSaturationHandle() {}

PositionJointSaturationHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jcmdh,
const hardware_interface::JointHandle & jposh,
const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits)
: JointSaturationLimitHandle(jposh, jcmdh, limits)
{
Expand All @@ -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;
Expand All @@ -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;
}
Expand Down Expand Up @@ -267,8 +271,8 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle
{}

PositionJointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
Expand All @@ -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_;

Expand Down Expand Up @@ -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();
}
};

Expand All @@ -354,9 +358,9 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle
EffortJointSaturationHandle() {}

EffortJointSaturationHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jvelh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
{
Expand All @@ -373,10 +377,10 @@ class EffortJointSaturationHandle : public JointSaturationLimitHandle
}

EffortJointSaturationHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
{
}

Expand All @@ -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) {
Expand All @@ -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);
}
};

Expand All @@ -421,9 +425,9 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle
EffortJointSoftLimitsHandle() {}

EffortJointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jvelh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
Expand All @@ -441,11 +445,11 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle
}

EffortJointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
{
}

Expand All @@ -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
Expand Down Expand Up @@ -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);
}
};

Expand All @@ -511,10 +515,10 @@ class VelocityJointSaturationHandle : public JointSaturationLimitHandle
VelocityJointSaturationHandle() {}

VelocityJointSaturationHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jvelh, // currently unused
const std::shared_ptr<hardware_interface::JointHandle> & 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(
Expand All @@ -524,9 +528,12 @@ class VelocityJointSaturationHandle : public JointSaturationLimitHandle
}

VelocityJointSaturationHandle(
const std::shared_ptr<hardware_interface::JointHandle> & 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(
Expand Down Expand Up @@ -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();
}
};

Expand All @@ -577,9 +584,9 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle
VelocityJointSoftLimitsHandle() {}

VelocityJointSoftLimitsHandle(
const std::shared_ptr<hardware_interface::JointHandle> & jposh,
const std::shared_ptr<hardware_interface::JointHandle> & jvelh,
const std::shared_ptr<hardware_interface::JointHandle> & 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)
Expand All @@ -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_);
Expand All @@ -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:
Expand Down
Loading

0 comments on commit 7f8c299

Please sign in to comment.