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  const bool jnt_config_ok = getJointConfig(transmission_info,
57  jnt_reduction,
58  jnt_offset);
59 
60  if (!jnt_config_ok) {return TransmissionSharedPtr();}
61 
62  // Transmission instance
63  try
64  {
65  TransmissionSharedPtr transmission(new DifferentialTransmission(act_reduction,
66  jnt_reduction,
67  jnt_offset));
68  return transmission;
69  }
70  catch(const TransmissionInterfaceException& ex)
71  {
73  ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" << transmission_info.name_ << "' of type '" <<
74  demangledTypeName<DifferentialTransmission>()<< "'. " << ex.what());
75  return TransmissionSharedPtr();
76  }
77 }
78 
80  std::vector<double>& actuator_reduction)
81 {
82  const std::string ACTUATOR1_ROLE = "actuator1";
83  const std::string ACTUATOR2_ROLE = "actuator2";
84 
85  std::vector<TiXmlElement> act_elements(2,"");
86  std::vector<std::string> act_names(2);
87  std::vector<std::string> act_roles(2);
88 
89  for (unsigned int i = 0; i < 2; ++i)
90  {
91  // Actuator name
92  act_names[i] = transmission_info.actuators_[i].name_;
93 
94  // Actuator xml element
95  act_elements[i] = loadXmlElement(transmission_info.actuators_[i].xml_element_);
96 
97  // Populate role string
98  std::string& act_role = act_roles[i];
99  const ParseStatus act_role_status = getActuatorRole(act_elements[i],
100  act_names[i],
101  transmission_info.name_,
102  true, // Required
103  act_role);
104  if (act_role_status != SUCCESS) {return false;}
105 
106  // Validate role string
107  if (ACTUATOR1_ROLE != act_role && ACTUATOR2_ROLE != act_role)
108  {
109  ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << act_names[i] << "' of transmission '" << transmission_info.name_ <<
110  "' does not specify a valid <role> element. Got '" << act_role << "', expected '" <<
111  ACTUATOR1_ROLE << "' or '" << ACTUATOR2_ROLE << "'.");
112  return false;
113  }
114  }
115 
116  // Roles must be different
117  if (act_roles[0] == act_roles[1])
118  {
119  ROS_ERROR_STREAM_NAMED("parser", "Actuators '" << act_names[0] << "' and '" << act_names[1] <<
120  "' of transmission '" << transmission_info.name_ <<
121  "' must have different roles. Both specify '" << act_roles[0] << "'.");
122  return false;
123  }
124 
125  // Indices sorted according to role
126  std::vector<unsigned int> id_map(2);
127  if (ACTUATOR1_ROLE == act_roles[0])
128  {
129  id_map[0] = 0;
130  id_map[1] = 1;
131 
132  }
133  else
134  {
135  id_map[0] = 1;
136  id_map[1] = 0;
137  }
138 
139  // Parse required mechanical reductions
140  actuator_reduction.resize(2);
141  for (unsigned int i = 0; i < 2; ++i)
142  {
143  const unsigned int id = id_map[i];
144  const ParseStatus reduction_status = getActuatorReduction(act_elements[id],
145  act_names[id],
146  transmission_info.name_,
147  true, // Required
148  actuator_reduction[i]);
149  if (reduction_status != SUCCESS) {return false;}
150  }
151 
152  return true;
153 }
154 
156  std::vector<double>& joint_reduction,
157  std::vector<double>& joint_offset)
158 {
159  const std::string JOINT1_ROLE = "joint1";
160  const std::string JOINT2_ROLE = "joint2";
161 
162  std::vector<TiXmlElement> jnt_elements(2,"");
163  std::vector<std::string> jnt_names(2);
164  std::vector<std::string> jnt_roles(2);
165 
166  for (unsigned int i = 0; i < 2; ++i)
167  {
168  // Joint name
169  jnt_names[i] = transmission_info.joints_[i].name_;
170 
171  // Joint xml element
172  jnt_elements[i] = loadXmlElement(transmission_info.joints_[i].xml_element_);
173 
174  // Populate role string
175  std::string& jnt_role = jnt_roles[i];
176  const ParseStatus jnt_role_status = getJointRole(jnt_elements[i],
177  jnt_names[i],
178  transmission_info.name_,
179  true, // Required
180  jnt_role);
181  if (jnt_role_status != SUCCESS) {return false;}
182 
183  // Validate role string
184  if (JOINT1_ROLE != jnt_role && JOINT2_ROLE != jnt_role)
185  {
186  ROS_ERROR_STREAM_NAMED("parser", "Joint '" << jnt_names[i] << "' of transmission '" << transmission_info.name_ <<
187  "' does not specify a valid <role> element. Got '" << jnt_role << "', expected '" <<
188  JOINT1_ROLE << "' or '" << JOINT2_ROLE << "'.");
189  return false;
190  }
191  }
192 
193  // Roles must be different
194  if (jnt_roles[0] == jnt_roles[1])
195  {
196  ROS_ERROR_STREAM_NAMED("parser", "Joints '" << jnt_names[0] << "' and '" << jnt_names[1] <<
197  "' of transmission '" << transmission_info.name_ <<
198  "' must have different roles. Both specify '" << jnt_roles[0] << "'.");
199  return false;
200  }
201 
202  // Indices sorted according to role
203  std::vector<unsigned int> id_map(2);
204  if (JOINT1_ROLE == jnt_roles[0])
205  {
206  id_map[0] = 0;
207  id_map[1] = 1;
208 
209  }
210  else
211  {
212  id_map[0] = 1;
213  id_map[1] = 0;
214  }
215 
216  // Joint configuration
217  joint_reduction.resize(2, 1.0);
218  joint_offset.resize(2, 0.0);
219  for (unsigned int i = 0; i < 2; ++i)
220  {
221  const unsigned int id = id_map[i];
222 
223  // Parse optional mechanical reductions. Even though it's optional --and to avoid surprises-- we fail if the element
224  // is specified but is of the wrong type
225  const ParseStatus reduction_status = getJointReduction(jnt_elements[id],
226  jnt_names[id],
227  transmission_info.name_,
228  false, // Optional
229  joint_reduction[i]);
230  if (reduction_status == BAD_TYPE) {return false;}
231 
232  // Parse optional joint offset. Even though it's optional --and to avoid surprises-- we fail if the element is
233  // specified but is of the wrong type
234  const ParseStatus offset_status = getJointOffset(jnt_elements[id],
235  jnt_names[id],
236  transmission_info.name_,
237  false, // Optional
238  joint_offset[i]);
239  if (offset_status == BAD_TYPE) {return false;}
240  }
241 
242  return true;
243 }
244 
245 } // namespace
246 
static TiXmlElement loadXmlElement(const std::string &element_str)
#define ROS_ERROR_STREAM_NAMED(name, args)
Abstract interface for loading transmission instances from configuration data.
static bool getJointConfig(const TransmissionInfo &transmission_info, std::vector< double > &joint_reduction, std::vector< double > &joint_offset)
Class for loading a differential transmission instance from configuration data.
Contains semantic info about a given transmission loaded from XML (URDF)
boost::shared_ptr< Transmission > TransmissionSharedPtr
Definition: transmission.h:165
static ParseStatus getActuatorReduction(const TiXmlElement &parent_el, const std::string &actuator_name, const std::string &transmission_name, bool required, double &reduction)
TransmissionSharedPtr load(const TransmissionInfo &transmission_info)
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::vector< ActuatorInfo > actuators_
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
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)
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 Apr 20 2020 03:52:15