49 odometer_ += fabs(old_position_ - jnt->
position_);
51 violated_limits_ =
true;
52 min_position_ = fmin(jnt->
position_, min_position_);
53 max_position_ = fmax(jnt->
position_, max_position_);
54 max_abs_velocity_ = fmax(fabs(jnt->
velocity_), max_abs_velocity_);
66 void JointStatistics::reset()
68 double tmp = min_position_;
69 min_position_ = max_position_;
71 max_abs_velocity_ = 0.0;
72 max_abs_effort_ = 0.0;
73 violated_limits_ =
false;
77 void JointState::enforceLimits()
79 double effort_high, effort_low;
81 getLimits(effort_low, effort_high);
85 min( max(commanded_effort_, effort_low), effort_high);
88 void JointState::getLimits(
double &effort_low,
double &effort_high)
91 if (!joint_->safety || !joint_->limits) {
92 effort_low = -std::numeric_limits<double>::max();
93 effort_high = std::numeric_limits<double>::max();
97 double vel_high = joint_->limits->velocity;
98 double vel_low = -joint_->limits->velocity;
99 effort_high = joint_->limits->effort;
100 effort_low = -joint_->limits->effort;
103 if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
107 vel_high = max(-joint_->limits->velocity,
108 min(joint_->limits->velocity,
109 -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit)));
110 vel_low = min(joint_->limits->velocity,
111 max(-joint_->limits->velocity,
112 -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit)));
116 effort_high = max(-joint_->limits->effort,
117 min(joint_->limits->effort,
118 -joint_->safety->k_velocity * (velocity_ - vel_high)));
119 effort_low = min(joint_->limits->effort,
120 max(-joint_->limits->effort,
121 -joint_->safety->k_velocity * (velocity_ - vel_low)));