54 T
saturate(
const T val,
const T min_val,
const T max_val)
56 return std::min(std::max(val, min_val), max_val);
71 if (limits_.has_position_limits)
73 min_pos_limit_ = limits_.min_position;
74 max_pos_limit_ = limits_.max_position;
78 min_pos_limit_ = -std::numeric_limits<double>::max();
79 max_pos_limit_ = std::numeric_limits<double>::max();
84 std::string
getName()
const {
return jh_.getName();}
93 if (std::isnan(prev_cmd_))
94 prev_cmd_ = jh_.getPosition();
96 double min_pos, max_pos;
97 if (limits_.has_velocity_limits)
99 const double delta_pos = limits_.max_velocity * period.
toSec();
100 min_pos = std::max(prev_cmd_ - delta_pos, min_pos_limit_);
101 max_pos = std::min(prev_cmd_ + delta_pos, max_pos_limit_);
105 min_pos = min_pos_limit_;
106 max_pos = max_pos_limit_;
118 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
126 double prev_cmd_ = {std::numeric_limits<double>::quiet_NaN()};
170 soft_limits_(soft_limits)
175 "'. It has no velocity limits specification.");
180 std::string
getName()
const {
return jh_.getName();}
190 assert(period.
toSec() > 0.0);
196 if (std::isnan(prev_cmd_)) {prev_cmd_ = jh_.getPosition();}
197 const double pos = prev_cmd_;
203 if (limits_.has_position_limits)
206 soft_min_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
207 -limits_.max_velocity,
208 limits_.max_velocity);
210 soft_max_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
211 -limits_.max_velocity,
212 limits_.max_velocity);
217 soft_min_vel = -limits_.max_velocity;
218 soft_max_vel = limits_.max_velocity;
222 const double dt = period.
toSec();
223 double pos_low = pos + soft_min_vel * dt;
224 double pos_high = pos + soft_max_vel * dt;
226 if (limits_.has_position_limits)
229 pos_low = std::max(pos_low, limits_.min_position);
230 pos_high = std::min(pos_high, limits_.max_position);
234 const double pos_cmd =
saturate(jh_.getCommand(),
237 jh_.setCommand(pos_cmd);
240 prev_cmd_ = jh_.getCommand();
247 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
255 double prev_cmd_ = {std::numeric_limits<double>::quiet_NaN()};
270 "'. It has no velocity limits specification.");
275 "'. It has no efforts limits specification.");
280 std::string
getName()
const {
return jh_.getName();}
287 double min_eff = -limits_.max_effort;
288 double max_eff = limits_.max_effort;
290 if (limits_.has_position_limits)
292 const double pos = jh_.getPosition();
293 if (pos < limits_.min_position)
295 else if (pos > limits_.max_position)
299 const double vel = jh_.getVelocity();
300 if (vel < -limits_.max_velocity)
302 else if (vel > limits_.max_velocity)
326 soft_limits_(soft_limits)
331 "'. It has no velocity limits specification.");
336 "'. It has no effort limits specification.");
341 std::string
getName()
const {
return jh_.getName();}
353 const double pos = jh_.getPosition();
354 const double vel = jh_.getVelocity();
360 if (limits_.has_position_limits)
363 soft_min_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
364 -limits_.max_velocity,
365 limits_.max_velocity);
367 soft_max_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
368 -limits_.max_velocity,
369 limits_.max_velocity);
374 soft_min_vel = -limits_.max_velocity;
375 soft_max_vel = limits_.max_velocity;
379 const double soft_min_eff =
saturate(-soft_limits_.k_velocity * (vel - soft_min_vel),
383 const double soft_max_eff =
saturate(-soft_limits_.k_velocity * (vel - soft_max_vel),
388 const double eff_cmd =
saturate(jh_.getCommand(),
391 jh_.setCommand(eff_cmd);
414 "'. It has no velocity limits specification.");
419 std::string
getName()
const {
return jh_.getName();}
433 if (limits_.has_acceleration_limits)
435 assert(period.
toSec() > 0.0);
436 const double dt = period.
toSec();
438 vel_low = std::max(prev_cmd_ - limits_.max_acceleration * dt, -limits_.max_velocity);
439 vel_high = std::min(prev_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity);
443 vel_low = -limits_.max_velocity;
444 vel_high = limits_.max_velocity;
448 const double vel_cmd =
saturate(jh_.getCommand(),
451 jh_.setCommand(vel_cmd);
454 prev_cmd_ = jh_.getCommand();
461 double prev_cmd_ = {0.0};
472 , soft_limits_(soft_limits)
477 max_vel_limit_ = std::numeric_limits<double>::max();
481 std::string
getName()
const {
return jh_.getName();}
492 double min_vel, max_vel;
493 if (limits_.has_position_limits)
496 const double pos = jh_.getPosition();
497 min_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
498 -max_vel_limit_, max_vel_limit_);
499 max_vel =
saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
500 -max_vel_limit_, max_vel_limit_);
504 min_vel = -max_vel_limit_;
505 max_vel = max_vel_limit_;
508 if (limits_.has_acceleration_limits)
510 const double vel = jh_.getVelocity();
511 const double delta_t = period.
toSec();
512 min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel);
513 max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel);
516 jh_.setCommand(
saturate(jh_.getCommand(), min_vel, max_vel));
535 template <
class HandleType>
546 catch(
const std::logic_error& e)
557 for (
auto&& resource_name_and_handle : this->resource_map_)
559 resource_name_and_handle.second.enforceLimits(period);
573 for (
auto&& resource_name_and_handle : this->resource_map_)
575 resource_name_and_handle.second.reset();
589 for (
auto&& resource_name_and_handle : this->resource_map_)
591 resource_name_and_handle.second.reset();
std::string getName() const
VelocityJointSaturationHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits)
void enforceLimits(const ros::Duration &)
Enforce position, velocity, and effort limits for a joint that is not subject to soft limits...
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
PositionJointSaturationHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits)
void reset()
Reset all managed handles.
hardware_interface::JointHandle jh_
PositionJointSoftLimitsHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits, const SoftJointLimits &soft_limits)
A handle used to enforce position and velocity limits of a position-controlled joint that does not ha...
hardware_interface::JointHandle jh_
void enforceLimits(const ros::Duration &period)
Enforce limits for all managed handles.
A handle used to enforce position, velocity, and effort limits of an effort-controlled joint that doe...
std::string getName() const
SoftJointLimits soft_limits_
hardware_interface::JointHandle jh_
void enforceLimits(const ros::Duration &period)
Enforce position, velocity, and acceleration limits for a velocity-controlled joint subject to soft l...
A handle used to enforce position, velocity, and acceleration limits of a velocity-controlled joint...
ResourceHandle getHandle(const std::string &name)
void reset()
Reset state, in case of mode switch or e-stop.
std::string getName() const
VelocityJointSoftLimitsHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits, const SoftJointLimits &soft_limits)
SoftJointLimits soft_limits_
hardware_interface::JointHandle jh_
std::string getName() const
void enforceLimits(const ros::Duration &period)
Enforce joint velocity and acceleration limits.
EffortJointSoftLimitsHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits, const SoftJointLimits &soft_limits)
HandleType getHandle(const std::string &name)
A handle used to enforce position and velocity limits of a position-controlled joint.
hardware_interface::JointHandle jh_
std::string getName() const
An exception related to a JointLimitsInterface.
SoftJointLimits soft_limits_
Interface for enforcing joint limits.
A handle used to enforce velocity and acceleration limits of a velocity-controlled joint...
void reset()
Reset state, in case of mode switch or e-stop.
void enforceLimits(const ros::Duration &)
Enforce position, velocity and effort limits for a joint subject to soft limits.
T saturate(const T val, const T min_val, const T max_val)
A handle used to enforce position, velocity and effort limits of an effort-controlled joint...
void enforceLimits(const ros::Duration &period)
Enforce position and velocity limits for a joint subject to soft limits.
std::string getName() const
void reset()
Reset all managed handles.
hardware_interface::JointHandle jh_
EffortJointSaturationHandle(const hardware_interface::JointHandle &jh, const JointLimits &limits)
void enforceLimits(const ros::Duration &period)
Enforce position and velocity limits for a joint that is not subject to soft limits.