| Directory: | ./ |
|---|---|
| File: | include/pinocchio/parsers/mjcf/model.hxx |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 41 | 44 | 93.2% |
| Branches: | 20 | 42 | 47.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016-2023 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_parsers_mjcf_model_hxx__ | ||
| 6 | #define __pinocchio_parsers_mjcf_model_hxx__ | ||
| 7 | |||
| 8 | #include "pinocchio/parsers/mjcf.hpp" | ||
| 9 | #include "pinocchio/parsers/mjcf/mjcf-graph.hpp" | ||
| 10 | #include "pinocchio/multibody/model.hpp" | ||
| 11 | #include "pinocchio/algorithm/contact-info.hpp" | ||
| 12 | |||
| 13 | namespace pinocchio | ||
| 14 | { | ||
| 15 | namespace mjcf | ||
| 16 | { | ||
| 17 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 18 | 9 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 19 | const std::string & filename, | ||
| 20 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 21 | const bool verbose) | ||
| 22 | { | ||
| 23 | 9 | return buildModelFromXML(filename, model, verbose); | |
| 24 | } | ||
| 25 | |||
| 26 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 27 | 9 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 28 | const std::string & xmlStream, | ||
| 29 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 30 | const bool verbose) | ||
| 31 | { | ||
| 32 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 33 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
18 | return buildModelFromXML(xmlStream, model, contact_models, verbose); |
| 34 | 9 | } | |
| 35 | |||
| 36 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 37 | 2 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 38 | const std::string & filename, | ||
| 39 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 40 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 41 | const bool verbose) | ||
| 42 | { | ||
| 43 | 2 | return buildModelFromXML(filename, model, contact_models, verbose); | |
| 44 | } | ||
| 45 | |||
| 46 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 47 | 11 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 48 | const std::string & xmlStream, | ||
| 49 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 50 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 51 | const bool verbose) | ||
| 52 | { | ||
| 53 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | ::pinocchio::urdf::details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model); |
| 54 | |||
| 55 | typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; | ||
| 56 | |||
| 57 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | MjcfGraph graph(visitor, xmlStream); |
| 58 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
|
11 | if (verbose) |
| 59 | ✗ | visitor.log = &std::cout; | |
| 60 | |||
| 61 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | graph.parseGraphFromXML(xmlStream); |
| 62 | |||
| 63 | // Use the Mjcf graph to create the model | ||
| 64 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | graph.parseRootTree(); |
| 65 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | graph.parseContactInformation(model, contact_models); |
| 66 | |||
| 67 | 11 | return model; | |
| 68 | 11 | } | |
| 69 | |||
| 70 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 71 | 3 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 72 | const std::string & filename, | ||
| 73 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 74 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 75 | const bool verbose) | ||
| 76 | { | ||
| 77 | 3 | return buildModelFromXML(filename, rootJoint, model, verbose); | |
| 78 | } | ||
| 79 | |||
| 80 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 81 | 3 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 82 | const std::string & xmlStream, | ||
| 83 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 84 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 85 | const bool verbose) | ||
| 86 | { | ||
| 87 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 88 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, contact_models, verbose); |
| 89 | 3 | } | |
| 90 | |||
| 91 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 92 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 93 | const std::string & filename, | ||
| 94 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 95 | const std::string & rootJointName, | ||
| 96 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 97 | const bool verbose) | ||
| 98 | { | ||
| 99 | 1 | return buildModelFromXML(filename, rootJoint, rootJointName, model, verbose); | |
| 100 | } | ||
| 101 | |||
| 102 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 103 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 104 | const std::string & xmlStream, | ||
| 105 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 106 | const std::string & rootJointName, | ||
| 107 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 108 | const bool verbose) | ||
| 109 | { | ||
| 110 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 111 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return buildModelFromXML(xmlStream, rootJoint, rootJointName, model, contact_models, verbose); |
| 112 | 1 | } | |
| 113 | |||
| 114 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 115 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | ||
| 116 | const std::string & filename, | ||
| 117 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 118 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 119 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 120 | const bool verbose) | ||
| 121 | { | ||
| 122 | return buildModelFromXML(filename, rootJoint, model, contact_models, verbose); | ||
| 123 | } | ||
| 124 | |||
| 125 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 126 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | ||
| 127 | const std::string & xmlStream, | ||
| 128 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 129 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 130 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 131 | const bool verbose) | ||
| 132 | { | ||
| 133 | return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, contact_models, verbose); | ||
| 134 | } | ||
| 135 | |||
| 136 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 137 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 138 | const std::string & filename, | ||
| 139 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 140 | const std::string & rootJointName, | ||
| 141 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 142 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 143 | const bool verbose) | ||
| 144 | { | ||
| 145 | 1 | return buildModelFromXML(filename, rootJoint, rootJointName, model, contact_models, verbose); | |
| 146 | } | ||
| 147 | |||
| 148 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 149 | 5 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 150 | const std::string & xmlStream, | ||
| 151 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 152 | const std::string & rootJointName, | ||
| 153 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 154 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, | ||
| 155 | const bool verbose) | ||
| 156 | { | ||
| 157 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
|
5 | if (rootJointName.empty()) |
| 158 | ✗ | throw std::invalid_argument( | |
| 159 | "rootJoint was given without a name. Please fill the argument rootJointName"); | ||
| 160 | |||
| 161 | ::pinocchio::urdf::details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> | ||
| 162 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | visitor(model, rootJoint, rootJointName); |
| 163 | |||
| 164 | typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; | ||
| 165 | |||
| 166 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | MjcfGraph graph(visitor, xmlStream); |
| 167 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
|
5 | if (verbose) |
| 168 | ✗ | visitor.log = &std::cout; | |
| 169 | |||
| 170 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | graph.parseGraphFromXML(xmlStream); |
| 171 | |||
| 172 | // Use the Mjcf graph to create the model | ||
| 173 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | graph.parseRootTree(); |
| 174 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | graph.parseContactInformation(model, contact_models); |
| 175 | |||
| 176 | 5 | return model; | |
| 177 | 5 | } | |
| 178 | |||
| 179 | } // namespace mjcf | ||
| 180 | } // namespace pinocchio | ||
| 181 | |||
| 182 | #endif // __pinocchio_parsers_mjcf_model_hxx__ | ||
| 183 |