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
00121 void reset(){
00122 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00123 }
00124
00125 private:
00126 hardware_interface::JointHandle jh_;
00127 JointLimits limits_;
00128 double min_pos_limit_, max_pos_limit_;
00129 double prev_cmd_;
00130 };
00131
00162
00163 class PositionJointSoftLimitsHandle
00164 {
00165 public:
00166 PositionJointSoftLimitsHandle()
00167 : prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00168 {}
00169
00170 PositionJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00171 const JointLimits& limits,
00172 const SoftJointLimits& soft_limits)
00173 : jh_(jh),
00174 limits_(limits),
00175 soft_limits_(soft_limits),
00176 prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00177 {
00178 if (!limits.has_velocity_limits)
00179 {
00180 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00181 "'. It has no velocity limits specification.");
00182 }
00183 }
00184
00186 std::string getName() const {return jh_.getName();}
00187
00194 void enforceLimits(const ros::Duration& period)
00195 {
00196 assert(period.toSec() > 0.0);
00197
00198 using internal::saturate;
00199
00200
00201
00202 if (std::isnan(prev_cmd_)) {prev_cmd_ = jh_.getPosition();}
00203 const double pos = prev_cmd_;
00204
00205
00206 double soft_min_vel;
00207 double soft_max_vel;
00208
00209 if (limits_.has_position_limits)
00210 {
00211
00212 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00213 -limits_.max_velocity,
00214 limits_.max_velocity);
00215
00216 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00217 -limits_.max_velocity,
00218 limits_.max_velocity);
00219 }
00220 else
00221 {
00222
00223 soft_min_vel = -limits_.max_velocity;
00224 soft_max_vel = limits_.max_velocity;
00225 }
00226
00227
00228 const double dt = period.toSec();
00229 double pos_low = pos + soft_min_vel * dt;
00230 double pos_high = pos + soft_max_vel * dt;
00231
00232 if (limits_.has_position_limits)
00233 {
00234
00235 pos_low = std::max(pos_low, limits_.min_position);
00236 pos_high = std::min(pos_high, limits_.max_position);
00237 }
00238
00239
00240 const double pos_cmd = saturate(jh_.getCommand(),
00241 pos_low,
00242 pos_high);
00243 jh_.setCommand(pos_cmd);
00244
00245
00246 prev_cmd_ = jh_.getCommand();
00247 }
00248
00252 void reset(){
00253 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00254 }
00255
00256 private:
00257 hardware_interface::JointHandle jh_;
00258 JointLimits limits_;
00259 SoftJointLimits soft_limits_;
00260
00261 double prev_cmd_;
00262 };
00263
00266 class EffortJointSaturationHandle
00267 {
00268 public:
00269 EffortJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00270 {
00271 jh_ = jh;
00272 limits_ = limits;
00273 }
00274
00276 std::string getName() const {return jh_.getName();}
00277
00281 void enforceLimits(const ros::Duration& )
00282 {
00283 double min_eff, max_eff;
00284 if (limits_.has_effort_limits)
00285 {
00286 min_eff = -limits_.max_effort;
00287 max_eff = limits_.max_effort;
00288 }
00289 else
00290 {
00291 min_eff = -std::numeric_limits<double>::max();
00292 max_eff = std::numeric_limits<double>::max();
00293 }
00294
00295 if (limits_.has_position_limits)
00296 {
00297 const double pos = jh_.getPosition();
00298 if (pos < limits_.min_position)
00299 min_eff = 0;
00300 else if (pos > limits_.max_position)
00301 max_eff = 0;
00302 }
00303
00304 if (limits_.has_velocity_limits)
00305 {
00306 const double vel = jh_.getVelocity();
00307 if (vel < -limits_.max_velocity)
00308 min_eff = 0;
00309 else if (vel > limits_.max_velocity)
00310 max_eff = 0;
00311 }
00312
00313 jh_.setCommand(internal::saturate(jh_.getCommand(), min_eff, max_eff));
00314 }
00315
00316 private:
00317 hardware_interface::JointHandle jh_;
00318 JointLimits limits_;
00319 };
00320
00323
00324 class EffortJointSoftLimitsHandle
00325 {
00326 public:
00327 EffortJointSoftLimitsHandle() {}
00328
00329 EffortJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00330 const JointLimits& limits,
00331 const SoftJointLimits& soft_limits)
00332 : jh_(jh),
00333 limits_(limits),
00334 soft_limits_(soft_limits)
00335 {
00336 if (!limits.has_velocity_limits)
00337 {
00338 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00339 "'. It has no velocity limits specification.");
00340 }
00341 if (!limits.has_effort_limits)
00342 {
00343 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00344 "'. It has no effort limits specification.");
00345 }
00346 }
00347
00349 std::string getName() const {return jh_.getName();}
00350
00356 void enforceLimits(const ros::Duration& )
00357 {
00358 using internal::saturate;
00359
00360
00361 const double pos = jh_.getPosition();
00362 const double vel = jh_.getVelocity();
00363
00364
00365 double soft_min_vel;
00366 double soft_max_vel;
00367
00368 if (limits_.has_position_limits)
00369 {
00370
00371 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00372 -limits_.max_velocity,
00373 limits_.max_velocity);
00374
00375 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00376 -limits_.max_velocity,
00377 limits_.max_velocity);
00378 }
00379 else
00380 {
00381
00382 soft_min_vel = -limits_.max_velocity;
00383 soft_max_vel = limits_.max_velocity;
00384 }
00385
00386
00387 const double soft_min_eff = saturate(-soft_limits_.k_velocity * (vel - soft_min_vel),
00388 -limits_.max_effort,
00389 limits_.max_effort);
00390
00391 const double soft_max_eff = saturate(-soft_limits_.k_velocity * (vel - soft_max_vel),
00392 -limits_.max_effort,
00393 limits_.max_effort);
00394
00395
00396 const double eff_cmd = saturate(jh_.getCommand(),
00397 soft_min_eff,
00398 soft_max_eff);
00399 jh_.setCommand(eff_cmd);
00400 }
00401
00402 private:
00403 hardware_interface::JointHandle jh_;
00404 JointLimits limits_;
00405 SoftJointLimits soft_limits_;
00406 };
00407
00408
00410 class VelocityJointSaturationHandle
00411 {
00412 public:
00413 VelocityJointSaturationHandle () {}
00414
00415 VelocityJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00416 : jh_(jh),
00417 limits_(limits)
00418 {
00419 if (!limits.has_velocity_limits)
00420 {
00421 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00422 "'. It has no velocity limits specification.");
00423 }
00424 }
00425
00427 std::string getName() const {return jh_.getName();}
00428
00433 void enforceLimits(const ros::Duration& period)
00434 {
00435 using internal::saturate;
00436
00437
00438 double vel_low;
00439 double vel_high;
00440
00441 if (limits_.has_acceleration_limits)
00442 {
00443 assert(period.toSec() > 0.0);
00444 const double vel = jh_.getVelocity();
00445 const double dt = period.toSec();
00446
00447 vel_low = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity);
00448 vel_high = std::min(vel + limits_.max_acceleration * dt, limits_.max_velocity);
00449 }
00450 else
00451 {
00452 vel_low = -limits_.max_velocity;
00453 vel_high = limits_.max_velocity;
00454 }
00455
00456
00457 const double vel_cmd = saturate(jh_.getCommand(),
00458 vel_low,
00459 vel_high);
00460 jh_.setCommand(vel_cmd);
00461 }
00462
00463 private:
00464 hardware_interface::JointHandle jh_;
00465 JointLimits limits_;
00466 };
00467
00469 class VelocityJointSoftLimitsHandle
00470 {
00471 public:
00472 VelocityJointSoftLimitsHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits,
00473 const SoftJointLimits& soft_limits)
00474 {
00475 jh_ = jh;
00476 limits_ = limits;
00477 soft_limits_ = soft_limits;
00478 if (limits.has_velocity_limits)
00479 max_vel_limit_ = limits.max_velocity;
00480 else
00481 max_vel_limit_ = std::numeric_limits<double>::max();
00482 }
00483
00485 std::string getName() const {return jh_.getName();}
00486
00492 void enforceLimits(const ros::Duration& period)
00493 {
00494 using internal::saturate;
00495
00496 double min_vel, max_vel;
00497 if (limits_.has_position_limits)
00498 {
00499
00500 const double pos = jh_.getPosition();
00501 min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00502 -max_vel_limit_, max_vel_limit_);
00503 max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00504 -max_vel_limit_, max_vel_limit_);
00505 }
00506 else
00507 {
00508 min_vel = -max_vel_limit_;
00509 max_vel = max_vel_limit_;
00510 }
00511
00512 if (limits_.has_acceleration_limits)
00513 {
00514 const double vel = jh_.getVelocity();
00515 const double delta_t = period.toSec();
00516 min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel);
00517 max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel);
00518 }
00519
00520 jh_.setCommand(saturate(jh_.getCommand(), min_vel, max_vel));
00521 }
00522
00523 private:
00524 hardware_interface::JointHandle jh_;
00525 JointLimits limits_;
00526 SoftJointLimits soft_limits_;
00527 double max_vel_limit_;
00528 };
00529
00539 template <class HandleType>
00540 class JointLimitsInterface : public hardware_interface::ResourceManager<HandleType>
00541 {
00542 public:
00543 HandleType getHandle(const std::string& name)
00544 {
00545
00546 try
00547 {
00548 return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
00549 }
00550 catch(const std::logic_error& e)
00551 {
00552 throw JointLimitsInterfaceException(e.what());
00553 }
00554 }
00555
00559 void enforceLimits(const ros::Duration& period)
00560 {
00561 typedef typename hardware_interface::ResourceManager<HandleType>::ResourceMap::iterator ItratorType;
00562 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00563 {
00564 it->second.enforceLimits(period);
00565 }
00566 }
00567
00568 };
00569
00571 class PositionJointSaturationInterface : public JointLimitsInterface<PositionJointSaturationHandle> {
00572 public:
00576 void reset()
00577 {
00578 typedef hardware_interface::ResourceManager<PositionJointSaturationHandle>::ResourceMap::iterator ItratorType;
00579 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00580 {
00581 it->second.reset();
00582 }
00583 }
00584
00585 };
00586
00588 class PositionJointSoftLimitsInterface : public JointLimitsInterface<PositionJointSoftLimitsHandle> {
00589 public:
00593 void reset()
00594 {
00595 typedef hardware_interface::ResourceManager<PositionJointSoftLimitsHandle>::ResourceMap::iterator ItratorType;
00596 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00597 {
00598 it->second.reset();
00599 }
00600 }
00601
00602 };
00603
00605 class EffortJointSaturationInterface : public JointLimitsInterface<EffortJointSaturationHandle> {};
00606
00608 class EffortJointSoftLimitsInterface : public JointLimitsInterface<EffortJointSoftLimitsHandle> {};
00609
00611 class VelocityJointSaturationInterface : public JointLimitsInterface<VelocityJointSaturationHandle> {};
00612
00614 class VelocityJointSoftLimitsInterface : public JointLimitsInterface<VelocityJointSoftLimitsHandle> {};
00615
00616 }
00617
00618 #endif