joint_calibration_controller.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 "ros/time.h"
38 
40 
41 using namespace std;
42 
43 namespace controller {
44 
45 JointCalibrationController::JointCalibrationController()
46 : robot_(NULL), last_publish_time_(0),
47  actuator_(NULL), joint_(NULL)
48 {
49 }
50 
52 {
53 }
54 
56 {
57  robot_ = robot;
58  node_ = n;
59 
60  // Joint
61  std::string joint_name;
62  if (!node_.getParam("joint", joint_name))
63  {
64  ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
65  return false;
66  }
67  if (!(joint_ = robot->getJointState(joint_name)))
68  {
69  ROS_ERROR("Could not find joint %s (namespace: %s)",
70  joint_name.c_str(), node_.getNamespace().c_str());
71  return false;
72  }
73  if (!joint_->joint_->calibration)
74  {
75  ROS_ERROR("Joint %s has no calibration reference position specified (namespace: %s)",
76  joint_name.c_str(), node_.getNamespace().c_str());
77  return false;
78  }
79 
80  // Actuator
81  std::string actuator_name;
82  if (!node_.getParam("actuator", actuator_name))
83  {
84  ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
85  return false;
86  }
87  if (!(actuator_ = robot->model_->getActuator(actuator_name)))
88  {
89  ROS_ERROR("Could not find actuator %s (namespace: %s)",
90  actuator_name.c_str(), node_.getNamespace().c_str());
91  return false;
92  }
93 
94  bool force_calibration = false;
95  node_.getParam("force_calibration", force_calibration);
96 
98  joint_->calibrated_ = false;
99  if (actuator_->state_.zero_offset_ != 0)
100  {
101  if (force_calibration)
102  {
103  ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f",
104  joint_name.c_str(), actuator_->state_.zero_offset_);
105  }
106  else
107  {
108  ROS_INFO("Joint %s is already calibrated at offset %f",
109  joint_name.c_str(), actuator_->state_.zero_offset_);
110  state_ = CALIBRATED;
111  joint_->calibrated_ = true;
112  }
113  }
114  else{
115  ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
116  }
117 
118  // Transmission
119  std::string transmission_name;
120  if (!node_.getParam("transmission", transmission_name))
121  {
122  ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
123  return false;
124  }
125  if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
126  {
127  ROS_ERROR("Could not find transmission %s (namespace: %s)",
128  transmission_name.c_str(), node_.getNamespace().c_str());
129  return false;
130  }
131 
132  if (!node_.getParam("velocity", search_velocity_))
133  {
134  ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
135  return false;
136  }
137 
138  // check if calibration fields are supported by this controller
139  if (!joint_->joint_->calibration->falling && !joint_->joint_->calibration->rising){
140  ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", joint_name.c_str());
141  return false;
142  }
143  if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising && joint_->joint_->type != urdf::Joint::CONTINUOUS){
144  ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", joint_name.c_str());
145  return false;
146  }
147  if (search_velocity_ < 0){
148  search_velocity_ *= -1;
149  ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positive.", joint_name.c_str());
150  }
151 
152  // finds search velocity based on rising or falling edge
153  if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising){
154  joint_->reference_position_ = *(joint_->joint_->calibration->rising);
155  ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
156  }
157  else if (joint_->joint_->calibration->falling){
158  joint_->reference_position_ = *(joint_->joint_->calibration->falling);
159  search_velocity_ *= -1.0;
160  ROS_DEBUG("Using negative search velocity for joint %s", joint_name.c_str());
161  }
162  else if (joint_->joint_->calibration->rising){
163  joint_->reference_position_ = *(joint_->joint_->calibration->rising);
164  ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
165  }
166 
167 
168  // Contained velocity controller
169  if (!vc_.init(robot, node_))
170  return false;
171 
172  // advertise service to check calibration
174 
175  // "Calibrated" topic
177 
178 
179  return true;
180 }
181 
182 
184 {
186  joint_->calibrated_ = false;
188 }
189 
190 
191 bool JointCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
192  pr2_controllers_msgs::QueryCalibrationState::Response& resp)
193 {
194  ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
195  resp.is_calibrated = (state_ == CALIBRATED);
196  return true;
197 }
198 
199 
201 {
202  assert(joint_);
203  assert(actuator_);
204 
205  switch(state_)
206  {
207  case INITIALIZED:
208  vc_.setCommand(0.0);
209  state_ = BEGINNING;
210  break;
211  case BEGINNING:
214  else{
217  }
218  break;
219  case MOVING_TO_LOW:
222  {
223  if (--countdown_ <= 0){
226  }
227  }
228  else
229  countdown_ = 200;
230  break;
231  case MOVING_TO_HIGH: {
233 
235  {
236  // detect when we hit the wrong transition because someone pushed the joint during calibration
237  if ((search_velocity_ > 0.0 && (joint_->position_ - original_position_) < 0) ||
239  {
240  state_ = BEGINNING;
241  ROS_ERROR("Joint hit the falling edge instead of the rising edge. Calibrating again...");
242  ros::Duration(1.0).sleep(); // give joint some time to move away from transition
243  break;
244  }
245 
248 
249  // store position of flag in actuator
251 
252  // mark the joint as calibrated
253  joint_->calibrated_ = true;
254 
255  state_ = CALIBRATED;
256  vc_.setCommand(0.0);
257  }
258  break;
259  }
260  case CALIBRATED:
261  if (pub_calibrated_) {
263  assert(pub_calibrated_);
264  if (pub_calibrated_->trylock()) {
266  pub_calibrated_->unlockAndPublish();
267  }
268  }
269  }
270  break;
271  }
272 
273  if (state_ != CALIBRATED)
274  vc_.update();
275 }
276 } // namespace
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
boost::shared_ptr< pr2_mechanism_model::Transmission > getTransmission(const std::string &name) const
pr2_hardware_interface::Actuator * actuator_
bool sleep() const
virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Empty > > pub_calibrated_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< const urdf::Joint > joint_
boost::shared_ptr< pr2_mechanism_model::Transmission > transmission_
#define ROS_INFO(...)
bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request &req, pr2_controllers_msgs::QueryCalibrationState::Response &resp)
const std::string & getNamespace() const
JointState * getJointState(const std::string &name)
bool getParam(const std::string &key, std::string &s) const
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
#define ROS_ERROR(...)
controller::JointVelocityController vc_
bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid)
#define ROS_DEBUG(...)


pr2_calibration_controllers
Author(s): Stuart Glaser
autogenerated on Wed Jun 5 2019 19:34:11