wrist_calibration_controller.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 /*
31  * Author: Stuart Glaser
32  */
33 
36 
38 
39 namespace controller {
40 
41 
42 WristCalibrationController::WristCalibrationController()
43 : robot_(NULL), last_publish_time_(0)
44 {
45 }
46 
48 {
49  for (size_t i = 0; i < fake_as.size(); ++i)
50  delete fake_as[i];
51  for (size_t i = 0; i < fake_js.size(); ++i)
52  delete fake_js[i];
53 }
54 
56  ros::NodeHandle &n)
57 {
58  assert(robot);
59  node_ = n;
60  robot_ = robot;
61 
62  // Joints
63 
64  std::string roll_joint_name;
65  if (!node_.getParam("roll_joint", roll_joint_name))
66  {
67  ROS_ERROR("No roll joint given (namespace: %s)", node_.getNamespace().c_str());
68  return false;
69  }
70  if (!(roll_joint_ = robot->getJointState(roll_joint_name)))
71  {
72  ROS_ERROR("Could not find roll joint \"%s\" (namespace: %s)",
73  roll_joint_name.c_str(), node_.getNamespace().c_str());
74  return false;
75  }
76  if (!roll_joint_->joint_->calibration)
77  {
78  ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
79  roll_joint_name.c_str(), node_.getNamespace().c_str());
80  return false;
81  }
82 
83 
84  std::string flex_joint_name;
85  if (!node_.getParam("flex_joint", flex_joint_name))
86  {
87  ROS_ERROR("No flex joint given (namespace: %s)", node_.getNamespace().c_str());
88  return false;
89  }
90  if (!(flex_joint_ = robot->getJointState(flex_joint_name)))
91  {
92  ROS_ERROR("Could not find flex joint \"%s\" (namespace: %s)",
93  flex_joint_name.c_str(), node_.getNamespace().c_str());
94  return false;
95  }
96  if (!flex_joint_->joint_->calibration)
97  {
98  ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
99  flex_joint_name.c_str(), node_.getNamespace().c_str());
100  return false;
101  }
102 
103 
104 
105 
106  if (!node_.getParam("roll_velocity", roll_search_velocity_))
107  {
108  ROS_ERROR("No roll_velocity given (namespace: %s)", node_.getNamespace().c_str());
109  return false;
110  }
111  // check if calibration fields are supported by this controller
112  if (!roll_joint_->joint_->calibration->falling && !roll_joint_->joint_->calibration->rising){
113  ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", roll_joint_name.c_str());
114  return false;
115  }
116  if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising && roll_joint_->joint_->type != urdf::Joint::CONTINUOUS){
117  ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", roll_joint_name.c_str());
118  return false;
119  }
120  if (roll_search_velocity_ < 0){
121  roll_search_velocity_ *= -1;
122  ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", roll_joint_name.c_str());
123  }
124 
125  // sets reference position
126  if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising){
127  roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
128  }
129  else if (roll_joint_->joint_->calibration->falling){
130  roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->falling);
131  }
132  else if (roll_joint_->joint_->calibration->rising){
133  roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
134  }
135 
136 
137  if (!node_.getParam("flex_velocity", flex_search_velocity_))
138  {
139  ROS_ERROR("No flex_velocity given (namespace: %s)", node_.getNamespace().c_str());
140  return false;
141  }
142  // check if calibration fields are supported by this controller
143  if (!flex_joint_->joint_->calibration->falling && !flex_joint_->joint_->calibration->rising){
144  ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", flex_joint_name.c_str());
145  return false;
146  }
147  if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising && flex_joint_->joint_->type != urdf::Joint::CONTINUOUS){
148  ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", flex_joint_name.c_str());
149  return false;
150  }
151  if (flex_search_velocity_ < 0){
152  flex_search_velocity_ *= -1;
153  ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", flex_joint_name.c_str());
154  }
155 
156  // sets reference position
157  if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising){
158  flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
159  }
160  else if (flex_joint_->joint_->calibration->falling){
161  flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->falling);
162  }
163  else if (flex_joint_->joint_->calibration->rising){
164  flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
165  }
166 
167  // Actuators
168  std::string actuator_l_name;
169  if (!node_.getParam("actuator_l", actuator_l_name))
170  {
171  ROS_ERROR("No actuator_l given (namespace: %s)", node_.getNamespace().c_str());
172  return false;
173  }
174  if (!(actuator_l_ = robot->model_->getActuator(actuator_l_name)))
175  {
176  ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
177  actuator_l_name.c_str(), node_.getNamespace().c_str());
178  return false;
179  }
180 
181  std::string actuator_r_name;
182  if (!node_.getParam("actuator_r", actuator_r_name))
183  {
184  ROS_ERROR("No actuator_r given (namespace: %s)", node_.getNamespace().c_str());
185  return false;
186  }
187  if (!(actuator_r_ = robot->model_->getActuator(actuator_r_name)))
188  {
189  ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
190  actuator_r_name.c_str(), node_.getNamespace().c_str());
191  return false;
192  }
193 
194  bool force_calibration = false;
195  node_.getParam("force_calibration", force_calibration);
196 
197  roll_joint_->calibrated_ = false;
198  flex_joint_->calibrated_ = false;
201  if (force_calibration)
202  {
203  ROS_INFO("Joints %s and %s are already calibrated but will be recalibrated. "
204  "Actuator %s was zeroed at %f and %s was zeroed at %f.",
205  flex_joint_name.c_str(), roll_joint_name.c_str(),
208  );
209  }
210  else
211  {
212  ROS_INFO("Wrist joints %s and %s are already calibrated", flex_joint_name.c_str(), roll_joint_name.c_str());
213  flex_joint_->calibrated_ = true;
214  roll_joint_->calibrated_ = true;
215  state_ = CALIBRATED;
216  }
217  }
218  else{
219  ROS_INFO("Not both wrist joints %s and %s are are calibrated. Will re-calibrate both of them", flex_joint_name.c_str(), roll_joint_name.c_str());
220  }
221 
222 
223 
224  // Transmission
225  std::string transmission_name;
226  if (!node_.getParam("transmission", transmission_name))
227  {
228  ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
229  return false;
230  }
231  if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
232  {
233  ROS_ERROR("Could not find transmission \"%s\" (namespace: %s)",
234  transmission_name.c_str(), node_.getNamespace().c_str());
235  return false;
236  }
237 
238  // Prepares the namespaces for the velocity controllers
239 
241  node_.getParam("pid", pid);
242 
243  ros::NodeHandle roll_node(node_, "roll_velocity");
244  roll_node.setParam("type", std::string("JointVelocityController"));
245  roll_node.setParam("joint", roll_joint_name);
246  roll_node.setParam("pid", pid);
247 
248  ros::NodeHandle flex_node(node_, "flex_velocity");
249  flex_node.setParam("type", std::string("JointVelocityController"));
250  flex_node.setParam("joint", flex_joint_name);
251  flex_node.setParam("pid", pid);
252 
257 
262  fake_js[FLEX_JOINT]->joint_ = flex_joint_->joint_;
263  fake_js[ROLL_JOINT]->joint_ = roll_joint_->joint_;
264 
265  if (!vc_roll_.init(robot_, roll_node)) return false;
266  if (!vc_flex_.init(robot_, flex_node)) return false;
267 
268  // advertise service to check calibration
270 
271  // "Calibrated" topic
273 
274  return true;
275 }
276 
277 
279 {
283  flex_joint_->calibrated_ = false;
284  roll_joint_->calibrated_ = false;
285 }
286 
287 
288 bool WristCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
289  pr2_controllers_msgs::QueryCalibrationState::Response& resp)
290 {
291  ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
292  resp.is_calibrated = (state_ == CALIBRATED);
293  return true;
294 }
295 
296 
298 {
299  // Flex optical switch is connected to actuator_l
300  // Roll optical switch is connected to actuator_r
301 
302  switch(state_)
303  {
304  case INITIALIZED:
307  flex_joint_->calibrated_ = false;
308  roll_joint_->calibrated_ = false;
309  vc_flex_.setCommand(0);
310  vc_roll_.setCommand(0);
311  state_ = BEGINNING;
312  break;
313  case BEGINNING:
314  vc_roll_.setCommand(0);
315  // Because of the huge hysteresis on the wrist flex calibration sensor, we want to move towards the
316  // high side for a long period of time, regardless of which side we start on
317  countdown_ = 1000;
320  else
322  break;
323  case MOVING_FLEX_TO_HIGH:
325 
327  {
328  if (--countdown_ <= 0)
330  }
331  else
332  countdown_ = 1000;
333  break;
334  case MOVING_FLEX: {
335  // Calibrates across the falling edge in the positive joint direction.
338  {
340 
341  // But where was actuator_r at the transition? Unfortunately,
342  // actuator_r is not connected to the flex joint's optical
343  // switch, so we don't know directly. Instead, we estimate
344  // actuator_r's position based on the switch position of
345  // actuator_l.
348  double k = (flex_switch_l_ - prev_actuator_l_position_) / dl;
349  if (dl == 0)
350  {
351  // This might be a serious hardware failure, so we're going to break realtime.
352  ROS_WARN("Left actuator (flex joint) didn't move even though the calibration flag tripped. "
353  "This may indicate an encoder problem. (namespace: %s",
354  node_.getNamespace().c_str());
355  k = 0.5;
356  }
357  else if ( !(0 <= k && k <= 1) )
358  {
359  // This is really serious, so we're going to break realtime to report it.
360  ROS_ERROR("k = %.4lf is outside of [0,1]. This probably indicates a hardware failure "
361  "on the left actuator or the flex joint. dl = %.4lf, dr = %.4lf, prev = %.4lf. "
362  "Broke realtime to report (namespace: %s)",
363  k, dl, dr, prev_actuator_l_position_, node_.getNamespace().c_str());
365  break;
366  }
367 
369 
370  //original_switch_state_ = actuator_r_->state_.calibration_reading_;
371 
372  // Now we calibrate the roll joint
373  vc_flex_.setCommand(0);
376  else
378  }
379  break;
380  }
381  case MOVING_ROLL_TO_LOW:
385  break;
386  case MOVING_ROLL: {
387  // Calibrates across the rising edge in the positive joint direction.
390  {
392 
393  // See corresponding comment above.
396  double k = (roll_switch_r_ - prev_actuator_r_position_) / dr;
397  if (dr == 0)
398  {
399  // This might be a serious hardware failure, so we're going to break realtime.
400  ROS_WARN("Right actuator (roll joint) didn't move even though the calibration flag tripped. "
401  "This may indicate an encoder problem. (namespace: %s",
402  node_.getNamespace().c_str());
403  k = 0.5;
404  }
405  else if ( !(0 <= k && k <= 1) )
406  {
407  // This is really serious, so we're going to break realtime to report it.
408  ROS_ERROR("k = %.4lf is outside of [0,1]. This probably indicates a hardware failure "
409  "on the right actuator or the roll joint. dl = %.4lf, dr = %.4lf, prev = %.4lf. "
410  "Broke realtime to report (namespace: %s)",
411  k, dl, dr, prev_actuator_r_position_, node_.getNamespace().c_str());
413  break;
414  }
416 
417 
418  //----------------------------------------------------------------------
419  // Calibration computation
420  //----------------------------------------------------------------------
421 
422  // At this point, we know the actuator positions when the
423  // optical switches were hit. Now we compute the actuator
424  // positions when the joints should be at 0.
425 
430 
431  // Finds the (uncalibrated) joint position where the flex optical switch triggers
432  fake_as[LEFT_MOTOR]->state_.position_ = flex_switch_l_;
433  fake_as[RIGHT_MOTOR]->state_.position_ = flex_switch_r_;
434  transmission_->propagatePosition(fake_as, fake_js);
435  double flex_joint_switch = fake_js[FLEX_JOINT]->position_;
436 
437  // Finds the (uncalibrated) joint position where the roll optical switch triggers
438  fake_as[LEFT_MOTOR]->state_.position_ = roll_switch_l_;
439  fake_as[RIGHT_MOTOR]->state_.position_ = roll_switch_r_;
440  transmission_->propagatePosition(fake_as, fake_js);
441  double roll_joint_switch = fake_js[ROLL_JOINT]->position_;
442 
443  // Finds the (uncalibrated) joint position at the desired zero
444  fake_js[FLEX_JOINT]->position_ = flex_joint_switch;
445  fake_js[ROLL_JOINT]->position_ = roll_joint_switch;
446 
447  // Determines the actuator zero position from the desired joint zero positions
448  transmission_->propagatePositionBackwards(fake_js, fake_as);
449  if (std::isnan(fake_as[LEFT_MOTOR]->state_.position_) ||
450  std::isnan(fake_as[RIGHT_MOTOR]->state_.position_))
451  {
452  ROS_ERROR("Restarting calibration because a computed offset was NaN. If this happens "
453  "repeatedly it may indicate a hardware failure. (namespace: %s)",
454  node_.getNamespace().c_str());
456  break;
457  }
458  actuator_l_->state_.zero_offset_ = fake_as[LEFT_MOTOR]->state_.position_;
459  actuator_r_->state_.zero_offset_ = fake_as[RIGHT_MOTOR]->state_.position_;
460 
461  flex_joint_->calibrated_ = true;
462  roll_joint_->calibrated_ = true;
463  state_ = CALIBRATED;
464 
465  vc_flex_.setCommand(0);
466  vc_roll_.setCommand(0);
467  }
468 
469  break;
470  }
471  case CALIBRATED:
472  if (pub_calibrated_) {
474  assert(pub_calibrated_);
475  if (pub_calibrated_->trylock()) {
477  pub_calibrated_->unlockAndPublish();
478  }
479  }
480  }
481  break;
482  }
483 
484  if (state_ != CALIBRATED)
485  {
486  vc_flex_.update();
487  vc_roll_.update();
488  }
489 
492 }
493 }
494 
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
boost::shared_ptr< pr2_mechanism_model::Transmission > getTransmission(const std::string &name) const
std::vector< pr2_hardware_interface::Actuator * > fake_as
controller::JointVelocityController vc_flex_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< const urdf::Joint > joint_
#define ROS_WARN(...)
pr2_mechanism_model::JointState * roll_joint_
pr2_hardware_interface::Actuator * actuator_r_
controller::JointVelocityController vc_roll_
virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
#define ROS_INFO(...)
const std::string & getNamespace() const
std::vector< pr2_mechanism_model::JointState * > fake_js
bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request &req, pr2_controllers_msgs::QueryCalibrationState::Response &resp)
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Empty > > pub_calibrated_
JointState * getJointState(const std::string &name)
boost::shared_ptr< pr2_mechanism_model::Transmission > transmission_
bool getParam(const std::string &key, std::string &s) const
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
pr2_hardware_interface::Actuator * actuator_l_
#define ROS_ERROR(...)
pr2_mechanism_model::JointState * flex_joint_
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
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