48 odometer_ += fabs(old_position_ - jnt->
position_);
50 violated_limits_ =
true;
51 min_position_ = fmin(jnt->
position_, min_position_);
52 max_position_ = fmax(jnt->
position_, max_position_);
53 max_abs_velocity_ = fmax(fabs(jnt->
velocity_), max_abs_velocity_);
65 void JointStatistics::reset()
67 double tmp = min_position_;
68 min_position_ = max_position_;
70 max_abs_velocity_ = 0.0;
71 max_abs_effort_ = 0.0;
72 violated_limits_ =
false;
76 void JointState::enforceLimits()
78 double effort_high, effort_low;
80 getLimits(effort_low, effort_high);
84 min(
max(commanded_effort_, effort_low), effort_high);
87 void JointState::getLimits(
double &effort_low,
double &effort_high)
90 if (!joint_->safety || !joint_->limits) {
91 effort_low = -std::numeric_limits<double>::max();
92 effort_high = std::numeric_limits<double>::max();
96 double vel_high = joint_->limits->velocity;
97 double vel_low = -joint_->limits->velocity;
98 effort_high = joint_->limits->effort;
99 effort_low = -joint_->limits->effort;
102 if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
106 vel_high =
max(-joint_->limits->velocity,
107 min(joint_->limits->velocity,
108 -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit)));
109 vel_low =
min(joint_->limits->velocity,
110 max(-joint_->limits->velocity,
111 -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit)));
115 effort_high =
max(-joint_->limits->effort,
116 min(joint_->limits->effort,
117 -joint_->safety->k_velocity * (velocity_ - vel_high)));
118 effort_low =
min(joint_->limits->effort,
119 max(-joint_->limits->effort,
120 -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)