| Directory: | ./ |
|---|---|
| File: | include/pinocchio/parsers/mjcf/mjcf-graph.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 39 | 39 | 100.0% |
| Branches: | 54 | 102 | 52.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | |||
| 37 | /// @brief Informations that are stocked in the XML tag compile. | ||
| 38 | /// | ||
| 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 | |||
| 65 | /// @brief Convert the angle in radian if model was declared to use degree | ||
| 66 | /// @param angle_ angle to convert | ||
| 67 | /// @return converted angle | ||
| 68 | double convertAngle(const double & angle_) const; | ||
| 69 | |||
| 70 | /// @brief Convert the euler angles according to the convention declared in the compile tag. | ||
| 71 | /// @param angles Euler angles | ||
| 72 | /// @return Quaternion representation of the euler angles | ||
| 73 | Eigen::Matrix3d convertEuler(const Eigen::Vector3d & angles) const; | ||
| 74 | }; | ||
| 75 | |||
| 76 | /// @brief Structure to stock all default classes information | ||
| 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 | |||
| 88 | /// @brief All Bodies informations extracted from mjcf model | ||
| 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 | |||
| 115 | /// @brief All joint limits | ||
| 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 |
8/16✓ Branch 2 taken 349 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 349 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 349 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 349 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 349 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 349 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 349 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 349 times.
✗ Branch 24 not taken.
|
349 | RangeJoint() = default; |
| 143 | 198 | explicit RangeJoint(double v) | |
| 144 |
8/16✓ Branch 2 taken 198 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 198 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 198 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 198 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 198 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 198 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 198 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 198 times.
✗ Branch 24 not taken.
|
198 | { |
| 145 | 198 | const double infty = std::numeric_limits<double>::infinity(); | |
| 146 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | maxVel = Eigen::VectorXd::Constant(1, infty); |
| 147 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | maxEffort = Eigen::VectorXd::Constant(1, infty); |
| 148 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | minConfig = Eigen::VectorXd::Constant(1, -infty); |
| 149 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | maxConfig = Eigen::VectorXd::Constant(1, infty); |
| 150 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | springStiffness = Eigen::VectorXd::Constant(1, v); |
| 151 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | springReference = Eigen::VectorXd::Constant(1, v); |
| 152 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | friction = Eigen::VectorXd::Constant(1, 0.); |
| 153 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | damping = Eigen::VectorXd::Constant(1, 0.); |
| 154 |
2/4✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
|
198 | armature = Eigen::VectorXd::Constant(1, 0.); |
| 155 | 198 | } | |
| 156 | |||
| 157 | /// @brief Set dimension to the limits to match the joint nq and nv. | ||
| 158 | /// @tparam Nq joint configuration | ||
| 159 | /// @tparam Nv joint velocity | ||
| 160 | /// @return Range with new dimension | ||
| 161 | template<int Nq, int Nv> | ||
| 162 | RangeJoint setDimension() const; | ||
| 163 | |||
| 164 | /// @brief Concatenate 2 rangeJoint | ||
| 165 | /// @tparam Nq old_range, joint configuration | ||
| 166 | /// @tparam Nv old_range, joint velocity | ||
| 167 | /// @param range to concatenate with | ||
| 168 | /// @return Concatenated range. | ||
| 169 | template<int Nq, int Nv> | ||
| 170 | RangeJoint concatenate(const RangeJoint & range) const; | ||
| 171 | }; | ||
| 172 | |||
| 173 | /// @brief All joint information parsed from the mjcf model | ||
| 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 | |||
| 194 | /// @param el ptree joint node | ||
| 195 | /// @param currentBody body to which the joint belongs to | ||
| 196 | /// @param currentGraph current Mjcf graph (needed to get compiler information) | ||
| 197 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
| 198 | |||
| 199 | /// @brief Go through a joint node (default class or not) and parse info into the structure | ||
| 200 | /// @param el ptree joint node | ||
| 201 | /// @param use_limits whether to parse the limits or not | ||
| 202 | void | ||
| 203 | goThroughElement(const ptree & el, bool use_limits, const MjcfCompiler & currentCompiler); | ||
| 204 | }; | ||
| 205 | /// @brief All informations related to a mesh are stored here | ||
| 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 | |||
| 216 | /// @brief All informations related to a texture are stored here | ||
| 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 | |||
| 227 | /// @brief All informations related to material are stored here | ||
| 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 | |||
| 244 | /// @brief Go through a ptree node to look for material tag related | ||
| 245 | /// @param el ptree material node | ||
| 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 | |||
| 303 | /// @brief Find the geometry kind | ||
| 304 | void findKind(); | ||
| 305 | |||
| 306 | /// @brief Compute Geometry size based on sizeS and fromtoS | ||
| 307 | void computeSize(); | ||
| 308 | |||
| 309 | /// @brief Compute geometry inertia | ||
| 310 | void computeInertia(); | ||
| 311 | |||
| 312 | /// @brief Fill Geometry element with info from ptree nodes | ||
| 313 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
| 314 | |||
| 315 | /// @bried Go through a geom ptree node, to gather informations | ||
| 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 | |||
| 380 | /// @brief The graph which contains all information taken from the mjcf file | ||
| 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 | |||
| 430 | /// @brief graph constructor | ||
| 431 | /// @param urdfVisitor | ||
| 432 | 42 | MjcfGraph(UrdfVisitor & urdfVisitor, const std::string & modelPath) | |
| 433 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | : modelPath(modelPath) |
| 434 |
2/4✓ Branch 9 taken 42 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 42 times.
✗ Branch 13 not taken.
|
42 | , urdfVisitor(urdfVisitor) |
| 435 | { | ||
| 436 | 42 | } | |
| 437 | |||
| 438 | /// @brief Convert pose of an mjcf element into SE3 | ||
| 439 | /// @param el ptree element with all the pose element | ||
| 440 | /// @return pose in SE3 | ||
| 441 | SE3 convertPosition(const ptree & el) const; | ||
| 442 | |||
| 443 | /// @brief Convert Inertia of an mjcf element into Inertia model of pinocchio | ||
| 444 | /// @param el ptree element with all the inertial information | ||
| 445 | /// @return Inertia element in pinocchio | ||
| 446 | Inertia convertInertiaFromMjcf(const ptree & el) const; | ||
| 447 | |||
| 448 | /// @brief Go through the default part of the file and get all the class name. Fill the | ||
| 449 | /// mapOfDefault for later use. | ||
| 450 | /// @param el ptree element. Root of the default | ||
| 451 | void parseDefault(ptree & el, const ptree & parent, const std::string & parentTag); | ||
| 452 | |||
| 453 | /// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready | ||
| 454 | /// to create the model. | ||
| 455 | /// @param el root of the tree | ||
| 456 | /// @param parentName name of the parentBody in the robot tree | ||
| 457 | void parseJointAndBody( | ||
| 458 | const ptree & el, | ||
| 459 | const boost::optional<std::string> & childClass, | ||
| 460 | const std::string & parentName = ""); | ||
| 461 | |||
| 462 | /// @brief Parse all the info from the compile node into compilerInfo | ||
| 463 | /// @param el ptree compile node | ||
| 464 | void parseCompiler(const ptree & el); | ||
| 465 | |||
| 466 | /// @brief Parse all the info from a texture node | ||
| 467 | /// @param el ptree texture node | ||
| 468 | void parseTexture(const ptree & el); | ||
| 469 | |||
| 470 | /// @brief Parse all the info from a material node | ||
| 471 | /// @param el ptree material node | ||
| 472 | void parseMaterial(const ptree & el); | ||
| 473 | |||
| 474 | /// @brief Parse all the info from a mesh node | ||
| 475 | /// @param el ptree mesh node | ||
| 476 | void parseMesh(const ptree & el); | ||
| 477 | |||
| 478 | /// @brief Parse all the info from the meta tag asset (mesh, material, texture) | ||
| 479 | /// @param el ptree texture node | ||
| 480 | void parseAsset(const ptree & el); | ||
| 481 | |||
| 482 | /// @brief Parse all the info from the meta tag keyframe | ||
| 483 | /// @param el ptree keyframe node | ||
| 484 | void parseKeyFrame(const ptree & el); | ||
| 485 | |||
| 486 | /// @brief Parse all the info from the equality tag | ||
| 487 | /// @param el ptree equality node | ||
| 488 | void parseEquality(const ptree & el); | ||
| 489 | |||
| 490 | /// @brief parse the mjcf file into a graph | ||
| 491 | void parseGraph(); | ||
| 492 | |||
| 493 | /// @brief parse the mjcf file into a graph | ||
| 494 | /// @param xmlStr xml file name | ||
| 495 | void parseGraphFromXML(const std::string & xmlStr); | ||
| 496 | |||
| 497 | /// @brief Create a joint to add to the joint composite if needed | ||
| 498 | /// @tparam TypeX joint with axis X | ||
| 499 | /// @tparam TypeY joint with axis Y | ||
| 500 | /// @tparam TypeZ joint with axis Z | ||
| 501 | /// @tparam TypeUnaligned joint with axis unaligned | ||
| 502 | /// @param axis axis of the joint | ||
| 503 | /// @return one of the joint with the right axis | ||
| 504 | template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned> | ||
| 505 | JointModel createJoint(const Eigen::Vector3d & axis); | ||
| 506 | |||
| 507 | /// @brief Add a joint to the model. only needed when a body has a solo joint child | ||
| 508 | /// @param jointInfo The joint to add to the tree | ||
| 509 | /// @param currentBody The body associated with the joint | ||
| 510 | /// @param bodyInJoint Position of the body wrt to its joint | ||
| 511 | void | ||
| 512 | addSoloJoint(const MjcfJoint & jointInfo, const MjcfBody & currentBody, SE3 & bodyInJoint); | ||
| 513 | |||
| 514 | /// @brief Use all the infos that were parsed from the xml file to add a body and joint to | ||
| 515 | /// the model | ||
| 516 | /// @param nameOfBody Name of the body to add | ||
| 517 | void fillModel(const std::string & nameOfBody); | ||
| 518 | |||
| 519 | /// @brief Fill the pinocchio model with all the infos from the graph | ||
| 520 | void parseRootTree(); | ||
| 521 | |||
| 522 | /// @brief Fill reference configuration for a body and all it's associated dof | ||
| 523 | /// @param currentBody body to check | ||
| 524 | void fillReferenceConfig(const MjcfBody & currentBody); | ||
| 525 | |||
| 526 | /// @brief Add a keyframe to the model (ie reference configuration) | ||
| 527 | /// @param keyframe Keyframe to add | ||
| 528 | /// @param keyName Name of the keyframe | ||
| 529 | void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName); | ||
| 530 | |||
| 531 | /// @brief Parse the equality constraints and add them to the model | ||
| 532 | /// @param model Model to add the constraints to | ||
| 533 | /// @param contact_models Vector of contact models to add the constraints to | ||
| 534 | void parseContactInformation( | ||
| 535 | const Model & model, | ||
| 536 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models); | ||
| 537 | |||
| 538 | /// @brief Fill geometry model with all the info taken from the mjcf model file | ||
| 539 | /// @param type Type of geometry to parse (COLLISION or VISUAL) | ||
| 540 | /// @param geomModel geometry model to fill | ||
| 541 | /// @param meshLoader mesh loader from hpp::fcl | ||
| 542 | void parseGeomTree( | ||
| 543 | const GeometryType & type, | ||
| 544 | GeometryModel & geomModel, | ||
| 545 | ::hpp::fcl::MeshLoaderPtr & meshLoader); | ||
| 546 | }; | ||
| 547 | namespace internal | ||
| 548 | { | ||
| 549 | 1518 | inline std::istringstream getConfiguredStringStream(const std::string & str) | |
| 550 | { | ||
| 551 | 1518 | std::istringstream posStream(str); | |
| 552 |
1/2✓ Branch 1 taken 1518 times.
✗ Branch 2 not taken.
|
1518 | posStream.exceptions(std::ios::badbit); |
| 553 | 1518 | return posStream; | |
| 554 | } | ||
| 555 | |||
| 556 | template<int N> | ||
| 557 | 2908 | inline Eigen::Matrix<double, N, 1> getVectorFromStream(const std::string & str) | |
| 558 | { | ||
| 559 |
1/2✓ Branch 1 taken 1454 times.
✗ Branch 2 not taken.
|
2908 | std::istringstream stream = getConfiguredStringStream(str); |
| 560 |
1/2✓ Branch 1 taken 1454 times.
✗ Branch 2 not taken.
|
2908 | Eigen::Matrix<double, N, 1> vector; |
| 561 |
2/2✓ Branch 0 taken 4176 times.
✓ Branch 1 taken 1454 times.
|
11260 | for (int i = 0; i < N; i++) |
| 562 |
2/4✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4176 times.
✗ Branch 5 not taken.
|
8352 | stream >> vector(i); |
| 563 | |||
| 564 | 5816 | return vector; | |
| 565 | 2908 | } | |
| 566 | |||
| 567 | 8 | inline Eigen::VectorXd getUnknownSizeVectorFromStream(const std::string & str) | |
| 568 | { | ||
| 569 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | std::istringstream stream = getConfiguredStringStream(str); |
| 570 | 8 | std::vector<double> vector; | |
| 571 | double elem; | ||
| 572 |
4/6✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 61 times.
✓ Branch 7 taken 8 times.
|
69 | while (stream >> elem) |
| 573 | { | ||
| 574 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
61 | vector.push_back(elem); |
| 575 | } | ||
| 576 | |||
| 577 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | Eigen::VectorXd returnVector(vector.size()); |
| 578 |
2/2✓ Branch 1 taken 61 times.
✓ Branch 2 taken 8 times.
|
69 | for (std::size_t i = 0; i < vector.size(); i++) |
| 579 |
1/2✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
|
61 | returnVector(static_cast<Eigen::Index>(i)) = vector[i]; |
| 580 | |||
| 581 | 16 | return returnVector; | |
| 582 | 8 | } | |
| 583 | } // namespace internal | ||
| 584 | } // namespace details | ||
| 585 | } // namespace mjcf | ||
| 586 | } // namespace pinocchio | ||
| 587 | |||
| 588 | #endif // __pinocchio_parsers_mjcf_graph_hpp__ | ||
| 589 |