joint.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include <map>
37 #include <string>
38 #include <vector>
39 #include <cfloat>
40 #include <limits> // for std::numeric_limits
41 
42 using namespace std;
43 using namespace pr2_mechanism_model;
44 
45 
46 void JointStatistics::update(JointState* jnt)
47 {
48  if (initialized_){
49  odometer_ += fabs(old_position_ - jnt->position_);
50  if (jnt->joint_->safety && jnt->joint_->limits && (fabs(jnt->commanded_effort_) > fabs(jnt->measured_effort_)))
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_);
55  max_abs_effort_ = fmax(fabs(jnt->measured_effort_), max_abs_effort_);
56  }
57  else{
58  min_position_ = jnt->position_;
59  max_position_ = jnt->position_;
60  initialized_ = true;
61  }
62  old_position_ = jnt->position_;
63 }
64 
65 
66 void JointStatistics::reset()
67 {
68  double tmp = min_position_;
69  min_position_ = max_position_;
70  max_position_ = tmp;
71  max_abs_velocity_ = 0.0;
72  max_abs_effort_ = 0.0;
73  violated_limits_ = false;
74 }
75 
76 
77 void JointState::enforceLimits()
78 {
79  double effort_high, effort_low;
80 
81  getLimits(effort_low, effort_high);
82 
83  // limit the commanded effort based on position, velocity and effort limits
84  commanded_effort_ =
85  min( max(commanded_effort_, effort_low), effort_high);
86 }
87 
88 void JointState::getLimits(double &effort_low, double &effort_high)
89 {
90  // only enforce joints that specify joint limits and safety code
91  if (!joint_->safety || !joint_->limits) {
92  effort_low = -std::numeric_limits<double>::max();
93  effort_high = std::numeric_limits<double>::max();
94  return;
95  }
96 
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;
101 
102  // enforce position bounds on rotary and prismatic joints that are calibrated
103  if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
104  {
105  // Computes the velocity bounds based on the absolute limit and the
106  // proximity to the joint limit.
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)));
113  }
114 
115  // Computes the effort bounds based on the velocity and effort bounds.
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)));
122 }
123 
124 
double velocity_
The joint velocity in randians/sec or meters/sec (read-only variable)
Definition: joint.h:91
boost::shared_ptr< const urdf::Joint > joint_
A pointer to the corresponding urdf::Joint from the urdf::Model.
Definition: joint.h:84
double position_
The joint position in radians or meters (read-only variable)
Definition: joint.h:88
double commanded_effort_
The effort the joint should apply in Nm or N (write-to variable)
Definition: joint.h:100
double measured_effort_
The measured joint effort in Nm or N (read-only variable)
Definition: joint.h:94
double min(double a, double b)
double max(double a, double b)


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Tue Mar 7 2023 03:54:53