differential_transmission_loader.cpp
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
28 // ROS
29 #include <ros/console.h>
30 
31 // Pluginlib
33 
34 // ros_control
38 
39 namespace transmission_interface
40 {
41 
44 {
45  // Transmission should contain only one actuator/joint
46  if (!checkActuatorDimension(transmission_info, 2)) {return TransmissionSharedPtr();}
47  if (!checkJointDimension(transmission_info, 2)) {return TransmissionSharedPtr();}
48 
49  // Get actuator and joint configuration sorted by role: [actuator1, actuator2] and [joint1, joint2]
50  std::vector<double> act_reduction;
51  const bool act_config_ok = getActuatorConfig(transmission_info, act_reduction);
52  if (!act_config_ok) {return TransmissionSharedPtr();}
53 
54  std::vector<double> jnt_reduction;
55  std::vector<double> jnt_offset;
56  bool ignore_transmission_for_absolute_encoders;
57  const bool jnt_config_ok = getJointConfig(transmission_info,
58  jnt_reduction,
59  jnt_offset,
60  ignore_transmission_for_absolute_encoders);
61 
62  if (!jnt_config_ok) {return TransmissionSharedPtr();}
63 
64  // Transmission instance
65  try
66  {
67  TransmissionSharedPtr transmission(new DifferentialTransmission(act_reduction,
68  jnt_reduction,
69  ignore_transmission_for_absolute_encoders,
70  jnt_offset));
71  return transmission;
72  }
73  catch(const TransmissionInterfaceException& ex)
74  {
76  ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" << transmission_info.name_ << "' of type '" <<
77  demangledTypeName<DifferentialTransmission>()<< "'. " << ex.what());
78  return TransmissionSharedPtr();
79  }
80 }
81 
83  std::vector<double>& actuator_reduction)
84 {
85  const std::string ACTUATOR1_ROLE = "actuator1";
86  const std::string ACTUATOR2_ROLE = "actuator2";
87 
88  std::vector<TiXmlElement> act_elements(2,"");
89  std::vector<std::string> act_names(2);
90  std::vector<std::string> act_roles(2);
91 
92  for (unsigned int i = 0; i < 2; ++i)
93  {
94  // Actuator name
95  act_names[i] = transmission_info.actuators_[i].name_;
96 
97  // Actuator xml element
98  act_elements[i] = loadXmlElement(transmission_info.actuators_[i].xml_element_);
99 
100  // Populate role string
101  std::string& act_role = act_roles[i];
102  const ParseStatus act_role_status = getActuatorRole(act_elements[i],
103  act_names[i],
104  transmission_info.name_,
105  true, // Required
106  act_role);
107  if (act_role_status != SUCCESS) {return false;}
108 
109  // Validate role string
110  if (ACTUATOR1_ROLE != act_role && ACTUATOR2_ROLE != act_role)
111  {
112  ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << act_names[i] << "' of transmission '" << transmission_info.name_ <<
113  "' does not specify a valid <role> element. Got '" << act_role << "', expected '" <<
114  ACTUATOR1_ROLE << "' or '" << ACTUATOR2_ROLE << "'.");
115  return false;
116  }
117  }
118 
119  // Roles must be different
120  if (act_roles[0] == act_roles[1])
121  {
122  ROS_ERROR_STREAM_NAMED("parser", "Actuators '" << act_names[0] << "' and '" << act_names[1] <<
123  "' of transmission '" << transmission_info.name_ <<
124  "' must have different roles. Both specify '" << act_roles[0] << "'.");
125  return false;
126  }
127 
128  // Indices sorted according to role
129  std::vector<unsigned int> id_map(2);
130  if (ACTUATOR1_ROLE == act_roles[0])
131  {
132  id_map[0] = 0;
133  id_map[1] = 1;
134 
135  }
136  else
137  {
138  id_map[0] = 1;
139  id_map[1] = 0;
140  }
141 
142  // Parse required mechanical reductions
143  actuator_reduction.resize(2);
144  for (unsigned int i = 0; i < 2; ++i)
145  {
146  const unsigned int id = id_map[i];
147  const ParseStatus reduction_status = getActuatorReduction(act_elements[id],
148  act_names[id],
149  transmission_info.name_,
150  true, // Required
151  actuator_reduction[i]);
152  if (reduction_status != SUCCESS) {return false;}
153  }
154 
155  return true;
156 }
157 
159  std::vector<double>& joint_reduction,
160  std::vector<double>& joint_offset,
161  bool& ignore_transmission_for_absolute_encoders)
162 {
163  const std::string JOINT1_ROLE = "joint1";
164  const std::string JOINT2_ROLE = "joint2";
165 
166  std::vector<TiXmlElement> jnt_elements(2,"");
167  std::vector<std::string> jnt_names(2);
168  std::vector<std::string> jnt_roles(2);
169 
170  for (unsigned int i = 0; i < 2; ++i)
171  {
172  // Joint name
173  jnt_names[i] = transmission_info.joints_[i].name_;
174 
175  // Joint xml element
176  jnt_elements[i] = loadXmlElement(transmission_info.joints_[i].xml_element_);
177 
178  // Populate role string
179  std::string& jnt_role = jnt_roles[i];
180  const ParseStatus jnt_role_status = getJointRole(jnt_elements[i],
181  jnt_names[i],
182  transmission_info.name_,
183  true, // Required
184  jnt_role);
185  if (jnt_role_status != SUCCESS) {return false;}
186 
187  // Validate role string
188  if (JOINT1_ROLE != jnt_role && JOINT2_ROLE != jnt_role)
189  {
190  ROS_ERROR_STREAM_NAMED("parser", "Joint '" << jnt_names[i] << "' of transmission '" << transmission_info.name_ <<
191  "' does not specify a valid <role> element. Got '" << jnt_role << "', expected '" <<
192  JOINT1_ROLE << "' or '" << JOINT2_ROLE << "'.");
193  return false;
194  }
195  }
196 
197  // Roles must be different
198  if (jnt_roles[0] == jnt_roles[1])
199  {
200  ROS_ERROR_STREAM_NAMED("parser", "Joints '" << jnt_names[0] << "' and '" << jnt_names[1] <<
201  "' of transmission '" << transmission_info.name_ <<
202  "' must have different roles. Both specify '" << jnt_roles[0] << "'.");
203  return false;
204  }
205 
206  // Indices sorted according to role
207  std::vector<unsigned int> id_map(2);
208  if (JOINT1_ROLE == jnt_roles[0])
209  {
210  id_map[0] = 0;
211  id_map[1] = 1;
212 
213  }
214  else
215  {
216  id_map[0] = 1;
217  id_map[1] = 0;
218  }
219 
220  // Joint configuration
221  joint_reduction.resize(2, 1.0);
222  joint_offset.resize(2, 0.0);
223  for (unsigned int i = 0; i < 2; ++i)
224  {
225  const unsigned int id = id_map[i];
226 
227  // Parse optional mechanical reductions. Even though it's optional --and to avoid surprises-- we fail if the element
228  // is specified but is of the wrong type
229  const ParseStatus reduction_status = getJointReduction(jnt_elements[id],
230  jnt_names[id],
231  transmission_info.name_,
232  false, // Optional
233  joint_reduction[i]);
234  if (reduction_status == BAD_TYPE) {return false;}
235 
236  // Parse optional joint offset. Even though it's optional --and to avoid surprises-- we fail if the element is
237  // specified but is of the wrong type
238  const ParseStatus offset_status = getJointOffset(jnt_elements[id],
239  jnt_names[id],
240  transmission_info.name_,
241  false, // Optional
242  joint_offset[i]);
243  if (offset_status == BAD_TYPE) {return false;}
244  }
245 
246  // Parse if transmision has to be ignored for absolute encoders
247  ignore_transmission_for_absolute_encoders = true;
248  for (unsigned int i = 0; i < 2; ++i)
249  {
250  const TiXmlElement* role_el = jnt_elements[i].FirstChildElement("ignoreTransmissionAbsoluteEncoder");
251  ignore_transmission_for_absolute_encoders &= (role_el != nullptr);
252  }
253 
254  return true;
255 }
256 
257 } // namespace
258 
static TiXmlElement loadXmlElement(const std::string &element_str)
#define ROS_ERROR_STREAM_NAMED(name, args)
Abstract interface for loading transmission instances from configuration data.
Class for loading a differential transmission instance from configuration data.
Contains semantic info about a given transmission loaded from XML (URDF)
static ParseStatus getActuatorReduction(const TiXmlElement &parent_el, const std::string &actuator_name, const std::string &transmission_name, bool required, double &reduction)
static ParseStatus getActuatorRole(const TiXmlElement &parent_el, const std::string &actuator_name, const std::string &transmission_name, bool required, std::string &role)
static bool checkActuatorDimension(const TransmissionInfo &transmission_info, const unsigned int expected_dim)
static ParseStatus getJointRole(const TiXmlElement &parent_el, const std::string &joint_name, const std::string &transmission_name, bool required, std::string &role)
static ParseStatus getJointReduction(const TiXmlElement &parent_el, const std::string &joint_name, const std::string &transmission_name, bool required, double &reduction)
std::shared_ptr< Transmission > TransmissionSharedPtr
Definition: transmission.h:184
TransmissionSharedPtr load(const TransmissionInfo &transmission_info) override
static bool getJointConfig(const TransmissionInfo &transmission_info, std::vector< double > &joint_reduction, std::vector< double > &joint_offset, bool &ignore_transmission_for_absolute_encoders)
std::vector< ActuatorInfo > actuators_
static ParseStatus getJointOffset(const TiXmlElement &parent_el, const std::string &joint_name, const std::string &transmission_name, bool required, double &offset)
static bool getActuatorConfig(const TransmissionInfo &transmission_info, std::vector< double > &actuator_reduction)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Implementation of a differential transmission.
static bool checkJointDimension(const TransmissionInfo &transmission_info, const unsigned int expected_dim)


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Mon Feb 28 2022 23:30:26