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


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Tue Mar 7 2023 03:54:53