17 #ifndef URDF2GAZEBO_HH
18 #define URDF2GAZEBO_HH
20 #include <urdf_model/model.h>
21 #include <urdf_model/link.h>
33 #include "ode/rotation.h"
56 setStaticFlag =
false;
58 isDampingFactor =
false;
70 isInitialJointPosition =
false;
71 isFudgeFactor =
false;
72 provideFeedback =
false;
86 initialJointPosition = 0;
91 : material(ge.material), fdir1(ge.fdir1),
92 oldLinkName(ge.oldLinkName),
93 reductionTransform(ge.reductionTransform),
96 setStaticFlag = ge.setStaticFlag;
98 isDampingFactor = ge.isDampingFactor;
99 isMaxVel = ge.isMaxVel;
100 isMinDepth = ge.isMinDepth;
105 selfCollide = ge.selfCollide;
106 isLaserRetro = ge.isLaserRetro;
107 isStopCfm = ge.isStopCfm;
108 isStopErp = ge.isStopErp;
109 isInitialJointPosition = ge.isInitialJointPosition;
110 isFudgeFactor = ge.isFudgeFactor;
111 provideFeedback = ge.provideFeedback;
112 cfmDamping = ge.cfmDamping;
113 dampingFactor = ge.dampingFactor;
115 minDepth = ge.minDepth;
120 laserRetro = ge.laserRetro;
121 stopCfm = ge.stopCfm;
122 stopErp = ge.stopErp;
123 initialJointPosition = ge.initialJointPosition;
124 fudgeFactor = ge.fudgeFactor;
128 private: std::string material;
130 private: std::string fdir1;
133 private: std::string oldLinkName;
137 private: std::vector<TiXmlElement*> blobs;
140 private:
bool setStaticFlag;
141 private:
bool gravity;
142 private:
bool isDampingFactor;
143 private:
double dampingFactor;
144 private:
bool isMaxVel;
145 private:
double maxVel;
146 private:
bool isMinDepth;
147 private:
double minDepth;
148 private:
bool selfCollide;
151 private:
bool isMu1, isMu2, isKp, isKd;
152 private:
double mu1, mu2, kp, kd;
153 private:
bool isLaserRetro;
154 private:
double laserRetro;
157 private:
bool isStopCfm, isStopErp, isInitialJointPosition, isFudgeFactor;
158 private:
double stopCfm, stopErp, initialJointPosition, fudgeFactor;
159 private:
bool provideFeedback;
160 private:
bool cfmDamping;
176 public: TiXmlDocument
InitModelDoc(TiXmlDocument* _xmlDoc);
181 public: TiXmlDocument
InitModelFile(
const std::string &_filename);
189 bool _enforceLimits =
true);
195 private: urdf::Vector3 ParseVector3(TiXmlNode* _key,
double _scale = 1.0);
201 private: std::string Values2str(
unsigned int _count,
const double *_values);
206 private: std::string Vector32Str(
const urdf::Vector3 _vector);
212 private:
void AddKeyValue(TiXmlElement *_elem,
const std::string &_key,
213 const std::string &_value);
216 private:
void AddTransform(TiXmlElement *_elem,
217 const::gazebo::math::Pose& _transform);
223 private:
void PrintMass(
const std::string &_linkName, dMass _mass);
226 private: std::string GetKeyValueAsString(TiXmlElement* _elem);
230 private:
void ParseGazeboExtension(TiXmlDocument &_urdfXml);
233 private:
void InsertGazeboExtensionCollision(TiXmlElement *_elem,
234 const std::string &_linkName);
237 private:
void InsertGazeboExtensionVisual(TiXmlElement *_elem,
238 const std::string &_linkName);
241 private:
void InsertGazeboExtensionLink(TiXmlElement *_elem,
242 const std::string &_linkName);
245 private:
void InsertGazeboExtensionJoint(TiXmlElement *_elem,
246 const std::string &_jointName);
249 private:
void InsertGazeboExtensionRobot(TiXmlElement *_elem);
252 private:
void ListGazeboExtensions();
255 private:
void ListGazeboExtensions(
const std::string &_reference);
259 private:
void ReduceFixedJoints(TiXmlElement *_root,
UrdfLinkPtr _link);
262 private:
void ReduceInertialToParent(
UrdfLinkPtr _link);
265 private:
void ReduceVisualsToParent(
UrdfLinkPtr _link);
268 private:
void ReduceCollisionsToParent(
UrdfLinkPtr _link);
271 private:
void ReduceJointsToParent(
UrdfLinkPtr _link);
274 private:
void ReduceVisualToParent(
UrdfLinkPtr _link,
275 const std::string &_groupName,
279 private:
void ReduceCollisionToParent(
UrdfLinkPtr _link,
280 const std::string &_groupName,
293 private:
void ReduceGazeboExtensionToParent(
UrdfLinkPtr _link);
302 private:
void ReduceGazeboExtensionPluginFrameReplace(
303 std::vector<TiXmlElement*>::iterator _blobIt,
309 private:
void ReduceGazeboExtensionProjectorFrameReplace(
310 std::vector<TiXmlElement*>::iterator _blobIt,
315 private:
void ReduceGazeboExtensionGripperFrameReplace(
316 std::vector<TiXmlElement*>::iterator _blobIt,
321 private:
void ReduceGazeboExtensionJointFrameReplace(
322 std::vector<TiXmlElement*>::iterator _blobIt,
327 private:
void ReduceGazeboExtensionContactSensorFrameReplace(
328 std::vector<TiXmlElement*>::iterator _blobIt,
337 private:
void ReduceGazeboExtensionSensorTransformReduction(
338 std::vector<TiXmlElement*>::iterator _blobIt,
343 private:
void ReduceGazeboExtensionProjectorTransformReduction(
344 std::vector<TiXmlElement*>::iterator _blobIt,
348 private: urdf::Pose TransformToParentFrame(
349 urdf::Pose _transformInLinkFrame,
350 urdf::Pose _parentToLinkTransform);
354 urdf::Pose _parentToLinkTransform);
362 urdf::Pose _parentToLinkTransform);
370 private: std::string GetGeometryBoundingBox(
371 boost::shared_ptr<urdf::Geometry> _geometry,
double *_sizeVals);
375 private:
void PrintCollisionGroups(
UrdfLinkPtr _link);
378 private:
void CreateSDF(TiXmlElement *_root,
383 private:
void CreateLink(TiXmlElement *_root,
388 private:
void CreateCollisions(TiXmlElement* _elem,
392 private:
void CreateVisuals(TiXmlElement* _elem,
396 private:
void CreateInertial(TiXmlElement *_elem,
400 private:
void CreateCollision(TiXmlElement* _elem,
403 const std::string &_oldLinkName = std::string(
""));
406 private:
void CreateVisual(TiXmlElement *_elem,
409 const std::string &_oldLinkName = std::string(
""));
412 private:
void CreateJoint(TiXmlElement *_root,
417 private:
void CreateGeometry(TiXmlElement* _elem,
418 boost::shared_ptr<urdf::Geometry> _geometry);
420 private: std::map<std::string, std::vector<GazeboExtension*> > extensions;
422 private:
bool enforceLimits;
423 private:
bool reduceFixedJoints;