5 #ifndef __pinocchio_parsers_mjcf_graph_hpp__
6 #define __pinocchio_parsers_mjcf_graph_hpp__
8 #include "pinocchio/parsers/urdf.hpp"
9 #include "pinocchio/multibody/model.hpp"
10 #include "pinocchio/multibody/joint/joints.hpp"
11 #include "pinocchio/algorithm/contact-info.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>
43 bool autolimits =
true;
46 bool strippath =
false;
50 std::string texturedir;
53 double angle_converter = boost::math::constants::pi<double>() / 180.0;
55 Eigen::Matrix3d mapEulerAngles = Eigen::Matrix3d::Identity();
60 double boundInertia = 0;
63 boost::logic::tribool inertiafromgeom = boost::logic::indeterminate;
73 Eigen::Matrix3d
convertEuler(
const Eigen::Vector3d & angles)
const;
80 typedef boost::property_tree::ptree ptree;
83 std::string className;
95 std::string bodyParent;
97 std::string bodyClassName;
100 std::string childClass;
103 SE3 bodyPlacement = SE3::Identity();
105 Inertia bodyInertia = Inertia::Identity();
108 std::vector<MjcfJoint> jointChildren;
110 std::vector<MjcfGeom> geomChildren;
112 std::vector<MjcfSite> siteChildren;
119 Eigen::VectorXd maxEffort;
121 Eigen::VectorXd maxVel;
123 Eigen::VectorXd maxConfig;
125 Eigen::VectorXd minConfig;
128 Eigen::VectorXd springStiffness;
130 Eigen::VectorXd springReference;
133 Eigen::VectorXd friction;
135 Eigen::VectorXd damping;
138 Eigen::VectorXd armature;
140 double frictionLoss = 0.;
145 const double infty = std::numeric_limits<double>::infinity();
146 maxVel = Eigen::VectorXd::Constant(1, infty);
147 maxEffort = Eigen::VectorXd::Constant(1, infty);
148 minConfig = Eigen::VectorXd::Constant(1, -infty);
149 maxConfig = Eigen::VectorXd::Constant(1, infty);
150 springStiffness = Eigen::VectorXd::Constant(1, v);
151 springReference = Eigen::VectorXd::Constant(1, v);
152 friction = Eigen::VectorXd::Constant(1, 0.);
153 damping = Eigen::VectorXd::Constant(1, 0.);
154 armature = Eigen::VectorXd::Constant(1, 0.);
161 template<
int Nq,
int Nv>
169 template<
int Nq,
int Nv>
177 typedef boost::property_tree::ptree ptree;
180 std::string jointName =
"free";
182 SE3 jointPlacement = SE3::Identity();
185 Eigen::Vector3d axis = Eigen::Vector3d::UnitZ();
190 std::string jointType =
"hinge";
209 Eigen::Vector3d scale = Eigen::Vector3d::Constant(1);
211 std::string filePath;
213 Eigen::MatrixX3d vertices;
220 std::string textType =
"cube";
222 std::string filePath;
224 Eigen::Vector2d gridsize = Eigen::Vector2d::Constant(1);
230 typedef boost::property_tree::ptree ptree;
232 Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
234 float reflectance = 0;
236 float shininess = 0.5;
238 float specular = 0.5;
252 typedef boost::property_tree::ptree ptree;
262 std::string geomName;
265 std::string geomType =
"sphere";
268 TYPE geomKind = BOTH;
279 boost::optional<std::string> fromtoS;
281 Eigen::VectorXd size;
284 Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
287 std::string materialName;
289 std::string meshName;
292 double density = 1000;
294 bool shellinertia =
false;
297 SE3 geomPlacement = SE3::Identity();
299 Inertia geomInertia = Inertia::Identity();
301 boost::optional<double> massGeom;
321 typedef boost::property_tree::ptree ptree;
323 SE3 sitePlacement = SE3::Identity();
325 std::string siteName;
327 void fill(
const ptree & el,
const MjcfBody & currentBody,
const MjcfGraph & currentGraph);
328 void goThroughElement(
const ptree & el,
const MjcfGraph & currentGraph);
347 typedef boost::property_tree::ptree ptree;
368 Eigen::Vector3d anchor = Eigen::Vector3d::Zero();
384 typedef boost::property_tree::ptree ptree;
385 typedef std::vector<std::string> VectorOfStrings;
386 typedef std::unordered_map<std::string, MjcfBody> BodyMap_t;
387 typedef std::unordered_map<std::string, MjcfClass> ClassMap_t;
388 typedef std::unordered_map<std::string, MjcfMaterial> MaterialMap_t;
389 typedef std::unordered_map<std::string, MjcfMesh> MeshMap_t;
390 typedef std::unordered_map<std::string, MjcfTexture> TextureMap_t;
391 typedef std::unordered_map<std::string, Eigen::VectorXd> ConfigMap_t;
392 typedef std::map<std::string, MjcfEquality> EqualityMap_t;
397 ClassMap_t mapOfClasses;
399 BodyMap_t mapOfBodies;
401 MaterialMap_t mapOfMaterials;
403 MeshMap_t mapOfMeshes;
405 TextureMap_t mapOfTextures;
407 ConfigMap_t mapOfConfigs;
409 EqualityMap_t mapOfEqualities;
412 Eigen::VectorXd referenceConfig;
418 VectorOfStrings bodiesList;
421 std::string modelName;
422 std::string modelPath;
425 typedef pinocchio::urdf::details::
426 UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl>
428 UrdfVisitor & urdfVisitor;
432 MjcfGraph(UrdfVisitor & urdfVisitor,
const std::string & modelPath)
433 : modelPath(modelPath)
434 , urdfVisitor(urdfVisitor)
451 void parseDefault(ptree & el,
const ptree & parent,
const std::string & parentTag);
459 const boost::optional<std::string> & childClass,
460 const std::string & parentName =
"");
504 template<
typename TypeX,
typename TypeY,
typename TypeZ,
typename TypeUnaligned>
529 void addKeyFrame(
const Eigen::VectorXd & keyframe,
const std::string & keyName);
543 const GeometryType & type,
545 ::hpp::fcl::MeshLoaderPtr & meshLoader);
549 inline std::istringstream getConfiguredStringStream(
const std::string & str)
551 std::istringstream posStream(str);
552 posStream.exceptions(std::ios::badbit);
557 inline Eigen::Matrix<double, N, 1> getVectorFromStream(
const std::string & str)
559 std::istringstream stream = getConfiguredStringStream(str);
560 Eigen::Matrix<double, N, 1> vector;
561 for (
int i = 0; i < N; i++)
567 inline Eigen::VectorXd getUnknownSizeVectorFromStream(
const std::string & str)
569 std::istringstream stream = getConfiguredStringStream(str);
570 std::vector<double> vector;
572 while (stream >> elem)
574 vector.push_back(elem);
577 Eigen::VectorXd returnVector(vector.size());
578 for (std::size_t i = 0; i < vector.size(); i++)
579 returnVector(
static_cast<Eigen::Index
>(i)) = vector[i];
Main pinocchio namespace.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
All Bodies informations extracted from mjcf model.
Structure to stock all default classes information.
Informations that are stocked in the XML tag compile.
Eigen::Matrix3d convertEuler(const Eigen::Vector3d &angles) const
Convert the euler angles according to the convention declared in the compile tag.
double convertAngle(const double &angle_) const
Convert the angle in radian if model was declared to use degree.
void goThroughElement(const ptree &el, const MjcfGraph ¤tGraph)
@bried Go through a geom ptree node, to gather informations
void computeSize()
Compute Geometry size based on sizeS and fromtoS.
void fill(const ptree &el, const MjcfBody ¤tBody, const MjcfGraph ¤tGraph)
Fill Geometry element with info from ptree nodes.
void computeInertia()
Compute geometry inertia.
void findKind()
Find the geometry kind.
The graph which contains all information taken from the mjcf file.
MjcfGraph(UrdfVisitor &urdfVisitor, const std::string &modelPath)
graph constructor
SE3 convertPosition(const ptree &el) const
Convert pose of an mjcf element into SE3.
void parseCompiler(const ptree &el)
Parse all the info from the compile node into compilerInfo.
void parseContactInformation(const Model &model, PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) &contact_models)
Parse the equality constraints and add them to the model.
void addSoloJoint(const MjcfJoint &jointInfo, const MjcfBody ¤tBody, SE3 &bodyInJoint)
Add a joint to the model. only needed when a body has a solo joint child.
void parseGraph()
parse the mjcf file into a graph
void parseRootTree()
Fill the pinocchio model with all the infos from the graph.
void addKeyFrame(const Eigen::VectorXd &keyframe, const std::string &keyName)
Add a keyframe to the model (ie reference configuration)
void fillReferenceConfig(const MjcfBody ¤tBody)
Fill reference configuration for a body and all it's associated dof.
void parseMesh(const ptree &el)
Parse all the info from a mesh node.
void parseJointAndBody(const ptree &el, const boost::optional< std::string > &childClass, const std::string &parentName="")
Go through the main body of the mjcf file "worldbody" to get all the info ready to create the model.
void parseEquality(const ptree &el)
Parse all the info from the equality tag.
void parseKeyFrame(const ptree &el)
Parse all the info from the meta tag keyframe.
void parseTexture(const ptree &el)
Parse all the info from a texture node.
void parseDefault(ptree &el, const ptree &parent, const std::string &parentTag)
Go through the default part of the file and get all the class name. Fill the mapOfDefault for later u...
Inertia convertInertiaFromMjcf(const ptree &el) const
Convert Inertia of an mjcf element into Inertia model of pinocchio.
void parseGeomTree(const GeometryType &type, GeometryModel &geomModel, ::hpp::fcl::MeshLoaderPtr &meshLoader)
Fill geometry model with all the info taken from the mjcf model file.
void parseMaterial(const ptree &el)
Parse all the info from a material node.
JointModel createJoint(const Eigen::Vector3d &axis)
Create a joint to add to the joint composite if needed.
void parseGraphFromXML(const std::string &xmlStr)
parse the mjcf file into a graph
void parseAsset(const ptree &el)
Parse all the info from the meta tag asset (mesh, material, texture)
void fillModel(const std::string &nameOfBody)
Use all the infos that were parsed from the xml file to add a body and joint to the model.
All joint information parsed from the mjcf model.
void goThroughElement(const ptree &el, bool use_limits, const MjcfCompiler ¤tCompiler)
Go through a joint node (default class or not) and parse info into the structure.
void fill(const ptree &el, const MjcfBody ¤tBody, const MjcfGraph ¤tGraph)
All informations related to material are stored here.
void goThroughElement(const ptree &el)
Go through a ptree node to look for material tag related.
All informations related to a mesh are stored here.
All informations related to a texture are stored here.
RangeJoint setDimension() const
Set dimension to the limits to match the joint nq and nv.
RangeJoint concatenate(const RangeJoint &range) const
Concatenate 2 rangeJoint.