50 std::vector<double> act_reduction;
54 std::vector<double> jnt_reduction;
55 std::vector<double> jnt_offset;
56 bool ignore_transmission_for_absolute_encoders;
60 ignore_transmission_for_absolute_encoders);
69 ignore_transmission_for_absolute_encoders,
77 demangledTypeName<DifferentialTransmission>()<<
"'. " << ex.
what());
83 std::vector<double>& actuator_reduction)
85 const std::string ACTUATOR1_ROLE =
"actuator1";
86 const std::string ACTUATOR2_ROLE =
"actuator2";
88 std::vector<TiXmlElement> act_elements(2,
"");
89 std::vector<std::string> act_names(2);
90 std::vector<std::string> act_roles(2);
92 for (
unsigned int i = 0; i < 2; ++i)
95 act_names[i] = transmission_info.
actuators_[i].name_;
101 std::string& act_role = act_roles[i];
104 transmission_info.
name_,
107 if (act_role_status !=
SUCCESS) {
return false;}
110 if (ACTUATOR1_ROLE != act_role && ACTUATOR2_ROLE != act_role)
113 "' does not specify a valid <role> element. Got '" << act_role <<
"', expected '" <<
114 ACTUATOR1_ROLE <<
"' or '" << ACTUATOR2_ROLE <<
"'.");
120 if (act_roles[0] == act_roles[1])
123 "' of transmission '" << transmission_info.
name_ <<
124 "' must have different roles. Both specify '" << act_roles[0] <<
"'.");
129 std::vector<unsigned int> id_map(2);
130 if (ACTUATOR1_ROLE == act_roles[0])
143 actuator_reduction.resize(2);
144 for (
unsigned int i = 0; i < 2; ++i)
146 const unsigned int id = id_map[i];
149 transmission_info.
name_,
151 actuator_reduction[i]);
152 if (reduction_status !=
SUCCESS) {
return false;}
159 std::vector<double>& joint_reduction,
160 std::vector<double>& joint_offset,
161 bool& ignore_transmission_for_absolute_encoders)
163 const std::string JOINT1_ROLE =
"joint1";
164 const std::string JOINT2_ROLE =
"joint2";
166 std::vector<TiXmlElement> jnt_elements(2,
"");
167 std::vector<std::string> jnt_names(2);
168 std::vector<std::string> jnt_roles(2);
170 for (
unsigned int i = 0; i < 2; ++i)
173 jnt_names[i] = transmission_info.
joints_[i].name_;
179 std::string& jnt_role = jnt_roles[i];
182 transmission_info.
name_,
185 if (jnt_role_status !=
SUCCESS) {
return false;}
188 if (JOINT1_ROLE != jnt_role && JOINT2_ROLE != jnt_role)
191 "' does not specify a valid <role> element. Got '" << jnt_role <<
"', expected '" <<
192 JOINT1_ROLE <<
"' or '" << JOINT2_ROLE <<
"'.");
198 if (jnt_roles[0] == jnt_roles[1])
201 "' of transmission '" << transmission_info.
name_ <<
202 "' must have different roles. Both specify '" << jnt_roles[0] <<
"'.");
207 std::vector<unsigned int> id_map(2);
208 if (JOINT1_ROLE == jnt_roles[0])
221 joint_reduction.resize(2, 1.0);
222 joint_offset.resize(2, 0.0);
223 for (
unsigned int i = 0; i < 2; ++i)
225 const unsigned int id = id_map[i];
231 transmission_info.
name_,
234 if (reduction_status ==
BAD_TYPE) {
return false;}
240 transmission_info.
name_,
243 if (offset_status ==
BAD_TYPE) {
return false;}
247 ignore_transmission_for_absolute_encoders =
true;
248 for (
unsigned int i = 0; i < 2; ++i)
250 const TiXmlElement* role_el = jnt_elements[i].FirstChildElement(
"ignoreTransmissionAbsoluteEncoder");
251 ignore_transmission_for_absolute_encoders &= (role_el !=
nullptr);
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)
std::string demangledTypeName()
static ParseStatus getActuatorReduction(const TiXmlElement &parent_el, const std::string &actuator_name, const std::string &transmission_name, bool required, double &reduction)
const char * what() const noexcept override
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
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)
std::vector< JointInfo > joints_
Implementation of a differential transmission.
static bool checkJointDimension(const TransmissionInfo &transmission_info, const unsigned int expected_dim)