Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <vector>
00019 #include <ros/ros.h>
00020
00021 #include "cob_twist_controller/limiters/limiter.h"
00022
00023
00027 KDL::Twist LimiterContainer::enforceLimits(const KDL::Twist& v_in) const
00028 {
00029
00030 KDL::Twist v_out(v_in);
00031 for (input_LimIter_t it = this->input_limiters_.begin(); it != this->input_limiters_.end(); it++)
00032 {
00033 v_out = (*it)->enforceLimits(v_out);
00034 }
00035
00036 return v_out;
00037 }
00038 KDL::JntArray LimiterContainer::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00039 {
00040
00041 KDL::JntArray q_dot_norm(q_dot_ik);
00042 for (output_LimIter_t it = this->output_limiters_.begin(); it != this->output_limiters_.end(); it++)
00043 {
00044 q_dot_norm = (*it)->enforceLimits(q_dot_norm, q);
00045 }
00046
00047 return q_dot_norm;
00048 }
00049
00053 void LimiterContainer::init()
00054 {
00055 this->eraseAll();
00056
00057 if (limiter_params_.keep_direction)
00058 {
00059 if (limiter_params_.enforce_input_limits)
00060 {
00061 this->add(new LimiterAllCartesianVelocities(limiter_params_));
00062 }
00063
00064 if (limiter_params_.enforce_pos_limits)
00065 {
00066 this->add(new LimiterAllJointPositions(limiter_params_));
00067 }
00068
00069 if (limiter_params_.enforce_vel_limits)
00070 {
00071 this->add(new LimiterAllJointVelocities(limiter_params_));
00072 }
00073
00074 if (limiter_params_.enforce_acc_limits)
00075 {
00076 this->add(new LimiterAllJointAccelerations(limiter_params_));
00077 }
00078 }
00079 else
00080 {
00081 if (limiter_params_.enforce_input_limits)
00082 {
00083 this->add(new LimiterIndividualCartesianVelocities(limiter_params_));
00084 }
00085
00086 if (limiter_params_.enforce_pos_limits)
00087 {
00088 this->add(new LimiterIndividualJointPositions(limiter_params_));
00089 }
00090
00091 if (limiter_params_.enforce_vel_limits)
00092 {
00093 this->add(new LimiterIndividualJointVelocities(limiter_params_));
00094 }
00095
00096 if (limiter_params_.enforce_acc_limits)
00097 {
00098 this->add(new LimiterIndividualJointAccelerations(limiter_params_));
00099 }
00100 }
00101 }
00102
00106 void LimiterContainer::eraseAll()
00107 {
00108 for (uint32_t cnt = 0; cnt < this->input_limiters_.size(); ++cnt)
00109 {
00110 delete(this->input_limiters_[cnt]);
00111 }
00112 for (uint32_t cnt = 0; cnt < this->output_limiters_.size(); ++cnt)
00113 {
00114 delete(this->output_limiters_[cnt]);
00115 }
00116
00117 this->input_limiters_.clear();
00118 this->output_limiters_.clear();
00119 }
00120
00124 void LimiterContainer::add(const LimiterCartesianBase* lb)
00125 {
00126 this->input_limiters_.push_back(lb);
00127 }
00128 void LimiterContainer::add(const LimiterJointBase* lb)
00129 {
00130 this->output_limiters_.push_back(lb);
00131 }
00132
00136 LimiterContainer::~LimiterContainer()
00137 {
00138 this->eraseAll();
00139 }
00140
00141
00142
00150 KDL::JntArray LimiterAllJointPositions::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00151 {
00152 KDL::JntArray q_dot_norm(q_dot_ik);
00153 double tolerance = limiter_params_.limits_tolerance / 180.0 * M_PI;
00154 double max_factor = 1.0;
00155 int joint_index = -1;
00156
00157 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00158 {
00159 if ((limiter_params_.limits_max[i] - LIMIT_SAFETY_THRESHOLD <= q(i) && q_dot_ik(i) > 0) ||
00160 (limiter_params_.limits_min[i] + LIMIT_SAFETY_THRESHOLD >= q(i) && q_dot_ik(i) < 0))
00161 {
00162 ROS_ERROR_STREAM("Joint " << i << " violates its limits. Setting to Zero!");
00163 KDL::SetToZero(q_dot_norm);
00164 return q_dot_norm;
00165 }
00166
00167 if (fabs(limiter_params_.limits_max[i] - q(i)) <= tolerance)
00168 {
00169 if (q_dot_ik(i) > 0)
00170 {
00171 double temp = 1.0 / pow((0.5 + 0.5 * cos(M_PI * (q(i) + tolerance - limiter_params_.limits_max[i]) / tolerance)), 5.0);
00172
00173 if (temp > max_factor)
00174 {
00175 max_factor = temp;
00176 joint_index = i;
00177 }
00178 }
00179 }
00180
00181 if (fabs(q(i) - limiter_params_.limits_min[i]) <= tolerance)
00182 {
00183 if (q_dot_ik(i) < 0)
00184 {
00185 double temp = 1.0 / pow(0.5 + 0.5 * cos(M_PI * (q(i) - tolerance - limiter_params_.limits_min[i]) / tolerance), 5.0);
00186
00187 if (temp > max_factor)
00188 {
00189 max_factor = temp;
00190 joint_index = i;
00191 }
00192 }
00193 }
00194 }
00195
00196 if (max_factor > 1.0)
00197 {
00198 ROS_ERROR_STREAM_THROTTLE(1, "Position tolerance surpassed (by Joint " << joint_index << "): Scaling ALL VELOCITIES with factor = " << max_factor);
00199 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00200 {
00201 q_dot_norm(i) = q_dot_ik(i) / max_factor;
00202 }
00203 }
00204
00205 return q_dot_norm;
00206 }
00207
00208
00209
00214 KDL::JntArray LimiterAllJointVelocities::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00215 {
00216 KDL::JntArray q_dot_norm(q_dot_ik);
00217 double max_factor = 1.0;
00218 int joint_index = -1;
00219
00220 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00221 {
00222 if (max_factor < std::fabs(q_dot_ik(i) / limiter_params_.limits_vel[i]))
00223 {
00224 max_factor = std::fabs(q_dot_ik(i) / limiter_params_.limits_vel[i]);
00225 joint_index = i;
00226 }
00227 }
00228
00229 if (max_factor > 1.0)
00230 {
00231 ROS_WARN_STREAM_THROTTLE(1, "Velocity limit surpassed (by Joint " << joint_index << "): Scaling ALL VELOCITIES with factor = " << max_factor);
00232 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00233 {
00234 q_dot_norm(i) = q_dot_ik(i) / max_factor;
00235 }
00236 }
00237
00238 return q_dot_norm;
00239 }
00240
00241
00242
00247 KDL::JntArray LimiterAllJointAccelerations::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00248 {
00249 KDL::JntArray q_dot_norm(q_dot_ik);
00250
00251 ROS_WARN("LimiterAllJointAccelerations not yet implemented");
00252
00253 return q_dot_norm;
00254 }
00255
00256
00257
00262 KDL::Twist LimiterAllCartesianVelocities::enforceLimits(const KDL::Twist& v_in) const
00263 {
00264 KDL::Twist v_out(v_in);
00265
00266 double max_factor = 1.0;
00267 max_factor = std::max( max_factor, std::fabs(v_in.rot.x()/limiter_params_.max_rot_twist) );
00268 max_factor = std::max( max_factor, std::fabs(v_in.rot.y()/limiter_params_.max_rot_twist) );
00269 max_factor = std::max( max_factor, std::fabs(v_in.rot.z()/limiter_params_.max_rot_twist) );
00270 max_factor = std::max( max_factor, std::fabs(v_in.vel.x()/limiter_params_.max_lin_twist) );
00271 max_factor = std::max( max_factor, std::fabs(v_in.vel.y()/limiter_params_.max_lin_twist) );
00272 max_factor = std::max( max_factor, std::fabs(v_in.vel.z()/limiter_params_.max_lin_twist) );
00273
00274 if (max_factor > 1.0)
00275 {
00276 ROS_WARN_STREAM_THROTTLE(1, "Cartesian velocity limit surpassed: Scaling ALL VELOCITIES with factor = " << max_factor);
00277 v_out.rot.x(v_in.rot.x()/max_factor);
00278 v_out.rot.y(v_in.rot.y()/max_factor);
00279 v_out.rot.z(v_in.rot.z()/max_factor);
00280 v_out.vel.x(v_in.vel.x()/max_factor);
00281 v_out.vel.y(v_in.vel.y()/max_factor);
00282 v_out.vel.z(v_in.vel.z()/max_factor);
00283
00284 }
00285
00286 return v_out;
00287 }
00288
00289
00290
00295 KDL::JntArray LimiterIndividualJointPositions::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00296 {
00297 KDL::JntArray q_dot_norm(q_dot_ik);
00298 double tolerance = limiter_params_.limits_tolerance / 180.0 * M_PI;
00299
00300 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00301 {
00302 if ((limiter_params_.limits_max[i] - LIMIT_SAFETY_THRESHOLD <= q(i) && q_dot_ik(i) > 0) ||
00303 (limiter_params_.limits_min[i] + LIMIT_SAFETY_THRESHOLD >= q(i) && q_dot_ik(i) < 0))
00304 {
00305 ROS_ERROR_STREAM("Joint " << i << " violates its limits. Setting to Zero!");
00306 q_dot_norm(i) = 0.0;
00307 }
00308
00309 double factor = 1.0;
00310 if (fabs(limiter_params_.limits_max[i] - q(i)) <= tolerance)
00311 {
00312 if (q_dot_ik(i) > 0.0)
00313 {
00314 double temp = 1.0 / pow((0.5 + 0.5 * cos(M_PI * (q(i) + tolerance - limiter_params_.limits_max[i]) / tolerance)), 5.0);
00315
00316 factor = (temp > factor) ? temp : factor;
00317 }
00318 }
00319
00320 if (fabs(q(i) - limiter_params_.limits_min[i]) <= tolerance)
00321 {
00322 if (q_dot_ik(i) < 0.0)
00323 {
00324 double temp = 1.0 / pow(0.5 + 0.5 * cos(M_PI * (q(i) - tolerance - limiter_params_.limits_min[i]) / tolerance), 5.0);
00325
00326 factor = (temp > factor) ? temp : factor;
00327 }
00328 }
00329 q_dot_norm(i) = q_dot_norm(i) / factor;
00330 }
00331
00332 return q_dot_norm;
00333 }
00334
00335
00336
00341 KDL::JntArray LimiterIndividualJointVelocities::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00342 {
00343 KDL::JntArray q_dot_norm(q_dot_ik);
00344
00345 for (unsigned int i = 0; i < q_dot_ik.rows(); i++)
00346 {
00347 double factor = 1.0;
00348 if (factor < std::fabs(q_dot_ik(i) / limiter_params_.limits_vel[i]))
00349 {
00350 factor = std::fabs(q_dot_ik(i) / limiter_params_.limits_vel[i]);
00351 q_dot_norm(i) = q_dot_ik(i) / factor;
00352 }
00353 }
00354
00355 return q_dot_norm;
00356 }
00357
00358
00359
00364 KDL::JntArray LimiterIndividualJointAccelerations::enforceLimits(const KDL::JntArray& q_dot_ik, const KDL::JntArray& q) const
00365 {
00366 KDL::JntArray q_dot_norm(q_dot_ik);
00367
00368 ROS_WARN("LimiterIndividualJointAccelerations not yet implemented");
00369
00370 return q_dot_norm;
00371 }
00372
00373
00374
00379 KDL::Twist LimiterIndividualCartesianVelocities::enforceLimits(const KDL::Twist& v_in) const
00380 {
00381 KDL::Twist v_out(v_in);
00382
00383
00384 if (v_in.rot.x() >limiter_params_.max_rot_twist)
00385 v_out.rot.x(limiter_params_.max_rot_twist);
00386 if (v_in.rot.x() <-limiter_params_.max_rot_twist)
00387 v_out.rot.x(-limiter_params_.max_rot_twist);
00388
00389 if (v_in.rot.y() >limiter_params_.max_rot_twist)
00390 v_out.rot.y(limiter_params_.max_rot_twist);
00391 if (v_in.rot.y() <-limiter_params_.max_rot_twist)
00392 v_out.rot.y(-limiter_params_.max_rot_twist);
00393
00394 if (v_in.rot.z() >limiter_params_.max_rot_twist)
00395 v_out.rot.z(limiter_params_.max_rot_twist);
00396 if (v_in.rot.z() <-limiter_params_.max_rot_twist)
00397 v_out.rot.z(-limiter_params_.max_rot_twist);
00398
00399
00400 if (v_in.vel.x() >limiter_params_.max_lin_twist)
00401 v_out.vel.x(limiter_params_.max_lin_twist);
00402 if (v_in.vel.x() <-limiter_params_.max_lin_twist)
00403 v_out.vel.x(-limiter_params_.max_lin_twist);
00404
00405 if (v_in.vel.y() >limiter_params_.max_lin_twist)
00406 v_out.vel.y(limiter_params_.max_lin_twist);
00407 if (v_in.vel.y() <-limiter_params_.max_lin_twist)
00408 v_out.vel.y(-limiter_params_.max_lin_twist);
00409
00410 if (v_in.vel.z() >limiter_params_.max_lin_twist)
00411 v_out.vel.z(limiter_params_.max_lin_twist);
00412 if (v_in.vel.z() <-limiter_params_.max_lin_twist)
00413 v_out.vel.z(-limiter_params_.max_lin_twist);
00414
00415 return v_out;
00416 }
00417