pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
mjcf-graph.hpp
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_parsers_mjcf_graph_hpp__
6 #define __pinocchio_parsers_mjcf_graph_hpp__
7 
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>
19 
20 #include <sstream>
21 #include <limits>
22 #include <iostream>
23 #include <unordered_map>
24 #include <map>
25 
26 namespace pinocchio
27 {
28  namespace mjcf
29  {
30  namespace details
31  {
32  struct MjcfGraph;
33  struct MjcfJoint;
34  struct MjcfGeom;
35  struct MjcfSite;
36 
39  struct PINOCCHIO_PARSERS_DLLAPI MjcfCompiler
40  {
41  public:
42  // Global attribute to use limit that are in the model or not
43  bool autolimits = true;
44 
45  // Attribute to keep or not the full path of files specified in the model
46  bool strippath = false;
47  // Directory where all the meshes are (can be relative or absolute)
48  std::string meshdir;
49  // Directory where all the textures are (can be relative or absolute)
50  std::string texturedir;
51 
52  // Value for angle conversion (Mujoco default - degrees)
53  double angle_converter = boost::math::constants::pi<double>() / 180.0;
54  // Euler Axis to use to convert angles representation to quaternion
55  Eigen::Matrix3d mapEulerAngles = Eigen::Matrix3d::Identity();
56 
57  // Value to crop the mass (if mass < boundMass, mass = boundMass)
58  double boundMass = 0;
59  // Value to crop the diagonal of the inertia matrix (if mass < boundMass, mass = boundMass)
60  double boundInertia = 0;
61 
62  // True, false or auto - auto = indeterminate
63  boost::logic::tribool inertiafromgeom = boost::logic::indeterminate;
64 
68  double convertAngle(const double & angle_) const;
69 
73  Eigen::Matrix3d convertEuler(const Eigen::Vector3d & angles) const;
74  };
75 
77  struct MjcfClass
78  {
79  public:
80  typedef boost::property_tree::ptree ptree;
81 
82  // name of the default class
83  std::string className;
84  // Ptree associated with the class name
85  ptree classElement;
86  };
87 
89  struct MjcfBody
90  {
91  public:
92  // Name of the body
93  std::string bodyName;
94  // Name of the parent
95  std::string bodyParent;
96  // Name of the default class used by this body (optional)
97  std::string bodyClassName;
98  // Special default class, that is common to all bodies and children if not specified
99  // otherwise
100  std::string childClass;
101 
102  // Position of the body wrt to the previous body
103  SE3 bodyPlacement = SE3::Identity();
104  // Body inertia
105  Inertia bodyInertia = Inertia::Identity();
106 
107  // Vector of joints associated with the body
108  std::vector<MjcfJoint> jointChildren;
109  // Vector of geometries associated with the body
110  std::vector<MjcfGeom> geomChildren;
111  // Vector of sites
112  std::vector<MjcfSite> siteChildren;
113  };
114 
116  struct PINOCCHIO_PARSERS_DLLAPI RangeJoint
117  {
118  // Max effort
119  Eigen::VectorXd maxEffort;
120  // Max velocity
121  Eigen::VectorXd maxVel;
122  // Max position
123  Eigen::VectorXd maxConfig;
124  // Min position
125  Eigen::VectorXd minConfig;
126 
127  // Join Stiffness
128  Eigen::VectorXd springStiffness;
129  // joint position or angle in which the joint spring (if any) achieves equilibrium
130  Eigen::VectorXd springReference;
131 
132  // friction applied in this joint
133  Eigen::VectorXd friction;
134  // Damping applied by this joint.
135  Eigen::VectorXd damping;
136 
137  // Armature inertia created by this joint
138  Eigen::VectorXd armature;
139  // Dry friction.
140  double frictionLoss = 0.;
141 
142  RangeJoint() = default;
143  explicit RangeJoint(double v)
144  {
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.);
155  }
156 
161  template<int Nq, int Nv>
163 
169  template<int Nq, int Nv>
170  RangeJoint concatenate(const RangeJoint & range) const;
171  };
172 
174  struct PINOCCHIO_PARSERS_DLLAPI MjcfJoint
175  {
176  public:
177  typedef boost::property_tree::ptree ptree;
178 
179  // Name of the joint
180  std::string jointName = "free";
181  // Placement of the joint wrt to its body - default Identity
182  SE3 jointPlacement = SE3::Identity();
183 
184  // axis of the joint - default "0 0 1"
185  Eigen::Vector3d axis = Eigen::Vector3d::UnitZ();
186  // Limits that applie to this joint
187  RangeJoint range{1};
188 
189  // type of the joint (hinge, ball, slide, free) - default "hinge"
190  std::string jointType = "hinge";
191 
192  double posRef = 0.; // only possible for hinge and slides
193 
197  void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph);
198 
202  void
203  goThroughElement(const ptree & el, bool use_limits, const MjcfCompiler & currentCompiler);
204  };
206  struct MjcfMesh
207  {
208  // Scale of the mesh
209  Eigen::Vector3d scale = Eigen::Vector3d::Constant(1);
210  // Path to the mesh file
211  std::string filePath;
212  // Vertices of the mesh
213  Eigen::MatrixX3d vertices;
214  };
215 
217  struct MjcfTexture
218  {
219  // [2d, cube, skybox], “cube”
220  std::string textType = "cube";
221  // Path to the texture file
222  std::string filePath;
223  // Size of the grid if needed
224  Eigen::Vector2d gridsize = Eigen::Vector2d::Constant(1);
225  };
226 
228  struct PINOCCHIO_PARSERS_DLLAPI MjcfMaterial
229  {
230  typedef boost::property_tree::ptree ptree;
231  // Color of the material
232  Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
233 
234  float reflectance = 0;
235 
236  float shininess = 0.5;
237 
238  float specular = 0.5;
239 
240  float emission = 0;
241  // name of the texture to apply on the material
242  std::string texture;
243 
246  void goThroughElement(const ptree & el);
247  };
248 
249  struct PINOCCHIO_PARSERS_DLLAPI MjcfGeom
250  {
251  public:
252  typedef boost::property_tree::ptree ptree;
253 
254  // Kind of possible geometry
255  enum TYPE
256  {
257  VISUAL,
258  COLLISION,
259  BOTH
260  };
261  // name of the geometry object
262  std::string geomName;
263 
264  // [plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh, sdf], “sphere”
265  std::string geomType = "sphere";
266 
267  // Kind of the geometry object
268  TYPE geomKind = BOTH;
269 
270  // Contact filtering and dynamic pair (used here to determine geometry kind)
271  int contype = 1;
272  int conaffinity = 1;
273  // Geometry group (used to determine geometry kind)
274  int group = 0;
275 
276  // String that hold size parameter
277  std::string sizeS;
278  // Optional in case fromto tag is used
279  boost::optional<std::string> fromtoS;
280  // Size parameter
281  Eigen::VectorXd size;
282 
283  // Color of the geometry
284  Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1);
285 
286  // Name of the material applied on the geometry
287  std::string materialName;
288  // Name of the mesh (optional)
289  std::string meshName;
290 
291  // Density for computing the mass
292  double density = 1000;
293  // If mass is only on the outer layer of the geometry
294  bool shellinertia = false;
295 
296  // Geometry Placement in parent body. Center of the frame of geometry is the center of mass.
297  SE3 geomPlacement = SE3::Identity();
298  // Inertia of the geometry obj
299  Inertia geomInertia = Inertia::Identity();
300  // optional mass (if not defined, will use density)
301  boost::optional<double> massGeom;
302 
304  void findKind();
305 
307  void computeSize();
308 
311 
313  void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph);
314 
316  void goThroughElement(const ptree & el, const MjcfGraph & currentGraph);
317  };
318 
319  struct PINOCCHIO_PARSERS_DLLAPI MjcfSite
320  {
321  typedef boost::property_tree::ptree ptree;
322 
323  SE3 sitePlacement = SE3::Identity();
324 
325  std::string siteName;
326 
327  void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph);
328  void goThroughElement(const ptree & el, const MjcfGraph & currentGraph);
329  };
330 
331  /*
332  typedef struct mjsEquality_ { // equality specification
333  mjsElement* element; // element type
334  mjString* name; // name
335  mjtEq type; // constraint type
336  double data[mjNEQDATA]; // type-dependent data
337  mjtByte active; // is equality initially active
338  mjString* name1; // name of object 1
339  mjString* name2; // name of object 2
340  mjtNum solref[mjNREF]; // solver reference
341  mjtNum solimp[mjNIMP]; // solver impedance
342  mjString* info; // message appended to errors
343  } mjsEquality;
344  */
345  struct PINOCCHIO_PARSERS_DLLAPI MjcfEquality
346  {
347  typedef boost::property_tree::ptree ptree;
348 
349  // Optional name of the equality constraint
350  std::string name;
351 
352  // Type of the constraint: (connect for now)
353  std::string type;
354 
355  // // Optional class for setting unspecified attributes
356  // std::string class;
357 
358  // active: 'true' or 'false' (default: 'true')
359  // solref and solimp
360 
361  // Name of the first body participating in the constraint
362  std::string body1;
363  // Name of the second body participating in the constraint (optional, default: world)
364  std::string body2;
365 
366  // Coordinates of the 3D anchor point where the two bodies are connected.
367  // Specified relative to the local coordinate frame of the first body.
368  Eigen::Vector3d anchor = Eigen::Vector3d::Zero();
369 
370  // TODO: implement when weld is introduced
371  // This attribute specifies the relative pose (3D position followed by 4D quaternion
372  // orientation) of body2 relative to body1. If the quaternion part (i.e., last 4 components
373  // of the vector) are all zeros, as in the default setting, this attribute is ignored and
374  // the relative pose is the one corresponding to the model reference pose in qpos0. The
375  // unusual default is because all equality constraint types share the same default for their
376  // numeric parameters.
377  // Eigen::VectorXd relativePose = Eigen::VectorXd::Zero(7);
378  };
379 
381  struct PINOCCHIO_PARSERS_DLLAPI MjcfGraph
382  {
383  public:
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;
393 
394  // Compiler Info needed to properly parse the rest of file
395  MjcfCompiler compilerInfo;
396  // Map of default classes
397  ClassMap_t mapOfClasses;
398  // Map of bodies
399  BodyMap_t mapOfBodies;
400  // Map of Materials
401  MaterialMap_t mapOfMaterials;
402  // Map of Meshes
403  MeshMap_t mapOfMeshes;
404  // Map of textures
405  TextureMap_t mapOfTextures;
406  // Map of model configurations
407  ConfigMap_t mapOfConfigs;
408  // Map of equality constraints
409  EqualityMap_t mapOfEqualities;
410 
411  // reference configuration
412  Eigen::VectorXd referenceConfig;
413 
414  // property tree where xml file is stored
415  ptree pt;
416 
417  // Ordered list of bodies
418  VectorOfStrings bodiesList;
419 
420  // Name of the model
421  std::string modelName;
422  std::string modelPath;
423 
424  // Urdf Visitor to add joint and body
425  typedef pinocchio::urdf::details::
426  UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl>
427  UrdfVisitor;
428  UrdfVisitor & urdfVisitor;
429 
432  MjcfGraph(UrdfVisitor & urdfVisitor, const std::string & modelPath)
433  : modelPath(modelPath)
434  , urdfVisitor(urdfVisitor)
435  {
436  }
437 
441  SE3 convertPosition(const ptree & el) const;
442 
446  Inertia convertInertiaFromMjcf(const ptree & el) const;
447 
451  void parseDefault(ptree & el, const ptree & parent, const std::string & parentTag);
452 
458  const ptree & el,
459  const boost::optional<std::string> & childClass,
460  const std::string & parentName = "");
461 
464  void parseCompiler(const ptree & el);
465 
468  void parseTexture(const ptree & el);
469 
472  void parseMaterial(const ptree & el);
473 
476  void parseMesh(const ptree & el);
477 
480  void parseAsset(const ptree & el);
481 
484  void parseKeyFrame(const ptree & el);
485 
488  void parseEquality(const ptree & el);
489 
491  void parseGraph();
492 
495  void parseGraphFromXML(const std::string & xmlStr);
496 
504  template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned>
505  JointModel createJoint(const Eigen::Vector3d & axis);
506 
511  void
512  addSoloJoint(const MjcfJoint & jointInfo, const MjcfBody & currentBody, SE3 & bodyInJoint);
513 
517  void fillModel(const std::string & nameOfBody);
518 
521 
524  void fillReferenceConfig(const MjcfBody & currentBody);
525 
529  void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName);
530 
535  const Model & model,
536  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models);
537 
543  const GeometryType & type,
544  GeometryModel & geomModel,
545  ::hpp::fcl::MeshLoaderPtr & meshLoader);
546  };
547  namespace internal
548  {
549  inline std::istringstream getConfiguredStringStream(const std::string & str)
550  {
551  std::istringstream posStream(str);
552  posStream.exceptions(std::ios::badbit);
553  return posStream;
554  }
555 
556  template<int N>
557  inline Eigen::Matrix<double, N, 1> getVectorFromStream(const std::string & str)
558  {
559  std::istringstream stream = getConfiguredStringStream(str);
560  Eigen::Matrix<double, N, 1> vector;
561  for (int i = 0; i < N; i++)
562  stream >> vector(i);
563 
564  return vector;
565  }
566 
567  inline Eigen::VectorXd getUnknownSizeVectorFromStream(const std::string & str)
568  {
569  std::istringstream stream = getConfiguredStringStream(str);
570  std::vector<double> vector;
571  double elem;
572  while (stream >> elem)
573  {
574  vector.push_back(elem);
575  }
576 
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];
580 
581  return returnVector;
582  }
583  } // namespace internal
584  } // namespace details
585  } // namespace mjcf
586 } // namespace pinocchio
587 
588 #endif // __pinocchio_parsers_mjcf_graph_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
All Bodies informations extracted from mjcf model.
Definition: mjcf-graph.hpp:90
Structure to stock all default classes information.
Definition: mjcf-graph.hpp:78
Informations that are stocked in the XML tag compile.
Definition: mjcf-graph.hpp:40
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 &currentGraph)
@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 &currentBody, const MjcfGraph &currentGraph)
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.
Definition: mjcf-graph.hpp:382
MjcfGraph(UrdfVisitor &urdfVisitor, const std::string &modelPath)
graph constructor
Definition: mjcf-graph.hpp:432
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 &currentBody, 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 &currentBody)
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.
Definition: mjcf-graph.hpp:175
void goThroughElement(const ptree &el, bool use_limits, const MjcfCompiler &currentCompiler)
Go through a joint node (default class or not) and parse info into the structure.
void fill(const ptree &el, const MjcfBody &currentBody, const MjcfGraph &currentGraph)
All informations related to material are stored here.
Definition: mjcf-graph.hpp:229
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.
Definition: mjcf-graph.hpp:207
All informations related to a texture are stored here.
Definition: mjcf-graph.hpp:218
RangeJoint setDimension() const
Set dimension to the limits to match the joint nq and nv.
RangeJoint concatenate(const RangeJoint &range) const
Concatenate 2 rangeJoint.