simple_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: Stuart Glaser
36  */
37 #include <math.h>
41 
42 using namespace pr2_mechanism_model;
43 using namespace pr2_hardware_interface;
44 
47 
48 
49 bool SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
50 {
51  const char *name = elt->Attribute("name");
52  name_ = name ? name : "";
53 
54  TiXmlElement *jel = elt->FirstChildElement("joint");
55  const char *joint_name = jel ? jel->Attribute("name") : NULL;
56  if (!joint_name)
57  {
58  ROS_ERROR("SimpleTransmission did not specify joint name");
59  return false;
60  }
61 
62  const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
63  if (!joint)
64  {
65  ROS_ERROR("SimpleTransmission could not find joint named \"%s\"", joint_name);
66  return false;
67  }
68  joint_names_.push_back(joint_name);
69 
70  TiXmlElement *ael = elt->FirstChildElement("actuator");
71  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
72  Actuator *a;
73  if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
74  {
75  ROS_ERROR("SimpleTransmission could not find actuator named \"%s\"", actuator_name);
76  return false;
77  }
78  a->command_.enable_ = true;
79  actuator_names_.push_back(actuator_name);
80 
81  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
82 
83  // Get screw joint informations
84  for (TiXmlElement *j = elt->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
85  {
86  const char *joint_name = j->Attribute("name");
87  if (!joint_name)
88  {
89  ROS_ERROR("SimpleTransmission did not specify screw joint name");
90  use_simulated_actuated_joint_=false;
91  }
92  else
93  {
94  const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
95  if (!joint)
96  {
97  ROS_ERROR("SimpleTransmission could not find screw joint named \"%s\"", joint_name);
98  use_simulated_actuated_joint_=false;
99  }
100  else
101  {
102  use_simulated_actuated_joint_=true;
103  joint_names_.push_back(joint_name); // The first joint is the gap joint
104 
105  // get the thread pitch
106  const char *simulated_reduction = j->Attribute("simulated_reduction");
107  if (!simulated_reduction)
108  {
109  ROS_ERROR("SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
110  return false;
111  }
112  try
113  {
114  simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
115  }
116  catch (boost::bad_lexical_cast &e)
117  {
118  ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
119  return false;
120  }
121  }
122  }
123  }
124  return true;
125 }
126 
127 bool SimpleTransmission::initXml(TiXmlElement *elt)
128 {
129  const char *name = elt->Attribute("name");
130  name_ = name ? name : "";
131 
132  TiXmlElement *jel = elt->FirstChildElement("joint");
133  const char *joint_name = jel ? jel->Attribute("name") : NULL;
134  if (!joint_name)
135  {
136  ROS_ERROR("SimpleTransmission did not specify joint name");
137  return false;
138  }
139  joint_names_.push_back(joint_name);
140 
141  TiXmlElement *ael = elt->FirstChildElement("actuator");
142  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
143  if (!actuator_name)
144  {
145  ROS_ERROR("SimpleTransmission could not find actuator named \"%s\"", actuator_name);
146  return false;
147  }
148  actuator_names_.push_back(actuator_name);
149 
150  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
151 
152  // Get screw joint informations
153  for (TiXmlElement *j = elt->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
154  {
155  const char *joint_name = j->Attribute("name");
156  if (!joint_name)
157  {
158  ROS_ERROR("SimpleTransmission screw joint did not specify joint name");
159  use_simulated_actuated_joint_=false;
160  }
161  else
162  {
163  use_simulated_actuated_joint_=true;
164  joint_names_.push_back(joint_name); // The first joint is the gap joint
165 
166  // get the thread pitch
167  const char *simulated_reduction = j->Attribute("simulated_reduction");
168  if (!simulated_reduction)
169  {
170  ROS_ERROR("SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
171  return false;
172  }
173  try
174  {
175  simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
176  }
177  catch (boost::bad_lexical_cast &e)
178  {
179  ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
180  return false;
181  }
182  }
183  }
184  return true;
185 }
186 
188  std::vector<Actuator*>& as, std::vector<JointState*>& js)
189 {
190  assert(as.size() == 1);
191  if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
192  else {assert(js.size() == 1);}
193  js[0]->position_ = (as[0]->state_.position_ / mechanical_reduction_) + js[0]->reference_position_;
194  js[0]->velocity_ = as[0]->state_.velocity_ / mechanical_reduction_;
195  js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
196 
197  if (use_simulated_actuated_joint_)
198  {
199  // screw joint state is not important to us, fill with zeros
200  js[1]->position_ = 0;
201  js[1]->velocity_ = 0;
202  js[1]->measured_effort_ = 0;
203  js[1]->reference_position_ = 0;
204  js[1]->calibrated_ = true; // treat passive simulation joints as "calibrated"
205  }
206 }
207 
209  std::vector<JointState*>& js, std::vector<Actuator*>& as)
210 {
211  assert(as.size() == 1);
212  if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
213  else {assert(js.size() == 1);}
214  as[0]->state_.position_ = (js[0]->position_ - js[0]->reference_position_) * mechanical_reduction_;
215  as[0]->state_.velocity_ = js[0]->velocity_ * mechanical_reduction_;
216  as[0]->state_.last_measured_effort_ = js[0]->measured_effort_ / mechanical_reduction_;
217 
218  // Update the timing (making sure it's initialized).
219  if (! simulated_actuator_timestamp_initialized_)
220  {
221  // Set the time stamp to zero (it is measured relative to the start time).
222  as[0]->state_.sample_timestamp_ = ros::Duration(0);
223 
224  // Try to set the start time. Only then do we claim initialized.
225  if (ros::isStarted())
226  {
227  simulated_actuator_start_time_ = ros::Time::now();
228  simulated_actuator_timestamp_initialized_ = true;
229  }
230  }
231  else
232  {
233  // Measure the time stamp relative to the start time.
234  as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
235  }
236  // Set the historical (double) timestamp accordingly.
237  as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
238 
239  // simulate calibration sensors by filling out actuator states
240  this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
241 }
242 
244  std::vector<JointState*>& js, std::vector<Actuator*>& as)
245 {
246  assert(as.size() == 1);
247  if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
248  else {assert(js.size() == 1);}
249  as[0]->command_.enable_ = true;
250  as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
251 }
252 
254  std::vector<Actuator*>& as, std::vector<JointState*>& js)
255 {
256  assert(as.size() == 1);
257  if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
258  else {assert(js.size() == 1);}
259  if (use_simulated_actuated_joint_)
260  {
261  // set screw joint effort if simulated
262  js[1]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_/simulated_reduction_;
263  }
264  else
265  js[0]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_;
266 }
267 
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.
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.
void propagateEffort(std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &)
Uses commanded joint efforts to fill out commanded motor currents.
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::SimpleTransmission, pr2_mechanism_model::Transmission) bool SimpleTransmission
ROSCPP_DECL bool isStarted()
static Time now()
void propagatePosition(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses encoder data to fill out joint position and velocities.
#define ROS_ERROR(...)
This class provides the controllers with an interface to the robot model.
Definition: robot.h:78


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