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 
41 using namespace std;
42 using namespace pr2_mechanism_model;
43 
44 
45 void JointStatistics::update(JointState* jnt)
46 {
47  if (initialized_){
48  odometer_ += fabs(old_position_ - jnt->position_);
49  if (jnt->joint_->safety && jnt->joint_->limits && (fabs(jnt->commanded_effort_) > fabs(jnt->measured_effort_)))
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_);
54  max_abs_effort_ = fmax(fabs(jnt->measured_effort_), max_abs_effort_);
55  }
56  else{
57  min_position_ = jnt->position_;
58  max_position_ = jnt->position_;
59  initialized_ = true;
60  }
61  old_position_ = jnt->position_;
62 }
63 
64 
65 void JointStatistics::reset()
66 {
67  double tmp = min_position_;
68  min_position_ = max_position_;
69  max_position_ = tmp;
70  max_abs_velocity_ = 0.0;
71  max_abs_effort_ = 0.0;
72  violated_limits_ = false;
73 }
74 
75 
76 void JointState::enforceLimits()
77 {
78  double effort_high, effort_low;
79 
80  getLimits(effort_low, effort_high);
81 
82  // limit the commanded effort based on position, velocity and effort limits
83  commanded_effort_ =
84  min( max(commanded_effort_, effort_low), effort_high);
85 }
86 
87 void JointState::getLimits(double &effort_low, double &effort_high)
88 {
89  // only enforce joints that specify joint limits and safety code
90  if (!joint_->safety || !joint_->limits) {
91  effort_low = -std::numeric_limits<double>::max();
92  effort_high = std::numeric_limits<double>::max();
93  return;
94  }
95 
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;
100 
101  // enforce position bounds on rotary and prismatic joints that are calibrated
102  if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
103  {
104  // Computes the velocity bounds based on the absolute limit and the
105  // proximity to the joint limit.
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)));
112  }
113 
114  // Computes the effort bounds based on the velocity and effort bounds.
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)));
121 }
122 
123 
double velocity_
The joint velocity in randians/sec or meters/sec (read-only variable)
Definition: joint.h:87
boost::shared_ptr< const urdf::Joint > joint_
A pointer to the corresponding urdf::Joint from the urdf::Model.
Definition: joint.h:81
double position_
The joint position in radians or meters (read-only variable)
Definition: joint.h:84
double commanded_effort_
The effort the joint should apply in Nm or N (write-to variable)
Definition: joint.h:96
double measured_effort_
The measured joint effort in Nm or N (read-only variable)
Definition: joint.h:90
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 Mon Jun 10 2019 14:19:04