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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <pr2_mechanism_model/joint.h>
00036 #include <map>
00037 #include <string>
00038 #include <vector>
00039 #include <cfloat>
00040
00041 using namespace std;
00042 using namespace pr2_mechanism_model;
00043
00044
00045 void JointStatistics::update(JointState* jnt)
00046 {
00047 if (initialized_){
00048 odometer_ += fabs(old_position_ - jnt->position_);
00049 if (jnt->joint_->safety && jnt->joint_->limits && (fabs(jnt->commanded_effort_) > fabs(jnt->measured_effort_)))
00050 violated_limits_ = true;
00051 min_position_ = fmin(jnt->position_, min_position_);
00052 max_position_ = fmax(jnt->position_, max_position_);
00053 max_abs_velocity_ = fmax(fabs(jnt->velocity_), max_abs_velocity_);
00054 max_abs_effort_ = fmax(fabs(jnt->measured_effort_), max_abs_effort_);
00055 }
00056 else{
00057 min_position_ = jnt->position_;
00058 max_position_ = jnt->position_;
00059 initialized_ = true;
00060 }
00061 old_position_ = jnt->position_;
00062 }
00063
00064
00065 void JointStatistics::reset()
00066 {
00067 double tmp = min_position_;
00068 min_position_ = max_position_;
00069 max_position_ = tmp;
00070 max_abs_velocity_ = 0.0;
00071 max_abs_effort_ = 0.0;
00072 violated_limits_ = false;
00073 }
00074
00075
00076 void JointState::enforceLimits()
00077 {
00078 double effort_high, effort_low;
00079
00080 getLimits(effort_low, effort_high);
00081
00082
00083 commanded_effort_ =
00084 min( max(commanded_effort_, effort_low), effort_high);
00085 }
00086
00087 void JointState::getLimits(double &effort_low, double &effort_high)
00088 {
00089
00090 if (!joint_->safety || !joint_->limits) {
00091 effort_low = -std::numeric_limits<double>::max();
00092 effort_high = std::numeric_limits<double>::max();
00093 return;
00094 }
00095
00096 double vel_high = joint_->limits->velocity;
00097 double vel_low = -joint_->limits->velocity;
00098 effort_high = joint_->limits->effort;
00099 effort_low = -joint_->limits->effort;
00100
00101
00102 if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
00103 {
00104
00105
00106 vel_high = max(-joint_->limits->velocity,
00107 min(joint_->limits->velocity,
00108 -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit)));
00109 vel_low = min(joint_->limits->velocity,
00110 max(-joint_->limits->velocity,
00111 -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit)));
00112 }
00113
00114
00115 effort_high = max(-joint_->limits->effort,
00116 min(joint_->limits->effort,
00117 -joint_->safety->k_velocity * (velocity_ - vel_high)));
00118 effort_low = min(joint_->limits->effort,
00119 max(-joint_->limits->effort,
00120 -joint_->safety->k_velocity * (velocity_ - vel_low)));
00121 }
00122
00123