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)));
double velocity_
The joint velocity in randians/sec or meters/sec (read-only variable)
boost::shared_ptr< const urdf::Joint > joint_
A pointer to the corresponding urdf::Joint from the urdf::Model.
double position_
The joint position in radians or meters (read-only variable)
double commanded_effort_
The effort the joint should apply in Nm or N (write-to variable)
double measured_effort_
The measured joint effort in Nm or N (read-only variable)
double min(double a, double b)
double max(double a, double b)