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 <ros/duration.h>
00040
00041 #include <hardware_interface/internal/resource_manager.h>
00042 #include <hardware_interface/joint_command_interface.h>
00043
00044 #include <joint_limits_interface/joint_limits.h>
00045 #include <joint_limits_interface/joint_limits_interface_exception.h>
00046
00047 namespace joint_limits_interface
00048 {
00049
00050 namespace internal
00051 {
00052
00053 template<typename T>
00054 T saturate(const T val, const T min_val, const T max_val)
00055 {
00056 return std::min(std::max(val, min_val), max_val);
00057 }
00058
00059 }
00060
00063 class PositionJointSaturationHandle
00064 {
00065 public:
00066 PositionJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00067 {
00068 jh_ = jh;
00069 limits_ = limits;
00070
00071 if (limits_.has_position_limits)
00072 {
00073 min_pos_limit_ = limits_.min_position;
00074 max_pos_limit_ = limits_.max_position;
00075 }
00076 else
00077 {
00078 min_pos_limit_ = -std::numeric_limits<double>::max();
00079 max_pos_limit_ = std::numeric_limits<double>::max();
00080 }
00081
00082 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00083 }
00084
00086 std::string getName() const {return jh_.getName();}
00087
00093 void enforceLimits(const ros::Duration& period)
00094 {
00095 if (std::isnan(prev_cmd_))
00096 prev_cmd_ = jh_.getPosition();
00097
00098 double min_pos, max_pos;
00099 if (limits_.has_velocity_limits)
00100 {
00101 const double delta_pos = limits_.max_velocity * period.toSec();
00102 min_pos = std::max(prev_cmd_ - delta_pos, min_pos_limit_);
00103 max_pos = std::min(prev_cmd_ + delta_pos, max_pos_limit_);
00104 }
00105 else
00106 {
00107 min_pos = min_pos_limit_;
00108 max_pos = max_pos_limit_;
00109 }
00110
00111 const double cmd = internal::saturate(jh_.getCommand(), min_pos, max_pos);
00112 jh_.setCommand(cmd);
00113 prev_cmd_ = cmd;
00114 }
00115
00119 void reset(){
00120 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00121 }
00122
00123 private:
00124 hardware_interface::JointHandle jh_;
00125 JointLimits limits_;
00126 double min_pos_limit_, max_pos_limit_;
00127 double prev_cmd_;
00128 };
00129
00160
00161 class PositionJointSoftLimitsHandle
00162 {
00163 public:
00164 PositionJointSoftLimitsHandle()
00165 : prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00166 {}
00167
00168 PositionJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00169 const JointLimits& limits,
00170 const SoftJointLimits& soft_limits)
00171 : jh_(jh),
00172 limits_(limits),
00173 soft_limits_(soft_limits),
00174 prev_cmd_(std::numeric_limits<double>::quiet_NaN())
00175 {
00176 if (!limits.has_velocity_limits)
00177 {
00178 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00179 "'. It has no velocity limits specification.");
00180 }
00181 }
00182
00184 std::string getName() const {return jh_.getName();}
00185
00192 void enforceLimits(const ros::Duration& period)
00193 {
00194 assert(period.toSec() > 0.0);
00195
00196 using internal::saturate;
00197
00198
00199
00200 if (std::isnan(prev_cmd_)) {prev_cmd_ = jh_.getPosition();}
00201 const double pos = prev_cmd_;
00202
00203
00204 double soft_min_vel;
00205 double soft_max_vel;
00206
00207 if (limits_.has_position_limits)
00208 {
00209
00210 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00211 -limits_.max_velocity,
00212 limits_.max_velocity);
00213
00214 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00215 -limits_.max_velocity,
00216 limits_.max_velocity);
00217 }
00218 else
00219 {
00220
00221 soft_min_vel = -limits_.max_velocity;
00222 soft_max_vel = limits_.max_velocity;
00223 }
00224
00225
00226 const double dt = period.toSec();
00227 double pos_low = pos + soft_min_vel * dt;
00228 double pos_high = pos + soft_max_vel * dt;
00229
00230 if (limits_.has_position_limits)
00231 {
00232
00233 pos_low = std::max(pos_low, limits_.min_position);
00234 pos_high = std::min(pos_high, limits_.max_position);
00235 }
00236
00237
00238 const double pos_cmd = saturate(jh_.getCommand(),
00239 pos_low,
00240 pos_high);
00241 jh_.setCommand(pos_cmd);
00242
00243
00244 prev_cmd_ = jh_.getCommand();
00245 }
00246
00250 void reset(){
00251 prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
00252 }
00253
00254 private:
00255 hardware_interface::JointHandle jh_;
00256 JointLimits limits_;
00257 SoftJointLimits soft_limits_;
00258
00259 double prev_cmd_;
00260 };
00261
00264 class EffortJointSaturationHandle
00265 {
00266 public:
00267 EffortJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00268 {
00269 jh_ = jh;
00270 limits_ = limits;
00271 }
00272
00274 std::string getName() const {return jh_.getName();}
00275
00279 void enforceLimits(const ros::Duration& )
00280 {
00281 double min_eff, max_eff;
00282 if (limits_.has_effort_limits)
00283 {
00284 min_eff = -limits_.max_effort;
00285 max_eff = limits_.max_effort;
00286 }
00287 else
00288 {
00289 min_eff = -std::numeric_limits<double>::max();
00290 max_eff = std::numeric_limits<double>::max();
00291 }
00292
00293 if (limits_.has_position_limits)
00294 {
00295 const double pos = jh_.getPosition();
00296 if (pos < limits_.min_position)
00297 min_eff = 0;
00298 else if (pos > limits_.max_position)
00299 max_eff = 0;
00300 }
00301
00302 if (limits_.has_velocity_limits)
00303 {
00304 const double vel = jh_.getVelocity();
00305 if (vel < -limits_.max_velocity)
00306 min_eff = 0;
00307 else if (vel > limits_.max_velocity)
00308 max_eff = 0;
00309 }
00310
00311 jh_.setCommand(internal::saturate(jh_.getCommand(), min_eff, max_eff));
00312 }
00313
00314 private:
00315 hardware_interface::JointHandle jh_;
00316 JointLimits limits_;
00317 };
00318
00321
00322 class EffortJointSoftLimitsHandle
00323 {
00324 public:
00325 EffortJointSoftLimitsHandle() {}
00326
00327 EffortJointSoftLimitsHandle(const hardware_interface::JointHandle& jh,
00328 const JointLimits& limits,
00329 const SoftJointLimits& soft_limits)
00330 : jh_(jh),
00331 limits_(limits),
00332 soft_limits_(soft_limits)
00333 {
00334 if (!limits.has_velocity_limits)
00335 {
00336 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00337 "'. It has no velocity limits specification.");
00338 }
00339 if (!limits.has_effort_limits)
00340 {
00341 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00342 "'. It has no effort limits specification.");
00343 }
00344 }
00345
00347 std::string getName() const {return jh_.getName();}
00348
00354 void enforceLimits(const ros::Duration& )
00355 {
00356 using internal::saturate;
00357
00358
00359 const double pos = jh_.getPosition();
00360 const double vel = jh_.getVelocity();
00361
00362
00363 double soft_min_vel;
00364 double soft_max_vel;
00365
00366 if (limits_.has_position_limits)
00367 {
00368
00369 soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00370 -limits_.max_velocity,
00371 limits_.max_velocity);
00372
00373 soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00374 -limits_.max_velocity,
00375 limits_.max_velocity);
00376 }
00377 else
00378 {
00379
00380 soft_min_vel = -limits_.max_velocity;
00381 soft_max_vel = limits_.max_velocity;
00382 }
00383
00384
00385 const double soft_min_eff = saturate(-soft_limits_.k_velocity * (vel - soft_min_vel),
00386 -limits_.max_effort,
00387 limits_.max_effort);
00388
00389 const double soft_max_eff = saturate(-soft_limits_.k_velocity * (vel - soft_max_vel),
00390 -limits_.max_effort,
00391 limits_.max_effort);
00392
00393
00394 const double eff_cmd = saturate(jh_.getCommand(),
00395 soft_min_eff,
00396 soft_max_eff);
00397 jh_.setCommand(eff_cmd);
00398 }
00399
00400 private:
00401 hardware_interface::JointHandle jh_;
00402 JointLimits limits_;
00403 SoftJointLimits soft_limits_;
00404 };
00405
00406
00408 class VelocityJointSaturationHandle
00409 {
00410 public:
00411 VelocityJointSaturationHandle () {}
00412
00413 VelocityJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
00414 : jh_(jh),
00415 limits_(limits)
00416 {
00417 if (!limits.has_velocity_limits)
00418 {
00419 throw JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() +
00420 "'. It has no velocity limits specification.");
00421 }
00422 }
00423
00425 std::string getName() const {return jh_.getName();}
00426
00431 void enforceLimits(const ros::Duration& period)
00432 {
00433 using internal::saturate;
00434
00435
00436 double vel_low;
00437 double vel_high;
00438
00439 if (limits_.has_acceleration_limits)
00440 {
00441 assert(period.toSec() > 0.0);
00442 const double vel = jh_.getVelocity();
00443 const double dt = period.toSec();
00444
00445 vel_low = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity);
00446 vel_high = std::min(vel + limits_.max_acceleration * dt, limits_.max_velocity);
00447 }
00448 else
00449 {
00450 vel_low = -limits_.max_velocity;
00451 vel_high = limits_.max_velocity;
00452 }
00453
00454
00455 const double vel_cmd = saturate(jh_.getCommand(),
00456 vel_low,
00457 vel_high);
00458 jh_.setCommand(vel_cmd);
00459 }
00460
00461 private:
00462 hardware_interface::JointHandle jh_;
00463 JointLimits limits_;
00464 };
00465
00467 class VelocityJointSoftLimitsHandle
00468 {
00469 public:
00470 VelocityJointSoftLimitsHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits,
00471 const SoftJointLimits& soft_limits)
00472 {
00473 jh_ = jh;
00474 limits_ = limits;
00475 soft_limits_ = soft_limits;
00476 if (limits.has_velocity_limits)
00477 max_vel_limit_ = limits.max_velocity;
00478 else
00479 max_vel_limit_ = std::numeric_limits<double>::max();
00480 }
00481
00483 std::string getName() const {return jh_.getName();}
00484
00490 void enforceLimits(const ros::Duration& period)
00491 {
00492 using internal::saturate;
00493
00494 double min_vel, max_vel;
00495 if (limits_.has_position_limits)
00496 {
00497
00498 const double pos = jh_.getPosition();
00499 min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position),
00500 -max_vel_limit_, max_vel_limit_);
00501 max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position),
00502 -max_vel_limit_, max_vel_limit_);
00503 }
00504 else
00505 {
00506 min_vel = -max_vel_limit_;
00507 max_vel = max_vel_limit_;
00508 }
00509
00510 if (limits_.has_acceleration_limits)
00511 {
00512 const double vel = jh_.getVelocity();
00513 const double delta_t = period.toSec();
00514 min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel);
00515 max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel);
00516 }
00517
00518 jh_.setCommand(saturate(jh_.getCommand(), min_vel, max_vel));
00519 }
00520
00521 private:
00522 hardware_interface::JointHandle jh_;
00523 JointLimits limits_;
00524 SoftJointLimits soft_limits_;
00525 double max_vel_limit_;
00526 };
00527
00537 template <class HandleType>
00538 class JointLimitsInterface : public hardware_interface::ResourceManager<HandleType>
00539 {
00540 public:
00541 HandleType getHandle(const std::string& name)
00542 {
00543
00544 try
00545 {
00546 return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
00547 }
00548 catch(const std::logic_error& e)
00549 {
00550 throw JointLimitsInterfaceException(e.what());
00551 }
00552 }
00553
00557 void enforceLimits(const ros::Duration& period)
00558 {
00559 typedef typename hardware_interface::ResourceManager<HandleType>::ResourceMap::iterator ItratorType;
00560 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00561 {
00562 it->second.enforceLimits(period);
00563 }
00564 }
00565
00566 };
00567
00569 class PositionJointSaturationInterface : public JointLimitsInterface<PositionJointSaturationHandle> {
00570 public:
00574 void reset()
00575 {
00576 typedef hardware_interface::ResourceManager<PositionJointSaturationHandle>::ResourceMap::iterator ItratorType;
00577 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00578 {
00579 it->second.reset();
00580 }
00581 }
00582
00583 };
00584
00586 class PositionJointSoftLimitsInterface : public JointLimitsInterface<PositionJointSoftLimitsHandle> {
00587 public:
00591 void reset()
00592 {
00593 typedef hardware_interface::ResourceManager<PositionJointSoftLimitsHandle>::ResourceMap::iterator ItratorType;
00594 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
00595 {
00596 it->second.reset();
00597 }
00598 }
00599
00600 };
00601
00603 class EffortJointSaturationInterface : public JointLimitsInterface<EffortJointSaturationHandle> {};
00604
00606 class EffortJointSoftLimitsInterface : public JointLimitsInterface<EffortJointSoftLimitsHandle> {};
00607
00609 class VelocityJointSaturationInterface : public JointLimitsInterface<VelocityJointSaturationHandle> {};
00610
00612 class VelocityJointSoftLimitsInterface : public JointLimitsInterface<VelocityJointSoftLimitsHandle> {};
00613
00614 }
00615
00616 #endif