joint_calibration_simulator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Wim Meeussen
31 
33 #include <angles/angles.h>
34 
35 
36 namespace pr2_mechanism_model {
37 
38 
40  : calibration_initialized_(false),
41  calibration_has_rising_(false),
42  calibration_has_falling_(false),
43  calibration_continuous_(false),
44  got_info_(false)
45 {
46 }
47 
49 {
50  assert(js->joint_);
51 
52  // simulate calibration backward propagation
53  if (js->joint_->calibration){
54  if (js->joint_->calibration->rising){
56  calibration_rising_ = *(js->joint_->calibration->rising);
57  }
58  if (js->joint_->calibration->falling){
60  calibration_falling_ = *(js->joint_->calibration->falling);
61  }
62  }
63 
64 
65  // continuous joints
66  if (js->joint_->type == urdf::Joint::CONTINUOUS){
71  }
75  }
79  calibration_bump_ = true;
80  else
81  calibration_bump_ = false;
82  }
83 
84  // check
85  if (js->joint_->type != urdf::Joint::CONTINUOUS && calibration_has_rising_ && calibration_has_falling_)
86  ROS_ERROR("Non-continuous joint with both rising and falling edge not supported");
87 
88  this->got_info_ = true;
89 }
90 
92 {
93  // setup calibration information for the joint
94  if (!this->got_info_) this->GetJointCalibrationInfo(js);
95 
96  // current joint_angle
97  double pos = js->position_ - js->reference_position_;
98  double as_pos = as->state_.position_;
99 
100  // compute calibration reading
102  double pos_c = angles::normalize_angle(pos);
103  if (calibration_bump_){
104  if (pos_c > calibration_rising_ && pos_c < calibration_falling_)
105  as->state_.calibration_reading_ = true;
106  else
107  as->state_.calibration_reading_ = false;
108  }
109  else{
110  if (pos_c < calibration_rising_ && pos_c > calibration_falling_)
111  as->state_.calibration_reading_ = false; // in low part
112  else
113  as->state_.calibration_reading_ = true;
114  }
115  }
116  else{
119  else if (calibration_has_falling_)
121  }
122 
124  // tripped calibration flag
125  //ROS_ERROR("debug: %s %d %d",js->joint_->name.c_str(),old_calibration_reading_ ,as->state_.calibration_reading_);
127  if (as->state_.calibration_reading_){ // low to high
128  if (pos > old_calibration_pos_) // joint pos increasing and we are in the high region
129  {
132  }
133  else
134  {
137  }
138  }
139  else{ // high to low
140  if (pos > old_calibration_pos_) // joint pos increasing and we are in the low region
141  {
144  }
145  else
146  {
149  }
150  }
151  }
152  }
153 
154  // store state
156  old_calibration_pos_ = pos;
157  old_calibration_as_pos_ = as_pos;
159 }
160 
161 
162 
163 } //namespace
164 
void simulateJointCalibration(pr2_mechanism_model::JointState *, pr2_hardware_interface::Actuator *)
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
def normalize_angle(angle)
double reference_position_
The position of the optical flag that was used to calibrate this joint.
Definition: joint.h:102
void GetJointCalibrationInfo(pr2_mechanism_model::JointState *)
#define ROS_ERROR(...)


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:04