00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00028
00030
00031 #ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_INTERFACE_H
00032 #define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_INTERFACE_H
00033
00034 #include <algorithm>
00035 #include <cassert>
00036 #include <cmath>
00037 #include <limits>
00038
00039 #include <boost/shared_ptr.hpp>
00040
00041 #include <ros/duration.h>
00042
00043 #include <hardware_interface/internal/resource_manager.h>
00044 #include <hardware_interface/joint_command_interface.h>
00045
00046 #include <joint_limits_interface/joint_limits.h>
00047 #include <joint_limits_interface/joint_limits_interface_exception.h>
00048
00049 namespace joint_limits_interface
00050 {
00051
00052 namespace internal
00053 {
00054
00055 template<typename T>
00056 T saturate(const T val, const T min_val, const T max_val)
00057 {
00058 return std::min(std::max(val, min_val), max_val);
00059 }
00060
00061 }
00062
00065 class PositionJointSaturationHandle
00066 {
00067 public:
00068 PositionJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00069 {
00070 jh_ = jh;
00071 limits_ = limits;
00072
00073 if (limits_.has_position_limits)
00074 {
00075 min_pos_limit_ = limits_.min_position;
00076 max_pos_limit_ = limits_.max_position;
00077 }
00078 else
00079 {
00080 min_pos_limit_ = -std::numeric_limits<double>::max();
00081 max_pos_limit_ = std::numeric_limits<double>::max();
00082 }
00083
00084 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00085 }
00086
00088 std::string getName() const {return jh_.getName();}
00089
00095 void enforceLimits(const ros::Duration& period)
00096 {
00097 if (std::isnan(prev_cmd_))
00098 prev_cmd_ = jh_.getPosition();
00099
00100 double min_pos, max_pos;
00101 if (limits_.has_velocity_limits)
00102 {
00103 const double delta_pos = limits_.max_velocity * period.toSec();
00104 min_pos = std::max(prev_cmd_ - delta_pos, min_pos_limit_);
00105 max_pos = std::min(prev_cmd_ + delta_pos, max_pos_limit_);
00106 }
00107 else
00108 {
00109 min_pos = min_pos_limit_;
00110 max_pos = max_pos_limit_;
00111 }
00112
00113 const double cmd = internal::saturate(jh_.getCommand(), min_pos, max_pos);
00114 jh_.setCommand(cmd);
00115 prev_cmd_ = cmd;
00116 }
00117
00118 private:
00119 hardware_interface::JointHandle jh_;
00120 JointLimits limits_;
00121 double min_pos_limit_, max_pos_limit_;
00122 double prev_cmd_;
00123 };
00124
00155
00156 class PositionJointSoftLimitsHandle
00157 {
00158 public:
00159 PositionJointSoftLimitsHandle()
00160 : prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00161 {}
00162
00163 PositionJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00164 const JointLimits& limits,
00165 const SoftJointLimits& soft_limits)
00166 : jh_(jh),
00167 limits_(limits),
00168 soft_limits_(soft_limits),
00169 prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00170 {
00171 if (!limits.has_velocity_limits)
00172 {
00173 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00174 "'. It has no velocity limits specification.");
00175 }
00176 }
00177
00179 std::string getName() const {return jh_.getName();}
00180
00187 void enforceLimits(const ros::Duration& period)
00188 {
00189 assert(period.toSec() > 0.0);
00190
00191 using internal::saturate;
00192
00193
00194
00195 if (std::isnan(prev_cmd_)) {prev_cmd_ = jh_.getPosition();}
00196 const double pos = prev_cmd_;
00197
00198
00199 double soft_min_vel;
00200 double soft_max_vel;
00201
00202 if (limits_.has_position_limits)
00203 {
00204
00205 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00206 -limits_.max_velocity,
00207 limits_.max_velocity);
00208
00209 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00210 -limits_.max_velocity,
00211 limits_.max_velocity);
00212 }
00213 else
00214 {
00215
00216 soft_min_vel = -limits_.max_velocity;
00217 soft_max_vel = limits_.max_velocity;
00218 }
00219
00220
00221 const double dt = period.toSec();
00222 double pos_low = pos + soft_min_vel * dt;
00223 double pos_high = pos + soft_max_vel * dt;
00224
00225 if (limits_.has_position_limits)
00226 {
00227
00228 pos_low = std::max(pos_low, limits_.min_position);
00229 pos_high = std::min(pos_high, limits_.max_position);
00230 }
00231
00232
00233 const double pos_cmd = saturate(jh_.getCommand(),
00234 pos_low,
00235 pos_high);
00236 jh_.setCommand(pos_cmd);
00237
00238
00239 prev_cmd_ = jh_.getCommand();
00240 }
00241
00242 private:
00243 hardware_interface::JointHandle jh_;
00244 JointLimits limits_;
00245 SoftJointLimits soft_limits_;
00246
00247 double prev_cmd_;
00248 };
00249
00252 class EffortJointSaturationHandle
00253 {
00254 public:
00255 EffortJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00256 {
00257 jh_ = jh;
00258 limits_ = limits;
00259 }
00260
00262 std::string getName() const {return jh_.getName();}
00263
00267 void enforceLimits(const ros::Duration& )
00268 {
00269 double min_eff, max_eff;
00270 if (limits_.has_effort_limits)
00271 {
00272 min_eff = -limits_.max_effort;
00273 max_eff = limits_.max_effort;
00274 }
00275 else
00276 {
00277 min_eff = -std::numeric_limits<double>::max();
00278 max_eff = std::numeric_limits<double>::max();
00279 }
00280
00281 if (limits_.has_position_limits)
00282 {
00283 const double pos = jh_.getPosition();
00284 if (pos < limits_.min_position)
00285 min_eff = 0;
00286 else if (pos > limits_.max_position)
00287 max_eff = 0;
00288 }
00289
00290 if (limits_.has_velocity_limits)
00291 {
00292 const double vel = jh_.getVelocity();
00293 if (vel < -limits_.max_velocity)
00294 min_eff = 0;
00295 else if (vel > limits_.max_velocity)
00296 max_eff = 0;
00297 }
00298
00299 jh_.setCommand(internal::saturate(jh_.getCommand(), min_eff, max_eff));
00300 }
00301
00302 private:
00303 hardware_interface::JointHandle jh_;
00304 JointLimits limits_;
00305 };
00306
00309
00310 class EffortJointSoftLimitsHandle
00311 {
00312 public:
00313 EffortJointSoftLimitsHandle() {}
00314
00315 EffortJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00316 const JointLimits& limits,
00317 const SoftJointLimits& soft_limits)
00318 : jh_(jh),
00319 limits_(limits),
00320 soft_limits_(soft_limits)
00321 {
00322 if (!limits.has_velocity_limits)
00323 {
00324 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00325 "'. It has no velocity limits specification.");
00326 }
00327 if (!limits.has_effort_limits)
00328 {
00329 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00330 "'. It has no effort limits specification.");
00331 }
00332 }
00333
00335 std::string getName() const {return jh_.getName();}
00336
00342 void enforceLimits(const ros::Duration& )
00343 {
00344 using internal::saturate;
00345
00346
00347 const double pos = jh_.getPosition();
00348 const double vel = jh_.getVelocity();
00349
00350
00351 double soft_min_vel;
00352 double soft_max_vel;
00353
00354 if (limits_.has_position_limits)
00355 {
00356
00357 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00358 -limits_.max_velocity,
00359 limits_.max_velocity);
00360
00361 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00362 -limits_.max_velocity,
00363 limits_.max_velocity);
00364 }
00365 else
00366 {
00367
00368 soft_min_vel = -limits_.max_velocity;
00369 soft_max_vel = limits_.max_velocity;
00370 }
00371
00372
00373 const double soft_min_eff = saturate(-soft_limits_.k_velocity * (vel - soft_min_vel),
00374 -limits_.max_effort,
00375 limits_.max_effort);
00376
00377 const double soft_max_eff = saturate(-soft_limits_.k_velocity * (vel - soft_max_vel),
00378 -limits_.max_effort,
00379 limits_.max_effort);
00380
00381
00382 const double eff_cmd = saturate(jh_.getCommand(),
00383 soft_min_eff,
00384 soft_max_eff);
00385 jh_.setCommand(eff_cmd);
00386 }
00387
00388 private:
00389 hardware_interface::JointHandle jh_;
00390 JointLimits limits_;
00391 SoftJointLimits soft_limits_;
00392 };
00393
00394
00396 class VelocityJointSaturationHandle
00397 {
00398 public:
00399 VelocityJointSaturationHandle () {}
00400
00401 VelocityJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00402 : jh_(jh),
00403 limits_(limits)
00404 {
00405 if (!limits.has_velocity_limits)
00406 {
00407 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00408 "'. It has no velocity limits specification.");
00409 }
00410 }
00411
00413 std::string getName() const {return jh_.getName();}
00414
00419 void enforceLimits(const ros::Duration& period)
00420 {
00421 using internal::saturate;
00422
00423
00424 double vel_low;
00425 double vel_high;
00426
00427 if (limits_.has_acceleration_limits)
00428 {
00429 assert(period.toSec() > 0.0);
00430 const double vel = jh_.getVelocity();
00431 const double dt = period.toSec();
00432
00433 vel_low = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity);
00434 vel_high = std::min(vel + limits_.max_acceleration * dt, limits_.max_velocity);
00435 }
00436 else
00437 {
00438 vel_low = -limits_.max_velocity;
00439 vel_high = limits_.max_velocity;
00440 }
00441
00442
00443 const double vel_cmd = saturate(jh_.getCommand(),
00444 vel_low,
00445 vel_high);
00446 jh_.setCommand(vel_cmd);
00447 }
00448
00449 private:
00450 hardware_interface::JointHandle jh_;
00451 JointLimits limits_;
00452 };
00453
00455 class VelocityJointSoftLimitsHandle
00456 {
00457 public:
00458 VelocityJointSoftLimitsHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits,
00459 const SoftJointLimits& soft_limits)
00460 {
00461 jh_ = jh;
00462 limits_ = limits;
00463 soft_limits_ = soft_limits;
00464 if (limits.has_velocity_limits)
00465 max_vel_limit_ = limits.max_velocity;
00466 else
00467 max_vel_limit_ = std::numeric_limits<double>::max();
00468 }
00469
00471 std::string getName() const {return jh_.getName();}
00472
00478 void enforceLimits(const ros::Duration& period)
00479 {
00480 using internal::saturate;
00481
00482 double min_vel, max_vel;
00483 if (limits_.has_position_limits)
00484 {
00485
00486 const double pos = jh_.getPosition();
00487 min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00488 -max_vel_limit_, max_vel_limit_);
00489 max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00490 -max_vel_limit_, max_vel_limit_);
00491 }
00492 else
00493 {
00494 min_vel = -max_vel_limit_;
00495 max_vel = max_vel_limit_;
00496 }
00497
00498 if (limits_.has_acceleration_limits)
00499 {
00500 const double vel = jh_.getVelocity();
00501 const double delta_t = period.toSec();
00502 min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel);
00503 max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel);
00504 }
00505
00506 jh_.setCommand(saturate(jh_.getCommand(), min_vel, max_vel));
00507 }
00508
00509 private:
00510 hardware_interface::JointHandle jh_;
00511 JointLimits limits_;
00512 SoftJointLimits soft_limits_;
00513 double max_vel_limit_;
00514 };
00515
00525 template <class HandleType>
00526 class JointLimitsInterface : public hardware_interface::ResourceManager<HandleType>
00527 {
00528 public:
00529 HandleType getHandle(const std::string& name)
00530 {
00531
00532 try
00533 {
00534 return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
00535 }
00536 catch(const std::logic_error& e)
00537 {
00538 throw JointLimitsInterfaceException(e.what());
00539 }
00540 }
00541
00545 void enforceLimits(const ros::Duration& period)
00546 {
00547 typedef typename hardware_interface::ResourceManager<HandleType>::ResourceMap::iterator ItratorType;
00548 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00549 {
00550 it->second.enforceLimits(period);
00551 }
00552 }
00553
00554 };
00555
00557 class PositionJointSaturationInterface : public JointLimitsInterface<PositionJointSaturationHandle> {};
00558
00560 class PositionJointSoftLimitsInterface : public JointLimitsInterface<PositionJointSoftLimitsHandle> {};
00561
00563 class EffortJointSaturationInterface : public JointLimitsInterface<EffortJointSaturationHandle> {};
00564
00566 class EffortJointSoftLimitsInterface : public JointLimitsInterface<EffortJointSoftLimitsHandle> {};
00567
00569 class VelocityJointSaturationInterface : public JointLimitsInterface<VelocityJointSaturationHandle> {};
00570
00572 class VelocityJointSoftLimitsInterface : public JointLimitsInterface<VelocityJointSoftLimitsHandle> {};
00573
00574 }
00575
00576 #endif