| Directory: | ./ |
|---|---|
| File: | include/pinocchio/parsers/mjcf/mjcf-graph.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 39 | 39 | 100.0% |
| Branches: | 50 | 94 | 53.2% |
| 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 | |||
| 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 | |||
| 25 | namespace pinocchio | ||
| 26 | { | ||
| 27 | namespace mjcf | ||
| 28 | { | ||
| 29 | namespace details | ||
| 30 | { | ||
| 31 | struct MjcfGraph; | ||
| 32 | struct MjcfJoint; | ||
| 33 | struct MjcfGeom; | ||
| 34 | struct MjcfSite; | ||
| 35 | |||
| 36 | /// @brief Informations that are stocked in the XML tag compile. | ||
| 37 | /// | ||
| 38 | struct PINOCCHIO_PARSERS_DLLAPI MjcfCompiler | ||
| 39 | { | ||
| 40 | public: | ||
| 41 | // Global attribute to use limit that are in the model or not | ||
| 42 | bool autolimits = true; | ||
| 43 | |||
| 44 | // Attribute to keep or not the full path of files specified in the model | ||
| 45 | bool strippath = false; | ||
| 46 | // Directory where all the meshes are (can be relative or absolute) | ||
| 47 | std::string meshdir; | ||
| 48 | // Directory where all the textures are (can be relative or absolute) | ||
| 49 | std::string texturedir; | ||
| 50 | |||
| 51 | // Value for angle conversion (Mujoco default - degrees) | ||
| 52 | double angle_converter = boost::math::constants::pi<double>() / 180.0; | ||
| 53 | // Euler Axis to use to convert angles representation to quaternion | ||
| 54 | Eigen::Matrix3d mapEulerAngles; | ||
| 55 | |||
| 56 | // Value to crop the mass (if mass < boundMass, mass = boundMass) | ||
| 57 | double boundMass = 0; | ||
| 58 | // Value to crop the diagonal of the inertia matrix (if mass < boundMass, mass = boundMass) | ||
| 59 | double boundInertia = 0; | ||
| 60 | |||
| 61 | // True, false or auto - auto = indeterminate | ||
| 62 | boost::logic::tribool inertiafromgeom = boost::logic::indeterminate; | ||
| 63 | |||
| 64 | /// @brief Convert the angle in radian if model was declared to use degree | ||
| 65 | /// @param angle_ angle to convert | ||
| 66 | /// @return converted angle | ||
| 67 | double convertAngle(const double & angle_) const; | ||
| 68 | |||
| 69 | /// @brief Convert the euler angles according to the convention declared in the compile tag. | ||
| 70 | /// @param angles Euler angles | ||
| 71 | /// @return Quaternion representation of the euler angles | ||
| 72 | Eigen::Matrix3d convertEuler(const Eigen::Vector3d & angles) const; | ||
| 73 | }; | ||
| 74 | |||
| 75 | /// @brief Structure to stock all default classes information | ||
| 76 | struct MjcfClass | ||
| 77 | { | ||
| 78 | public: | ||
| 79 | typedef boost::property_tree::ptree ptree; | ||
| 80 | |||
| 81 | // name of the default class | ||
| 82 | std::string className; | ||
| 83 | // Ptree associated with the class name | ||
| 84 | ptree classElement; | ||
| 85 | }; | ||
| 86 | |||
| 87 | /// @brief All Bodies informations extracted from mjcf model | ||
| 88 | struct MjcfBody | ||
| 89 | { | ||
| 90 | public: | ||
| 91 | // Name of the body | ||
| 92 | std::string bodyName; | ||
| 93 | // Name of the parent | ||
| 94 | std::string bodyParent; | ||
| 95 | // Name of the default class used by this body (optional) | ||
| 96 | std::string bodyClassName; | ||
| 97 | // Special default class, that is common to all bodies and children if not specified | ||
| 98 | // otherwise | ||
| 99 | std::string childClass; | ||
| 100 | |||
| 101 | // Position of the body wrt to the previous body | ||
| 102 | SE3 bodyPlacement = SE3::Identity(); | ||
| 103 | // Body inertia | ||
| 104 | Inertia bodyInertia = Inertia::Identity(); | ||
| 105 | |||
| 106 | // Vector of joints associated with the body | ||
| 107 | std::vector<MjcfJoint> jointChildren; | ||
| 108 | // Vector of geometries associated with the body | ||
| 109 | std::vector<MjcfGeom> geomChildren; | ||
| 110 | // Vector of sites | ||
| 111 | std::vector<MjcfSite> siteChildren; | ||
| 112 | }; | ||
| 113 | |||
| 114 | /// @brief All joint limits | ||
| 115 | struct PINOCCHIO_PARSERS_DLLAPI RangeJoint | ||
| 116 | { | ||
| 117 | // Max effort | ||
| 118 | Eigen::VectorXd maxEffort; | ||
| 119 | // Max velocity | ||
| 120 | Eigen::VectorXd maxVel; | ||
| 121 | // Max position | ||
| 122 | Eigen::VectorXd maxConfig; | ||
| 123 | // Min position | ||
| 124 | Eigen::VectorXd minConfig; | ||
| 125 | |||
| 126 | // Join Stiffness | ||
| 127 | Eigen::VectorXd springStiffness; | ||
| 128 | // joint position or angle in which the joint spring (if any) achieves equilibrium | ||
| 129 | Eigen::VectorXd springReference; | ||
| 130 | |||
| 131 | // friction applied in this joint | ||
| 132 | Eigen::VectorXd friction; | ||
| 133 | // Damping applied by this joint. | ||
| 134 | Eigen::VectorXd damping; | ||
| 135 | |||
| 136 | // Armature inertia created by this joint | ||
| 137 | double armature = 0.; | ||
| 138 | // Dry friction. | ||
| 139 | double frictionLoss = 0.; | ||
| 140 | |||
| 141 |
7/14✓ Branch 2 taken 156 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 156 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 156 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 156 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 156 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 156 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 156 times.
✗ Branch 21 not taken.
|
156 | RangeJoint() = default; |
| 142 | 85 | explicit RangeJoint(double v) | |
| 143 |
7/14✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 85 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 85 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 85 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 85 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 85 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 85 times.
✗ Branch 21 not taken.
|
85 | { |
| 144 | 85 | const double infty = std::numeric_limits<double>::infinity(); | |
| 145 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxVel = Eigen::VectorXd::Constant(1, infty); |
| 146 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxEffort = Eigen::VectorXd::Constant(1, infty); |
| 147 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | minConfig = Eigen::VectorXd::Constant(1, -infty); |
| 148 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxConfig = Eigen::VectorXd::Constant(1, infty); |
| 149 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | springStiffness = Eigen::VectorXd::Constant(1, v); |
| 150 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | springReference = Eigen::VectorXd::Constant(1, v); |
| 151 | ; | ||
| 152 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | friction = Eigen::VectorXd::Constant(1, 0.); |
| 153 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | damping = Eigen::VectorXd::Constant(1, 0.); |
| 154 | 85 | } | |
| 155 | |||
| 156 | /// @brief Set dimension to the limits to match the joint nq and nv. | ||
| 157 | /// @tparam Nq joint configuration | ||
| 158 | /// @tparam Nv joint velocity | ||
| 159 | /// @return Range with new dimension | ||
| 160 | template<int Nq, int Nv> | ||
| 161 | RangeJoint setDimension() const; | ||
| 162 | |||
| 163 | /// @brief Concatenate 2 rangeJoint | ||
| 164 | /// @tparam Nq old_range, joint configuration | ||
| 165 | /// @tparam Nv old_range, joint velocity | ||
| 166 | /// @param range to concatenate with | ||
| 167 | /// @return Concatenated range. | ||
| 168 | template<int Nq, int Nv> | ||
| 169 | RangeJoint concatenate(const RangeJoint & range) const; | ||
| 170 | }; | ||
| 171 | |||
| 172 | /// @brief All joint information parsed from the mjcf model | ||
| 173 | struct PINOCCHIO_PARSERS_DLLAPI MjcfJoint | ||
| 174 | { | ||
| 175 | public: | ||
| 176 | typedef boost::property_tree::ptree ptree; | ||
| 177 | |||
| 178 | // Name of the joint | ||
| 179 | std::string jointName = "free"; | ||
| 180 | // Placement of the joint wrt to its body - default Identity | ||
| 181 | SE3 jointPlacement = SE3::Identity(); | ||
| 182 | |||
| 183 | // axis of the joint - default "0 0 1" | ||
| 184 | Eigen::Vector3d axis = Eigen::Vector3d::UnitZ(); | ||
| 185 | // Limits that applie to this joint | ||
| 186 | RangeJoint range{1}; | ||
| 187 | |||
| 188 | // type of the joint (hinge, ball, slide, free) - default "hinge" | ||
| 189 | std::string jointType = "hinge"; | ||
| 190 | |||
| 191 | double posRef = 0.; // only possible for hinge and slides | ||
| 192 | |||
| 193 | /// @param el ptree joint node | ||
| 194 | /// @param currentBody body to which the joint belongs to | ||
| 195 | /// @param currentGraph current Mjcf graph (needed to get compiler information) | ||
| 196 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
| 197 | |||
| 198 | /// @brief Go through a joint node (default class or not) and parse info into the structure | ||
| 199 | /// @param el ptree joint node | ||
| 200 | /// @param use_limits whether to parse the limits or not | ||
| 201 | void | ||
| 202 | goThroughElement(const ptree & el, bool use_limits, const MjcfCompiler & currentCompiler); | ||
| 203 | }; | ||
| 204 | /// @brief All informations related to a mesh are stored here | ||
| 205 | struct MjcfMesh | ||
| 206 | { | ||
| 207 | // Scale of the mesh | ||
| 208 | Eigen::Vector3d scale = Eigen::Vector3d::Constant(1); | ||
| 209 | // Path to the mesh file | ||
| 210 | std::string filePath; | ||
| 211 | }; | ||
| 212 | |||
| 213 | /// @brief All informations related to a texture are stored here | ||
| 214 | struct MjcfTexture | ||
| 215 | { | ||
| 216 | // [2d, cube, skybox], “cube” | ||
| 217 | std::string textType = "cube"; | ||
| 218 | // Path to the texture file | ||
| 219 | std::string filePath; | ||
| 220 | // Size of the grid if needed | ||
| 221 | Eigen::Vector2d gridsize = Eigen::Vector2d::Constant(1); | ||
| 222 | }; | ||
| 223 | |||
| 224 | /// @brief All informations related to material are stored here | ||
| 225 | struct PINOCCHIO_PARSERS_DLLAPI MjcfMaterial | ||
| 226 | { | ||
| 227 | typedef boost::property_tree::ptree ptree; | ||
| 228 | // Color of the material | ||
| 229 | Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1); | ||
| 230 | |||
| 231 | float reflectance = 0; | ||
| 232 | |||
| 233 | float shininess = 0.5; | ||
| 234 | |||
| 235 | float specular = 0.5; | ||
| 236 | |||
| 237 | float emission = 0; | ||
| 238 | // name of the texture to apply on the material | ||
| 239 | std::string texture; | ||
| 240 | |||
| 241 | /// @brief Go through a ptree node to look for material tag related | ||
| 242 | /// @param el ptree material node | ||
| 243 | void goThroughElement(const ptree & el); | ||
| 244 | }; | ||
| 245 | |||
| 246 | struct PINOCCHIO_PARSERS_DLLAPI MjcfGeom | ||
| 247 | { | ||
| 248 | public: | ||
| 249 | typedef boost::property_tree::ptree ptree; | ||
| 250 | |||
| 251 | // Kind of possible geometry | ||
| 252 | enum TYPE | ||
| 253 | { | ||
| 254 | VISUAL, | ||
| 255 | COLLISION, | ||
| 256 | BOTH | ||
| 257 | }; | ||
| 258 | // name of the geometry object | ||
| 259 | std::string geomName; | ||
| 260 | |||
| 261 | // [plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh, sdf], “sphere” | ||
| 262 | std::string geomType = "sphere"; | ||
| 263 | |||
| 264 | // Kind of the geometry object | ||
| 265 | TYPE geomKind = BOTH; | ||
| 266 | |||
| 267 | // Contact filtering and dynamic pair (used here to determine geometry kind) | ||
| 268 | int contype = 1; | ||
| 269 | int conaffinity = 1; | ||
| 270 | // Geometry group (used to determine geometry kind) | ||
| 271 | int group = 0; | ||
| 272 | |||
| 273 | // String that hold size parameter | ||
| 274 | std::string sizeS; | ||
| 275 | // Optional in case fromto tag is used | ||
| 276 | boost::optional<std::string> fromtoS; | ||
| 277 | // Size parameter | ||
| 278 | Eigen::VectorXd size; | ||
| 279 | |||
| 280 | // Color of the geometry | ||
| 281 | Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1); | ||
| 282 | |||
| 283 | // Name of the material applied on the geometry | ||
| 284 | std::string materialName; | ||
| 285 | // Name of the mesh (optional) | ||
| 286 | std::string meshName; | ||
| 287 | |||
| 288 | // Density for computing the mass | ||
| 289 | double density = 1000; | ||
| 290 | // If mass is only on the outer layer of the geometry | ||
| 291 | bool shellinertia = false; | ||
| 292 | |||
| 293 | // Geometry Placement in parent body. Center of the frame of geometry is the center of mass. | ||
| 294 | SE3 geomPlacement = SE3::Identity(); | ||
| 295 | // Inertia of the geometry obj | ||
| 296 | Inertia geomInertia = Inertia::Identity(); | ||
| 297 | // optional mass (if not defined, will use density) | ||
| 298 | boost::optional<double> massGeom; | ||
| 299 | |||
| 300 | /// @brief Find the geometry kind | ||
| 301 | void findKind(); | ||
| 302 | |||
| 303 | /// @brief Compute Geometry size based on sizeS and fromtoS | ||
| 304 | void computeSize(); | ||
| 305 | |||
| 306 | /// @brief Compute geometry inertia | ||
| 307 | void computeInertia(); | ||
| 308 | |||
| 309 | /// @brief Fill Geometry element with info from ptree nodes | ||
| 310 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
| 311 | |||
| 312 | /// @bried Go through a geom ptree node, to gather informations | ||
| 313 | void goThroughElement(const ptree & el, const MjcfGraph & currentGraph); | ||
| 314 | }; | ||
| 315 | |||
| 316 | struct PINOCCHIO_PARSERS_DLLAPI MjcfSite | ||
| 317 | { | ||
| 318 | typedef boost::property_tree::ptree ptree; | ||
| 319 | |||
| 320 | SE3 sitePlacement = SE3::Identity(); | ||
| 321 | |||
| 322 | std::string siteName; | ||
| 323 | |||
| 324 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
| 325 | void goThroughElement(const ptree & el, const MjcfGraph & currentGraph); | ||
| 326 | }; | ||
| 327 | |||
| 328 | /// @brief The graph which contains all information taken from the mjcf file | ||
| 329 | struct PINOCCHIO_PARSERS_DLLAPI MjcfGraph | ||
| 330 | { | ||
| 331 | public: | ||
| 332 | typedef boost::property_tree::ptree ptree; | ||
| 333 | typedef std::vector<std::string> VectorOfStrings; | ||
| 334 | typedef std::unordered_map<std::string, MjcfBody> BodyMap_t; | ||
| 335 | typedef std::unordered_map<std::string, MjcfClass> ClassMap_t; | ||
| 336 | typedef std::unordered_map<std::string, MjcfMaterial> MaterialMap_t; | ||
| 337 | typedef std::unordered_map<std::string, MjcfMesh> MeshMap_t; | ||
| 338 | typedef std::unordered_map<std::string, MjcfTexture> TextureMap_t; | ||
| 339 | typedef std::unordered_map<std::string, Eigen::VectorXd> ConfigMap_t; | ||
| 340 | |||
| 341 | // Compiler Info needed to properly parse the rest of file | ||
| 342 | MjcfCompiler compilerInfo; | ||
| 343 | // Map of default classes | ||
| 344 | ClassMap_t mapOfClasses; | ||
| 345 | // Map of bodies | ||
| 346 | BodyMap_t mapOfBodies; | ||
| 347 | // Map of Materials | ||
| 348 | MaterialMap_t mapOfMaterials; | ||
| 349 | // Map of Meshes | ||
| 350 | MeshMap_t mapOfMeshes; | ||
| 351 | // Map of textures | ||
| 352 | TextureMap_t mapOfTextures; | ||
| 353 | // Map of model configurations | ||
| 354 | ConfigMap_t mapOfConfigs; | ||
| 355 | |||
| 356 | // reference configuration | ||
| 357 | Eigen::VectorXd referenceConfig; | ||
| 358 | |||
| 359 | // property tree where xml file is stored | ||
| 360 | ptree pt; | ||
| 361 | |||
| 362 | // Ordered list of bodies | ||
| 363 | VectorOfStrings bodiesList; | ||
| 364 | |||
| 365 | // Name of the model | ||
| 366 | std::string modelName; | ||
| 367 | std::string modelPath; | ||
| 368 | |||
| 369 | // Urdf Visitor to add joint and body | ||
| 370 | typedef pinocchio::urdf::details:: | ||
| 371 | UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl> | ||
| 372 | UrdfVisitor; | ||
| 373 | UrdfVisitor & urdfVisitor; | ||
| 374 | |||
| 375 | /// @brief graph constructor | ||
| 376 | /// @param urdfVisitor | ||
| 377 | 26 | MjcfGraph(UrdfVisitor & urdfVisitor, const std::string & modelPath) | |
| 378 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | : modelPath(modelPath) |
| 379 |
2/4✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
|
26 | , urdfVisitor(urdfVisitor) |
| 380 | { | ||
| 381 | 26 | } | |
| 382 | |||
| 383 | /// @brief Convert pose of an mjcf element into SE3 | ||
| 384 | /// @param el ptree element with all the pose element | ||
| 385 | /// @return pose in SE3 | ||
| 386 | SE3 convertPosition(const ptree & el) const; | ||
| 387 | |||
| 388 | /// @brief Convert Inertia of an mjcf element into Inertia model of pinocchio | ||
| 389 | /// @param el ptree element with all the inertial information | ||
| 390 | /// @return Inertia element in pinocchio | ||
| 391 | Inertia convertInertiaFromMjcf(const ptree & el) const; | ||
| 392 | |||
| 393 | /// @brief Go through the default part of the file and get all the class name. Fill the | ||
| 394 | /// mapOfDefault for later use. | ||
| 395 | /// @param el ptree element. Root of the default | ||
| 396 | void parseDefault(ptree & el, const ptree & parent); | ||
| 397 | |||
| 398 | /// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready | ||
| 399 | /// to create the model. | ||
| 400 | /// @param el root of the tree | ||
| 401 | /// @param parentName name of the parentBody in the robot tree | ||
| 402 | void parseJointAndBody( | ||
| 403 | const ptree & el, | ||
| 404 | const boost::optional<std::string> & childClass, | ||
| 405 | const std::string & parentName = ""); | ||
| 406 | |||
| 407 | /// @brief Parse all the info from the compile node into compilerInfo | ||
| 408 | /// @param el ptree compile node | ||
| 409 | void parseCompiler(const ptree & el); | ||
| 410 | |||
| 411 | /// @brief Parse all the info from a texture node | ||
| 412 | /// @param el ptree texture node | ||
| 413 | void parseTexture(const ptree & el); | ||
| 414 | |||
| 415 | /// @brief Parse all the info from a material node | ||
| 416 | /// @param el ptree material node | ||
| 417 | void parseMaterial(const ptree & el); | ||
| 418 | |||
| 419 | /// @brief Parse all the info from a mesh node | ||
| 420 | /// @param el ptree mesh node | ||
| 421 | void parseMesh(const ptree & el); | ||
| 422 | |||
| 423 | /// @brief Parse all the info from the meta tag asset (mesh, material, texture) | ||
| 424 | /// @param el ptree texture node | ||
| 425 | void parseAsset(const ptree & el); | ||
| 426 | |||
| 427 | /// @brief Parse all the info from the meta tag keyframe | ||
| 428 | /// @param el ptree keyframe node | ||
| 429 | void parseKeyFrame(const ptree & el); | ||
| 430 | |||
| 431 | /// @brief parse the mjcf file into a graph | ||
| 432 | void parseGraph(); | ||
| 433 | |||
| 434 | /// @brief parse the mjcf file into a graph | ||
| 435 | /// @param xmlStr xml file name | ||
| 436 | void parseGraphFromXML(const std::string & xmlStr); | ||
| 437 | |||
| 438 | /// @brief Create a joint to add to the joint composite if needed | ||
| 439 | /// @tparam TypeX joint with axis X | ||
| 440 | /// @tparam TypeY joint with axis Y | ||
| 441 | /// @tparam TypeZ joint with axis Z | ||
| 442 | /// @tparam TypeUnaligned joint with axis unaligned | ||
| 443 | /// @param axis axis of the joint | ||
| 444 | /// @return one of the joint with the right axis | ||
| 445 | template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned> | ||
| 446 | JointModel createJoint(const Eigen::Vector3d & axis); | ||
| 447 | |||
| 448 | /// @brief Add a joint to the model. only needed when a body has a solo joint child | ||
| 449 | /// @param jointInfo The joint to add to the tree | ||
| 450 | /// @param currentBody The body associated with the joint | ||
| 451 | /// @param bodyInJoint Position of the body wrt to its joint | ||
| 452 | void | ||
| 453 | addSoloJoint(const MjcfJoint & jointInfo, const MjcfBody & currentBody, SE3 & bodyInJoint); | ||
| 454 | |||
| 455 | /// @brief Use all the infos that were parsed from the xml file to add a body and joint to | ||
| 456 | /// the model | ||
| 457 | /// @param nameOfBody Name of the body to add | ||
| 458 | void fillModel(const std::string & nameOfBody); | ||
| 459 | |||
| 460 | /// @brief Fill the pinocchio model with all the infos from the graph | ||
| 461 | void parseRootTree(); | ||
| 462 | |||
| 463 | /// @brief Fill reference configuration for a body and all it's associated dof | ||
| 464 | /// @param currentBody body to check | ||
| 465 | void fillReferenceConfig(const MjcfBody & currentBody); | ||
| 466 | |||
| 467 | /// @brief Add a keyframe to the model (ie reference configuration) | ||
| 468 | /// @param keyframe Keyframe to add | ||
| 469 | /// @param keyName Name of the keyframe | ||
| 470 | void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName); | ||
| 471 | |||
| 472 | /// @brief Fill geometry model with all the info taken from the mjcf model file | ||
| 473 | /// @param type Type of geometry to parse (COLLISION or VISUAL) | ||
| 474 | /// @param geomModel geometry model to fill | ||
| 475 | /// @param meshLoader mesh loader from hpp::fcl | ||
| 476 | void parseGeomTree( | ||
| 477 | const GeometryType & type, | ||
| 478 | GeometryModel & geomModel, | ||
| 479 | ::hpp::fcl::MeshLoaderPtr & meshLoader); | ||
| 480 | }; | ||
| 481 | namespace internal | ||
| 482 | { | ||
| 483 | 602 | inline std::istringstream getConfiguredStringStream(const std::string & str) | |
| 484 | { | ||
| 485 | 602 | std::istringstream posStream(str); | |
| 486 |
1/2✓ Branch 1 taken 602 times.
✗ Branch 2 not taken.
|
602 | posStream.exceptions(std::ios::failbit); |
| 487 | 602 | return posStream; | |
| 488 | } | ||
| 489 | |||
| 490 | template<int N> | ||
| 491 | 1190 | inline Eigen::Matrix<double, N, 1> getVectorFromStream(const std::string & str) | |
| 492 | { | ||
| 493 |
1/2✓ Branch 1 taken 595 times.
✗ Branch 2 not taken.
|
1190 | std::istringstream stream = getConfiguredStringStream(str); |
| 494 |
1/2✓ Branch 1 taken 595 times.
✗ Branch 2 not taken.
|
1190 | Eigen::Matrix<double, N, 1> vector; |
| 495 |
2/2✓ Branch 0 taken 1694 times.
✓ Branch 1 taken 595 times.
|
4578 | for (int i = 0; i < N; i++) |
| 496 |
2/4✓ Branch 1 taken 1694 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1694 times.
✗ Branch 5 not taken.
|
3388 | stream >> vector(i); |
| 497 | |||
| 498 | 2380 | return vector; | |
| 499 | 1190 | } | |
| 500 | |||
| 501 | 3 | inline Eigen::VectorXd getUnknownSizeVectorFromStream(const std::string & str) | |
| 502 | { | ||
| 503 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | std::istringstream stream = getConfiguredStringStream(str); |
| 504 | 3 | std::vector<double> vector; | |
| 505 | double elem; | ||
| 506 |
3/4✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 3 times.
|
19 | while (!stream.eof()) |
| 507 | { | ||
| 508 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | stream >> elem; |
| 509 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | vector.push_back(elem); |
| 510 | } | ||
| 511 | |||
| 512 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Eigen::VectorXd returnVector(vector.size()); |
| 513 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 3 times.
|
19 | for (std::size_t i = 0; i < vector.size(); i++) |
| 514 |
1/2✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
|
16 | returnVector(static_cast<Eigen::Index>(i)) = vector[i]; |
| 515 | |||
| 516 | 6 | return returnVector; | |
| 517 | 3 | } | |
| 518 | } // namespace internal | ||
| 519 | } // namespace details | ||
| 520 | } // namespace mjcf | ||
| 521 | } // namespace pinocchio | ||
| 522 | |||
| 523 | #endif // __pinocchio_parsers_mjcf_graph_hpp__ | ||
| 524 |