wrist_transmission.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 /*
35  * Author: Melonee Wise
36  */
37 #include <math.h>
41 
42 using namespace pr2_mechanism_model;
43 using namespace pr2_hardware_interface;
44 
47 
48 
49 static bool convertDouble(const char* val_str, double &value)
50 {
51  char *endptr=NULL;
52  value = strtod(val_str, &endptr);
53  if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
54  {
55  return false;
56  }
57 
58  return true;
59 }
60 
62 {
63  joint_offset_[0] = 0.0;
64  joint_offset_[1] = 0.0;
65 }
66 
67 
68 bool WristTransmission::initXml(TiXmlElement *elt, Robot *robot)
69 {
70  const char *name = elt->Attribute("name");
71  name_ = name ? name : "";
72 
73 
74  TiXmlElement *ael = elt->FirstChildElement("rightActuator");
75  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
76  Actuator *a;
77  if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
78  {
79  ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
80  return false;
81  }
82  a->command_.enable_ = true;
83  actuator_names_.push_back(actuator_name);
84  const char *act_red = ael->Attribute("mechanicalReduction");
85  if (!act_red)
86  {
87  ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
88  return false;
89  }
90  actuator_reduction_.push_back(atof(act_red));
91 
92  ael = elt->FirstChildElement("leftActuator");
93  actuator_name = ael ? ael->Attribute("name") : NULL;
94  if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
95  {
96  ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
97  return false;
98  }
99  a->command_.enable_ = true;
100  actuator_names_.push_back(actuator_name);
101  act_red = ael->Attribute("mechanicalReduction");
102  if (!act_red)
103  {
104  ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
105  return false;
106  }
107  actuator_reduction_.push_back(atof(act_red));
108 
109 
110  TiXmlElement *j = elt->FirstChildElement("flexJoint");
111  const char *joint_name = j->Attribute("name");
112  if (!joint_name)
113  {
114  ROS_ERROR("WristTransmission did not specify joint name");
115  return false;
116  }
117  const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
118 
119  if (!joint)
120  {
121  ROS_ERROR("WristTransmission could not find joint named \"%s\"", joint_name);
122  return false;
123  }
124  joint_names_.push_back(joint_name);
125  const char *joint_red = j->Attribute("mechanicalReduction");
126  if (!joint_red)
127  {
128  ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
129  return false;
130  }
131  joint_reduction_.push_back(atof(joint_red));
132  const char *joint_offset = j->Attribute("offset");
133  if (!joint_offset)
134  {
135  joint_offset_[0] = 0.0;
136  }
137  else
138  {
139  if (!convertDouble(joint_offset, joint_offset_[0]))
140  {
141  ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
142  joint_name, joint_offset);
143  return false;
144  }
145  else
146  {
147  ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
148  }
149  }
150 
151  j = elt->FirstChildElement("rollJoint");
152  joint_name = j->Attribute("name");
153  if (!joint_name)
154  {
155  ROS_ERROR("WristTransmission did not specify joint name");
156  return false;
157  }
158  const boost::shared_ptr<const urdf::Joint> joint2 = robot->robot_model_.getJoint(joint_name);
159 
160  if (!joint2)
161  {
162  ROS_ERROR("WristTransmission could not find joint named \"%s\"", joint_name);
163  return false;
164  }
165  joint_names_.push_back(joint_name);
166  joint_red = j->Attribute("mechanicalReduction");
167  if (!joint_red)
168  {
169  ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
170  return false;
171  }
172  joint_reduction_.push_back(atof(joint_red));
173  joint_offset = j->Attribute("offset");
174  if (!joint_offset)
175  {
176  joint_offset_[1] = 0.0;
177  }
178  else
179  {
180  if (!convertDouble(joint_offset, joint_offset_[1]))
181  {
182  ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
183  joint_name, joint_offset);
184  return false;
185  }
186  else
187  {
188  ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
189  }
190  }
191 
192 
193  return true;
194 }
195 
196 bool WristTransmission::initXml(TiXmlElement *elt)
197 {
198  const char *name = elt->Attribute("name");
199  name_ = name ? name : "";
200 
201 
202  TiXmlElement *ael = elt->FirstChildElement("rightActuator");
203  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
204  if (!actuator_name)
205  {
206  ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
207  return false;
208  }
209  actuator_names_.push_back(actuator_name);
210  const char *act_red = ael->Attribute("mechanicalReduction");
211  if (!act_red)
212  {
213  ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
214  return false;
215  }
216  actuator_reduction_.push_back(atof(act_red));
217 
218  ael = elt->FirstChildElement("leftActuator");
219  actuator_name = ael ? ael->Attribute("name") : NULL;
220  if (!actuator_name)
221  {
222  ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
223  return false;
224  }
225  actuator_names_.push_back(actuator_name);
226  act_red = ael->Attribute("mechanicalReduction");
227  if (!act_red)
228  {
229  ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
230  return false;
231  }
232  actuator_reduction_.push_back(atof(act_red));
233 
234 
235  TiXmlElement *j = elt->FirstChildElement("flexJoint");
236  const char *joint_name = j->Attribute("name");
237  if (!joint_name)
238  {
239  ROS_ERROR("WristTransmission did not specify joint name");
240  return false;
241  }
242  joint_names_.push_back(joint_name);
243  const char *joint_red = j->Attribute("mechanicalReduction");
244  if (!joint_red)
245  {
246  ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
247  return false;
248  }
249  joint_reduction_.push_back(atof(joint_red));
250  const char *joint_offset = j->Attribute("offset");
251  if (!joint_offset)
252  {
253  joint_offset_[0] = 0.0;
254  }
255  else
256  {
257  double offset;
258  if (!convertDouble(joint_offset, joint_offset_[0]))
259  {
260  ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
261  joint_name, joint_offset);
262  return false;
263  }
264  else
265  {
266  ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
267  }
268  }
269 
270  j = elt->FirstChildElement("rollJoint");
271  joint_name = j->Attribute("name");
272  if (!joint_name)
273  {
274  ROS_ERROR("WristTransmission did not specify joint name");
275  return false;
276  }
277  joint_names_.push_back(joint_name);
278  joint_red = j->Attribute("mechanicalReduction");
279  if (!joint_red)
280  {
281  ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
282  return false;
283  }
284  joint_reduction_.push_back(atof(joint_red));
285  if (!joint_offset)
286  {
287  joint_offset_[1] = 0.0;
288  }
289  else
290  {
291  if (!convertDouble(joint_offset, joint_offset_[1]))
292  {
293  ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
294  joint_name, joint_offset);
295  return false;
296  }
297  else
298  {
299  ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
300  }
301  }
302 
303  return true;
304 }
305 
307  std::vector<Actuator*>& as, std::vector<JointState*>& js)
308 {
309  assert(as.size() == 2);
310  assert(js.size() == 2);
311 
312  js[0]->position_ = ((as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
313  (2*joint_reduction_[0])) + js[0]->reference_position_+joint_offset_[0];
314  js[0]->velocity_ = (as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[0]);
315  js[0]->measured_effort_ = joint_reduction_[0]*(as[0]->state_.last_measured_effort_ * actuator_reduction_[0] - as[1]->state_.last_measured_effort_ * actuator_reduction_[1]);
316 
317  js[1]->position_ = ((-as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
318  (2*joint_reduction_[1]))+js[1]->reference_position_+joint_offset_[1];
319  js[1]->velocity_ = (-as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[1]);
320  js[1]->measured_effort_ = joint_reduction_[1]*(-as[0]->state_.last_measured_effort_ * actuator_reduction_[0] - as[1]->state_.last_measured_effort_ * actuator_reduction_[1]);
321 }
322 
324  std::vector<JointState*>& js, std::vector<Actuator*>& as)
325 {
326  assert(as.size() == 2);
327  assert(js.size() == 2);
328 
329  as[0]->state_.position_ = (((js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
330  (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[0]);
331  as[0]->state_.velocity_ = ((js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[0]);
332  as[0]->state_.last_measured_effort_ = (js[0]->measured_effort_/joint_reduction_[0] - js[1]->measured_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
333 
334  as[1]->state_.position_ = ((-(js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
335  (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[1]);
336  as[1]->state_.velocity_ = ((-js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[1]);
337  as[1]->state_.last_measured_effort_ = (-js[0]->measured_effort_/joint_reduction_[0] - js[1]->measured_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
338 
339  // Update the timing (making sure it's initialized).
340  if (! simulated_actuator_timestamp_initialized_)
341  {
342  // Set the time stamp to zero (it is measured relative to the start time).
343  as[0]->state_.sample_timestamp_ = ros::Duration(0);
344  as[1]->state_.sample_timestamp_ = ros::Duration(0);
345 
346  // Try to set the start time. Only then do we claim initialized.
347  if (ros::isStarted())
348  {
349  simulated_actuator_start_time_ = ros::Time::now();
350  simulated_actuator_timestamp_initialized_ = true;
351  }
352  }
353  else
354  {
355  // Measure the time stamp relative to the start time.
356  as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
357  as[1]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
358  }
359  // Set the historical (double) timestamp accordingly.
360  as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
361  as[1]->state_.timestamp_ = as[1]->state_.sample_timestamp_.toSec();
362 
363  // simulate calibration sensors by filling out actuator states
364  // this is where to embed the hack which joint connects to which mcb
365  this->joint_calibration_simulator_[0].simulateJointCalibration(js[0],as[1]);
366  this->joint_calibration_simulator_[1].simulateJointCalibration(js[1],as[0]);
367 }
368 
370  std::vector<JointState*>& js, std::vector<Actuator*>& as)
371 {
372  assert(as.size() == 2);
373  assert(js.size() == 2);
374 
375  as[0]->command_.enable_ = true;
376  as[1]->command_.enable_ = true;
377 
378  as[0]->command_.effort_ = (js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
379  as[1]->command_.effort_ = (-js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
380 }
381 
383  std::vector<Actuator*>& as, std::vector<JointState*>& js)
384 {
385  assert(as.size() == 2);
386  assert(js.size() == 2);
387 
388  js[0]->commanded_effort_ = joint_reduction_[0]*(as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
389  js[1]->commanded_effort_ = joint_reduction_[1]*(-as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
390 }
391 
392 
393 void WristTransmission::setReductions(std::vector<double>& ar, std::vector<double>& jr)
394 {
395  actuator_reduction_ = ar;
396  joint_reduction_ = jr;
397 }
398 
void propagateEffort(std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &)
Uses commanded joint efforts to fill out commanded motor currents.
void propagateEffortBackwards(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses the actuator&#39;s commanded effort to fill out the torque on the joint.
#define ROS_WARN(...)
void propagatePosition(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses encoder data to fill out joint position and velocities.
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
ROSCPP_DECL bool isStarted()
void propagatePositionBackwards(std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &)
Uses the joint position to fill out the actuator&#39;s encoder.
static Time now()
double & value
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
get an actuator pointer based on the actuator name. Returns NULL on failure
Definition: robot.cpp:134
void setReductions(std::vector< double > &ar, std::vector< double > &jr)
#define ROS_ERROR(...)
This class provides the controllers with an interface to the robot model.
Definition: robot.h:78
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
Definition: robot.h:91
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::WristTransmission, pr2_mechanism_model::Transmission) static bool convertDouble(const char *val_str


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