5 #ifndef __pinocchio_parsers_mjcf_graph_hpp__
6 #define __pinocchio_parsers_mjcf_graph_hpp__
12 #include <boost/property_tree/xml_parser.hpp>
13 #include <boost/property_tree/ptree.hpp>
14 #include <boost/foreach.hpp>
15 #include <boost/math/constants/constants.hpp>
16 #include <boost/filesystem.hpp>
17 #include <boost/logic/tribool.hpp>
18 #include <boost/lexical_cast.hpp>
23 #include <unordered_map>
42 bool autolimits =
true;
45 bool strippath =
false;
52 double angle_converter = boost::math::constants::pi<double>() / 180.0;
59 double boundInertia = 0;
62 boost::logic::tribool inertiafromgeom = boost::logic::indeterminate;
67 double convertAngle(
const double & angle_)
const;
72 Eigen::Matrix3d convertEuler(
const Eigen::Vector3d & angles)
const;
137 double armature = 0.;
139 double frictionLoss = 0.;
144 const double infty = std::numeric_limits<double>::infinity();
145 maxVel = Eigen::VectorXd::Constant(1, infty);
146 maxEffort = Eigen::VectorXd::Constant(1, infty);
147 minConfig = Eigen::VectorXd::Constant(1, -infty);
148 maxConfig = Eigen::VectorXd::Constant(1, infty);
149 springStiffness = Eigen::VectorXd::Constant(1,
v);
150 springReference = Eigen::VectorXd::Constant(1,
v);
152 friction = Eigen::VectorXd::Constant(1, 0.);
153 damping = Eigen::VectorXd::Constant(1, 0.);
160 template<
int Nq,
int Nv>
168 template<
int Nq,
int Nv>
179 std::string jointName =
"free";
184 Eigen::Vector3d
axis = Eigen::Vector3d::UnitZ();
189 std::string jointType =
"hinge";
202 goThroughElement(
const ptree & el,
bool use_limits,
const MjcfCompiler & currentCompiler);
208 Eigen::Vector3d
scale = Eigen::Vector3d::Constant(1);
221 Eigen::Vector2d
gridsize = Eigen::Vector2d::Constant(1);
229 Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
231 float reflectance = 0;
233 float shininess = 0.5;
235 float specular = 0.5;
243 void goThroughElement(
const ptree & el);
262 std::string geomType =
"sphere";
281 Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
289 double density = 1000;
291 bool shellinertia =
false;
307 void computeInertia();
313 void goThroughElement(
const ptree & el,
const MjcfGraph & currentGraph);
325 void goThroughElement(
const ptree & el,
const MjcfGraph & currentGraph);
334 typedef std::unordered_map<std::string, MjcfBody>
BodyMap_t;
335 typedef std::unordered_map<std::string, MjcfClass>
ClassMap_t;
337 typedef std::unordered_map<std::string, MjcfMesh>
MeshMap_t;
339 typedef std::unordered_map<std::string, Eigen::VectorXd>
ConfigMap_t;
370 typedef pinocchio::urdf::details::
371 UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl>
378 : modelPath(modelPath)
379 , urdfVisitor(urdfVisitor)
386 SE3 convertPosition(
const ptree & el)
const;
402 void parseJointAndBody(
404 const boost::optional<std::string> & childClass,
405 const std::string & parentName =
"");
409 void parseCompiler(
const ptree & el);
413 void parseTexture(
const ptree & el);
417 void parseMaterial(
const ptree & el);
421 void parseMesh(
const ptree & el);
425 void parseAsset(
const ptree & el);
429 void parseKeyFrame(
const ptree & el);
436 void parseGraphFromXML(
const std::string & xmlStr);
445 template<
typename TypeX,
typename TypeY,
typename TypeZ,
typename TypeUnaligned>
446 JointModel createJoint(
const Eigen::Vector3d & axis);
458 void fillModel(
const std::string & nameOfBody);
465 void fillReferenceConfig(
const MjcfBody & currentBody);
470 void addKeyFrame(
const Eigen::VectorXd & keyframe,
const std::string & keyName);
485 std::istringstream posStream(
str);
486 posStream.exceptions(std::ios::failbit);
494 Eigen::Matrix<double, N, 1> vector;
495 for (
int i = 0;
i <
N;
i++)
504 std::vector<double> vector;
506 while (!stream.eof())
509 vector.push_back(elem);
512 Eigen::VectorXd returnVector(vector.size());
513 for (std::size_t
i = 0;
i < vector.size();
i++)
523 #endif // __pinocchio_parsers_mjcf_graph_hpp__