GCC Code Coverage Report

Directory: ./
File: src/parsers/mjcf/mjcf-graph.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 643 708 90.8%
Branches: 1005 2587 38.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2024 CNRS INRIA
3 //
4
5 #include "pinocchio/parsers/mjcf/mjcf-graph.hpp"
6 #include "pinocchio/multibody/model.hpp"
7 #include "pinocchio/algorithm/contact-info.hpp"
8
9 namespace pinocchio
10 {
11 namespace mjcf
12 {
13 namespace details
14 {
15 typedef boost::property_tree::ptree ptree;
16 typedef pinocchio::urdf::details::
17 UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl>
18 UrdfVisitor;
19
20 // supported elements from mjcf
21 static const std::array<std::string, 3> ELEMENTS = {"joint", "geom", "site"};
22
23 /// @brief Copy the value of ptree src into dst
24 /// @param src ptree to copy
25 /// @param dst ptree where copy is made
26 30 static void copyPtree(const ptree & src, ptree & dst)
27 {
28
6/10
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 44 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 74 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 44 times.
✓ Branch 13 taken 30 times.
 74 for (const ptree::value_type & v : src)
29
3/6
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 44 times.
✗ Branch 10 not taken.
 44 dst.put(ptree::path_type(v.first), v.second.data());
30 30 }
31
32 /// @brief Update class Element in order to have all info of parent classes
33 /// @param current current class
34 /// @param dst parent class
35 39 static void updateClassElement(ptree & current, const ptree & parent)
36 {
37
2/2
✓ Branch 0 taken 117 times.
✓ Branch 1 taken 39 times.
 156 for (const std::string & el : ELEMENTS)
38 {
39
1/2
✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
 117 std::string path = el + ".<xmlattr>";
40
4/6
✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 117 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✓ Branch 8 taken 87 times.
 234 if (parent.get_child_optional(path))
41 {
42
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 const ptree default_value = ptree();
43
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 30 ptree attr_parent = parent.get_child(path, default_value);
44
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 const ptree & attr_current = current.get_child(path, default_value);
45 // To only copy non existing attribute in current, we copy all current
46 // attribute (replacing) into a parent copy then we replace current with the new
47 // ptree
48
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 copyPtree(attr_current, attr_parent);
49
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 current.put_child(path, attr_parent);
50 30 }
51 117 }
52 39 }
53
54 7 static std::string getName(const ptree & el, const boost::filesystem::path & filePath)
55 {
56
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 14 auto n = el.get_optional<std::string>("<xmlattr>.name");
57
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
 7 if (n)
58
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 return *n;
59 else
60 {
61
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 if (filePath.extension().empty())
62 ✗ PINOCCHIO_THROW_PRETTY(
63 std::invalid_argument, "Cannot find extension for one of the mesh/texture");
64
65
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 auto st = filePath.stem();
66
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (!st.empty())
67
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return st.string();
68 else
69 ✗ PINOCCHIO_THROW_PRETTY(
70 std::invalid_argument, "Cannot find a name for one of the mesh.texture");
71 2 }
72 7 }
73
74 7 static boost::filesystem::path updatePath(
75 bool strippath,
76 const std::string & dir,
77 const std::string & modelPath,
78 const boost::filesystem::path & filePath)
79 {
80 namespace fs = boost::filesystem;
81
82 // Check if filename still has Absolute path, like said in mujoco doc
83
6/6
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 6 times.
 7 if (filePath.is_absolute() && !strippath)
84 1 return filePath;
85 else
86 {
87
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 auto filename = filePath;
88
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 5 times.
 6 if (strippath)
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 filename = filePath.filename();
90
91
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 fs::path meshPath(dir);
92
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 6 if (meshPath.is_absolute())
93 ✗ return (meshPath / filename);
94 else
95 {
96
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 fs::path mainPath(modelPath);
97
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 return (mainPath.parent_path() / meshPath / filename);
98 6 }
99 6 }
100 }
101
102 456 double MjcfCompiler::convertAngle(const double & angle_) const
103 {
104 456 return angle_ * angle_converter;
105 }
106
107 2 Eigen::Matrix3d MjcfCompiler::convertEuler(const Eigen::Vector3d & angles) const
108 {
109 Eigen::Matrix3d aa1 =
110
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 Eigen::AngleAxisd(convertAngle(angles(0)), mapEulerAngles.col(0)).toRotationMatrix();
111 Eigen::Matrix3d aa2 =
112
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 Eigen::AngleAxisd(convertAngle(angles(1)), mapEulerAngles.col(1)).toRotationMatrix();
113 Eigen::Matrix3d aa3 =
114
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 Eigen::AngleAxisd(convertAngle(angles(2)), mapEulerAngles.col(2)).toRotationMatrix();
115
116
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 return aa1 * aa2 * aa3;
117 }
118
119 template<int Nq, int Nv>
120 28 RangeJoint RangeJoint::setDimension() const
121 {
122 typedef UrdfVisitor::Vector Vector;
123 28 const double infty = std::numeric_limits<double>::infinity();
124
125
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 RangeJoint ret;
126
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.maxEffort = Vector::Constant(Nv, infty);
127
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.maxVel = Vector::Constant(Nv, infty);
128
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.maxConfig = Vector::Constant(Nq, 1.01);
129
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.minConfig = Vector::Constant(Nq, -1.01);
130
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.friction = Vector::Constant(Nv, 0.);
131
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 ret.damping = Vector::Constant(Nv, 0.);
132
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 ret.armature = Vector::Constant(Nv, armature[0]);
133 28 ret.frictionLoss = frictionLoss;
134
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ret.springStiffness = springStiffness;
135
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ret.springReference = springReference;
136 56 return ret;
137 }
138
139 template<int Nq, int Nv>
140 32 RangeJoint RangeJoint::concatenate(const RangeJoint & range) const
141 {
142
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(range.maxEffort.size() == Nv);
143
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(range.minConfig.size() == Nq);
144
145 32 RangeJoint ret(*this);
146
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.maxEffort.conservativeResize(maxEffort.size() + Nv);
147
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.maxEffort.tail(Nv) = range.maxEffort;
148
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.maxVel.conservativeResize(maxVel.size() + Nv);
149
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.maxVel.tail(Nv) = range.maxVel;
150
151
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.minConfig.conservativeResize(minConfig.size() + Nq);
152
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.minConfig.tail(Nq) = range.minConfig;
153
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.maxConfig.conservativeResize(maxConfig.size() + Nq);
154
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.maxConfig.tail(Nq) = range.maxConfig;
155
156
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.damping.conservativeResize(damping.size() + Nv);
157
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.damping.tail(Nv) = range.damping;
158
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.friction.conservativeResize(friction.size() + Nv);
159
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.friction.tail(Nv) = range.friction;
160
161
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.springReference.conservativeResize(springReference.size() + 1);
162
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.springReference.tail(1) = range.springReference;
163
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.springStiffness.conservativeResize(springStiffness.size() + 1);
164
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.springStiffness.tail(1) = range.springStiffness;
165
166
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 ret.armature.conservativeResize(armature.size() + Nv);
167
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ret.armature.tail(Nv) = range.armature;
168
169 32 return ret;
170 }
171
172 278 void MjcfJoint::goThroughElement(
173 const ptree & el, bool use_limits, const MjcfCompiler & currentCompiler)
174 {
175
176
4/18
✗ Branch 0 not taken.
✓ Branch 1 taken 278 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 278 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 278 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 278 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 278 if (!use_limits && el.get_optional<std::string>("<xmlattr>.range"))
177 ✗ PINOCCHIO_THROW_PRETTY(
178 std::invalid_argument, "Range limit is specified but it was not specify to use it.");
179
180 // Type
181
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 556 auto type_s = el.get_optional<std::string>("<xmlattr>.type");
182
2/2
✓ Branch 0 taken 66 times.
✓ Branch 1 taken 212 times.
 278 if (type_s)
183
2/4
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 66 times.
✗ Branch 5 not taken.
 66 jointType = *type_s;
184
185 // Axis
186
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 556 auto ax = el.get_optional<std::string>("<xmlattr>.axis");
187
2/2
✓ Branch 0 taken 209 times.
✓ Branch 1 taken 69 times.
 278 if (ax)
188
2/4
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 209 times.
✗ Branch 5 not taken.
 209 axis = internal::getVectorFromStream<3>(*ax);
189
190 // Range
191
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 556 auto range_ = el.get_optional<std::string>("<xmlattr>.range");
192
2/2
✓ Branch 0 taken 222 times.
✓ Branch 1 taken 56 times.
 278 if (range_)
193 {
194
2/4
✓ Branch 1 taken 222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 222 times.
✗ Branch 5 not taken.
 222 Eigen::Vector2d rangeT = internal::getVectorFromStream<2>(*range_);
195
3/6
✓ Branch 1 taken 222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 222 times.
✗ Branch 8 not taken.
 222 range.minConfig[0] = currentCompiler.convertAngle(rangeT(0));
196
3/6
✓ Branch 1 taken 222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 222 times.
✗ Branch 8 not taken.
 222 range.maxConfig[0] = currentCompiler.convertAngle(rangeT(1));
197 }
198 // Effort limit
199
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 range_ = el.get_optional<std::string>("<xmlattr>.actuatorfrcrange");
200
2/2
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 162 times.
 278 if (range_)
201 {
202
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 Eigen::Vector2d rangeT = internal::getVectorFromStream<2>(*range_);
203
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 range.maxEffort[0] = rangeT(1);
204 }
205
206 // Spring
207
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 auto value = el.get_optional<double>("<xmlattr>.springref");
208
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 278 times.
 278 if (value)
209 ✗ range.springReference[0] = *value;
210
211 // damping
212
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 value = el.get_optional<double>("<xmlattr>.damping");
213
2/2
✓ Branch 0 taken 106 times.
✓ Branch 1 taken 172 times.
 278 if (value)
214
2/4
✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106 times.
✗ Branch 5 not taken.
 106 range.damping[0] = *value;
215
216
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 value = el.get_optional<double>("<xmlattr>.armature");
217
2/2
✓ Branch 0 taken 97 times.
✓ Branch 1 taken 181 times.
 278 if (value)
218
2/4
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97 times.
✗ Branch 5 not taken.
 97 range.armature[0] = *value;
219
220 // friction loss
221
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 value = el.get_optional<double>("<xmlattr>.frictionloss");
222
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 278 times.
 278 if (value)
223 ✗ range.frictionLoss = *value;
224
225
2/4
✓ Branch 1 taken 278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 278 times.
✗ Branch 5 not taken.
 278 value = el.get_optional<double>("<xmlattr>.ref");
226
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 272 times.
 278 if (value)
227 {
228
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (jointType == "slide")
229
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 posRef = *value;
230
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 else if (jointType == "hinge")
231
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 posRef = currentCompiler.convertAngle(*value);
232 else
233 ✗ PINOCCHIO_THROW_PRETTY(
234 std::invalid_argument,
235 "Reference position can only be used with hinge or slide joints.");
236 }
237 278 }
238
239 194 void MjcfJoint::fill(
240 const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph)
241 {
242 194 bool use_limit = true;
243 // Name
244
2/4
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
 388 auto name_s = el.get_optional<std::string>("<xmlattr>.name");
245
2/2
✓ Branch 0 taken 185 times.
✓ Branch 1 taken 9 times.
 194 if (name_s)
246
2/4
✓ Branch 1 taken 185 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 185 times.
✗ Branch 5 not taken.
 185 jointName = *name_s;
247 else
248 jointName =
249
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 currentBody.bodyName + "Joint_" + std::to_string(currentBody.jointChildren.size());
250
251 // Check if we need to check for limited argument
252
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
 194 if (!currentGraph.compilerInfo.autolimits)
253 {
254 ✗ use_limit = false;
255 ✗ auto use_ls = el.get_optional<std::string>("<xmlattr>.limited");
256 ✗ use_limit = *use_ls == "true";
257 }
258
259 // Placement
260
2/4
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
 194 jointPlacement = currentGraph.convertPosition(el);
261
262 // default < ChildClass < Class < Real Joint
263
4/6
✓ Branch 3 taken 194 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 194 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 57 times.
✓ Branch 12 taken 137 times.
 194 if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
264 {
265
2/4
✓ Branch 2 taken 57 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57 times.
✗ Branch 6 not taken.
 57 const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
266
4/6
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 34 times.
✓ Branch 9 taken 23 times.
 114 if (auto joint_p = classD.classElement.get_child_optional("joint"))
267
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 34 goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo);
268 }
269 // childClass
270
2/2
✓ Branch 1 taken 52 times.
✓ Branch 2 taken 142 times.
 194 if (currentBody.childClass != "")
271 {
272
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 52 const MjcfClass & classE = currentGraph.mapOfClasses.at(currentBody.childClass);
273
4/6
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 30 times.
✓ Branch 9 taken 22 times.
 104 if (auto joint_p = classE.classElement.get_child_optional("joint"))
274
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 30 goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo);
275 }
276 // Class
277
2/4
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
 388 auto cl_s = el.get_optional<std::string>("<xmlattr>.class");
278
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 174 times.
 194 if (cl_s)
279 {
280
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 std::string className = *cl_s;
281
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 const MjcfClass & classE = currentGraph.mapOfClasses.at(className);
282
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
 40 if (auto joint_p = classE.classElement.get_child_optional("joint"))
283
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo);
284 20 }
285 // Joint
286
1/2
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
 194 goThroughElement(el, use_limit, currentGraph.compilerInfo);
287 194 }
288
289 724 SE3 MjcfGraph::convertPosition(const ptree & el) const
290 {
291
1/2
✓ Branch 1 taken 724 times.
✗ Branch 2 not taken.
 724 SE3 placement;
292
1/2
✓ Branch 1 taken 724 times.
✗ Branch 2 not taken.
 724 placement.setIdentity();
293
294 // position
295
2/4
✓ Branch 1 taken 724 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 724 times.
✗ Branch 5 not taken.
 1448 auto pos = el.get_optional<std::string>("<xmlattr>.pos");
296
2/2
✓ Branch 0 taken 461 times.
✓ Branch 1 taken 263 times.
 724 if (pos)
297
3/6
✓ Branch 1 taken 461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 461 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 461 times.
✗ Branch 8 not taken.
 461 placement.translation() = internal::getVectorFromStream<3>(*pos);
298
299 /////////// Rotation
300 // Quaternion (w, x, y, z)
301
2/4
✓ Branch 1 taken 724 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 724 times.
✗ Branch 5 not taken.
 1448 auto rot_s = el.get_optional<std::string>("<xmlattr>.quat");
302
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 698 times.
 724 if (rot_s)
303 {
304
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 26 Eigen::Vector4d quat = internal::getVectorFromStream<4>(*rot_s);
305
306
5/10
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 26 times.
✗ Branch 14 not taken.
 26 Eigen::Quaterniond quaternion(quat(0), quat(1), quat(2), quat(3));
307
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 quaternion.normalize();
308
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 26 placement.rotation() = quaternion.toRotationMatrix();
309 }
310 // Axis Angle
311
4/6
✓ Branch 1 taken 698 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 698 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 697 times.
 1396 else if ((rot_s = el.get_optional<std::string>("<xmlattr>.axisangle")))
312 {
313
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::Vector4d axis_angle = internal::getVectorFromStream<4>(*rot_s);
314
315
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 double angle = axis_angle(3);
316
317
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Eigen::AngleAxisd angleAxis(compilerInfo.convertAngle(angle), axis_angle.head(3));
318
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 placement.rotation() = angleAxis.toRotationMatrix();
319 }
320 // Euler Angles
321
4/6
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 695 times.
 1394 else if ((rot_s = el.get_optional<std::string>("<xmlattr>.euler")))
322 {
323
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::Vector3d angles = internal::getVectorFromStream<3>(*rot_s);
324
325
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 placement.rotation() = compilerInfo.convertEuler(angles);
326 }
327
4/6
✓ Branch 1 taken 695 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 695 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 15 times.
✓ Branch 10 taken 680 times.
 1390 else if ((rot_s = el.get_optional<std::string>("<xmlattr>.xyaxes")))
328 {
329
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 Eigen::Matrix<double, 6, 1> xyaxes = internal::getVectorFromStream<6>(*rot_s);
330
331
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 Eigen::Vector3d xAxis = xyaxes.head(3);
332
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 xAxis.normalize();
333
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 Eigen::Vector3d yAxis = xyaxes.tail(3);
334
335 // make y axis orthogonal to x axis, normalize
336
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 double d = xAxis.dot(yAxis);
337
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 yAxis -= xAxis * d;
338
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 yAxis.normalize();
339
340
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 Eigen::Vector3d zAxis = xAxis.cross(yAxis);
341
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 zAxis.normalize();
342
343
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 Eigen::Matrix3d rotation;
344
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 rotation.col(0) = xAxis;
345
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 rotation.col(1) = yAxis;
346
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 rotation.col(2) = zAxis;
347
348
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 placement.rotation() = rotation;
349 }
350
4/6
✓ Branch 1 taken 680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 680 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 679 times.
 1360 else if ((rot_s = el.get_optional<std::string>("<xmlattr>.zaxis")))
351 {
352
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::Vector3d zaxis = internal::getVectorFromStream<3>(*rot_s);
353 // Compute the rotation matrix that maps z_axis to unit z
354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 placement.rotation() =
355
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), zaxis).toRotationMatrix();
356 }
357 1448 return placement;
358 724 }
359
360 179 Inertia MjcfGraph::convertInertiaFromMjcf(const ptree & el) const
361 {
362
2/4
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
 179 double mass = std::max(el.get<double>("<xmlattr>.mass"), compilerInfo.boundMass);
363 ;
364
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 179 times.
 179 if (mass < 0)
365 ✗ PINOCCHIO_THROW_PRETTY(
366 std::invalid_argument, "Mass of body is not supposed to be negative");
367
368
1/2
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
 179 Inertia::Vector3 com;
369
2/4
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
 358 auto com_s = el.get_optional<std::string>("<xmlattr>.pos");
370
1/2
✓ Branch 0 taken 179 times.
✗ Branch 1 not taken.
 179 if (com_s)
371
2/4
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
 179 com = internal::getVectorFromStream<3>(*com_s);
372 else
373 ✗ com = Inertia::Vector3::Zero();
374
375
3/6
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 179 times.
✗ Branch 8 not taken.
 179 const Inertia::Matrix3 R = convertPosition(el).rotation();
376
377
2/4
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
 179 Inertia::Matrix3 I = Eigen::Matrix3d::Identity();
378
379
2/4
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
 358 auto inertia_s = el.get_optional<std::string>("<xmlattr>.diaginertia");
380
2/2
✓ Branch 0 taken 123 times.
✓ Branch 1 taken 56 times.
 179 if (inertia_s)
381 {
382
2/4
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 123 times.
✗ Branch 5 not taken.
 123 Eigen::Vector3d inertiaDiag = internal::getVectorFromStream<3>(*inertia_s);
383
2/4
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 123 times.
✗ Branch 5 not taken.
 123 I = inertiaDiag.asDiagonal();
384 }
385
386
3/6
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 56 times.
✗ Branch 10 not taken.
 112 else if ((inertia_s = el.get_optional<std::string>("<xmlattr>.fullinertia")))
387 {
388 // M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)
389
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 std::istringstream inertiaStream = internal::getConfiguredStringStream(*inertia_s);
390
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(0, 0);
391
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(1, 1);
392
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(2, 2);
393
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(0, 1);
394
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(0, 2);
395
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 inertiaStream >> I(1, 2);
396
397
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 I(1, 0) = I(0, 1);
398
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 I(2, 0) = I(0, 2);
399
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 I(2, 1) = I(1, 2);
400 56 }
401
402 // Extract the diagonal elements as a vector
403
2/2
✓ Branch 0 taken 537 times.
✓ Branch 1 taken 179 times.
 716 for (int i = 0; i < 3; i++)
404
2/4
✓ Branch 1 taken 537 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 537 times.
✗ Branch 6 not taken.
 537 I(i, i) = std::max(I(i, i), compilerInfo.boundInertia);
405
406
5/10
✓ Branch 1 taken 179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 179 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 179 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 179 times.
✗ Branch 14 not taken.
 358 return Inertia(mass, com, R * I * R.transpose());
407 179 }
408
409 237 void MjcfGraph::parseJointAndBody(
410 const ptree & el,
411 const boost::optional<std::string> & childClass,
412 const std::string & parentName)
413 {
414
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 MjcfBody currentBody;
415
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto chcl_s = childClass;
416 // if inertiafromgeom is false and inertia does not exist - throw
417
3/6
✓ Branch 2 taken 237 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 237 times.
✗ Branch 6 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 237 times.
 237 if (!compilerInfo.inertiafromgeom && !el.get_child_optional("inertial"))
418 ✗ PINOCCHIO_THROW_PRETTY(
419 std::invalid_argument, "Cannot get inertia from geom and no inertia was found");
420
421 237 bool usegeominertia = false;
422
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 221 times.
 237 if (compilerInfo.inertiafromgeom)
423 16 usegeominertia = true;
424 221 else if (
425
8/14
✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 221 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 44 times.
✓ Branch 11 taken 177 times.
✓ Branch 12 taken 221 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 44 times.
✓ Branch 16 taken 177 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 221 boost::indeterminate(compilerInfo.inertiafromgeom) && !el.get_child_optional("inertial"))
426 44 usegeominertia = true;
427
428
7/12
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 868 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 868 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1105 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 868 times.
✓ Branch 16 taken 237 times.
 1105 for (const ptree::value_type & v : el)
429 {
430 // Current body node
431
2/2
✓ Branch 1 taken 237 times.
✓ Branch 2 taken 631 times.
 868 if (v.first == "<xmlattr>")
432 {
433 // Name
434
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
 474 auto name_s = v.second.get_optional<std::string>("name");
435
1/2
✓ Branch 0 taken 237 times.
✗ Branch 1 not taken.
 237 if (name_s)
436
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
 237 currentBody.bodyName = *name_s;
437 else
438 ✗ currentBody.bodyName = parentName + "Bis";
439
440
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 currentBody.bodyParent = parentName;
441
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
 237 currentBody.bodyPlacement = convertPosition(el);
442
443
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 bodiesList.push_back(currentBody.bodyName);
444
445
4/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✓ Branch 8 taken 226 times.
 711 if (auto chcl_st = v.second.get_optional<std::string>("childclass"))
446 {
447
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 chcl_s = chcl_st;
448
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 currentBody.childClass = *chcl_s;
449 }
450
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 172 times.
 226 else if (childClass)
451
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 237 currentBody.childClass = *chcl_s;
452
453 // Class
454
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
 474 auto cl_s = v.second.get_optional<std::string>("class");
455
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 237 times.
 237 if (cl_s)
456 ✗ currentBody.bodyClassName = *cl_s;
457
458 // Still need to deal with gravcomp and figure out if we need mocap, and user param...
459 237 }
460 // Inertia
461
5/6
✓ Branch 1 taken 177 times.
✓ Branch 2 taken 691 times.
✓ Branch 3 taken 177 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 177 times.
✓ Branch 6 taken 691 times.
 868 if (v.first == "inertial" && !usegeominertia)
462
2/4
✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 177 times.
✗ Branch 5 not taken.
 177 currentBody.bodyInertia = convertInertiaFromMjcf(v.second);
463
464 // Geom
465
2/2
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 818 times.
 868 if (v.first == "geom")
466 {
467
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 MjcfGeom currentGeom;
468
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 currentGeom.fill(v.second, currentBody, *this);
469
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 currentBody.geomChildren.push_back(currentGeom);
470 50 }
471
472 // Joint
473
2/2
✓ Branch 1 taken 194 times.
✓ Branch 2 taken 674 times.
 868 if (v.first == "joint")
474 {
475
1/2
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
 194 MjcfJoint currentJoint;
476
1/2
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
 194 currentJoint.fill(v.second, currentBody, *this);
477
1/2
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
 194 currentBody.jointChildren.push_back(currentJoint);
478 194 }
479
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 864 times.
 868 if (v.first == "freejoint")
480 {
481
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 MjcfJoint currentJoint;
482
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 currentJoint.jointType = "free";
483
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 auto jointName = v.second.get_optional<std::string>("<xmlattr>.name");
484
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 if (jointName)
485
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 currentJoint.jointName = *jointName;
486 else
487
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 currentJoint.jointName = currentBody.bodyName + "_free";
488
489
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 currentBody.jointChildren.push_back(currentJoint);
490 4 }
491
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 866 times.
 868 if (v.first == "site")
492 {
493
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 MjcfSite currentSite;
494
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 currentSite.fill(v.second, currentBody, *this);
495
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 currentBody.siteChildren.push_back(currentSite);
496 2 }
497
2/2
✓ Branch 1 taken 202 times.
✓ Branch 2 taken 666 times.
 868 if (v.first == "body")
498 {
499
1/2
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
 202 parseJointAndBody(v.second, chcl_s, currentBody.bodyName);
500 }
501 }
502 // Add all geom inertias if needed
503
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 177 times.
 237 if (usegeominertia)
504 {
505
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 Inertia inert_temp(Inertia::Zero());
506
2/2
✓ Branch 5 taken 23 times.
✓ Branch 6 taken 60 times.
 83 for (const auto & geom : currentBody.geomChildren)
507 {
508
1/2
✓ Branch 0 taken 23 times.
✗ Branch 1 not taken.
 23 if (geom.geomKind != MjcfGeom::VISUAL)
509
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 inert_temp += geom.geomPlacement.act(geom.geomInertia);
510 }
511
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 currentBody.bodyInertia = inert_temp;
512 }
513
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
 237 mapOfBodies.insert(std::make_pair(currentBody.bodyName, currentBody));
514 237 }
515
516 1 void MjcfGraph::parseTexture(const ptree & el)
517 {
518 namespace fs = boost::filesystem;
519
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 MjcfTexture text;
520
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 auto file = el.get_optional<std::string>("<xmlattr>.file");
521
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 auto name_ = el.get_optional<std::string>("<xmlattr>.name");
522
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 auto type = el.get_optional<std::string>("<xmlattr>.type");
523
524 1 std::string name;
525
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (name_)
526
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 name = *name_;
527 ✗ else if (type && *type == "skybox")
528 ✗ name = *type;
529
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!file)
530 {
531 ✗ std::cout << "Warning - Only texture with files are supported" << std::endl;
532 ✗ if (name.empty())
533 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Textures need a name.");
534 }
535 else
536 {
537
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 fs::path filePath(*file);
538
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 name = getName(el, filePath);
539
540 text.filePath =
541
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 updatePath(compilerInfo.strippath, compilerInfo.texturedir, modelPath, filePath)
542
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 .string();
543 1 }
544
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 auto str_v = el.get_optional<std::string>("<xmlattr>.type");
545
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (str_v)
546
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 text.textType = *str_v;
547
548
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 2 if ((str_v = el.get_optional<std::string>("<xmlattr>.gridsize")))
549 ✗ text.gridsize = internal::getVectorFromStream<2>(*str_v);
550
551
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 mapOfTextures.insert(std::make_pair(name, text));
552 1 }
553
554 1 void MjcfGraph::parseMaterial(const ptree & el)
555 {
556 1 std::string name;
557
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 MjcfMaterial mat;
558
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 auto n = el.get_optional<std::string>("<xmlattr>.name");
559
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (n)
560
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 name = *n;
561 else
562 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Material was given without a name");
563
564 // default < Class < Attributes
565
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
 1 if (mapOfClasses.find("mujoco_default") != mapOfClasses.end())
566 {
567 ✗ const MjcfClass & classD = mapOfClasses.at("mujoco_default");
568 ✗ if (auto mat_p = classD.classElement.get_child_optional("material"))
569 ✗ mat.goThroughElement(*mat_p);
570 }
571
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 auto cl_s = el.get_optional<std::string>("<xmlattr>.class");
572
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (cl_s)
573 {
574 ✗ std::string className = *cl_s;
575 ✗ const MjcfClass & classE = mapOfClasses.at(className);
576 ✗ if (auto mat_p = classE.classElement.get_child_optional("material"))
577 ✗ mat.goThroughElement(*mat_p);
578 }
579
580
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 mat.goThroughElement(el);
581
582
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 mapOfMaterials.insert(std::make_pair(name, mat));
583 1 }
584
585 7 void MjcfGraph::parseMesh(const ptree & el)
586 {
587 namespace fs = boost::filesystem;
588
589
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 MjcfMesh mesh;
590
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 auto file = el.get_optional<std::string>("<xmlattr>.file");
591
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 14 auto scale = el.get_optional<std::string>("<xmlattr>.scale");
592
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 6 times.
 7 if (scale)
593
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 mesh.scale = internal::getVectorFromStream<3>(*scale);
594
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 if (file)
595 {
596
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 fs::path filePath(*file);
597
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 std::string name = getName(el, filePath);
598
599 mesh.filePath =
600
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 updatePath(compilerInfo.strippath, compilerInfo.meshdir, modelPath, filePath).string();
601
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 mapOfMeshes.insert(std::make_pair(name, mesh));
602 6 return;
603 6 }
604
605 // Handle vertex-based mesh
606
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 auto vertex = el.get_optional<std::string>("<xmlattr>.vertex");
607
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!vertex)
608 {
609 ✗ PINOCCHIO_THROW_PRETTY(
610 std::invalid_argument, "Only meshes with files/vertices are supported.")
611 }
612
613
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 auto name = el.get_optional<std::string>("<xmlattr>.name");
614
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!name)
615 {
616 ✗ PINOCCHIO_THROW_PRETTY(
617 std::invalid_argument, "Mesh with vertices without a name is not supported");
618 }
619
620 // Parse and validate vertices
621
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::VectorXd meshVertices = internal::getUnknownSizeVectorFromStream(*vertex);
622
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (meshVertices.size() % 3 != 0)
623 {
624 ✗ PINOCCHIO_THROW_PRETTY(
625 std::invalid_argument, "Number of vertices is not a multiple of 3");
626 }
627
628 // Convert to 3D vertex matrix
629 1 const auto numVertices = meshVertices.size() / 3;
630
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::MatrixX3d vertices(numVertices, 3);
631
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
 10 for (auto i = 0; i < numVertices; ++i)
632 {
633
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 9 vertices.row(i) = meshVertices.segment<3>(3 * i).transpose();
634 }
635
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 mesh.vertices = vertices;
636
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 mapOfMeshes.insert(std::make_pair(*name, mesh));
637
6/6
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 6 times.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 6 times.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 6 times.
 19 }
638
639 7 void MjcfGraph::parseAsset(const ptree & el)
640 {
641
7/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 16 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 9 times.
✓ Branch 16 taken 7 times.
 16 for (const ptree::value_type & v : el)
642 {
643
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 2 times.
 9 if (v.first == "mesh")
644
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 parseMesh(v.second);
645
646
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 8 times.
 9 if (v.first == "material")
647
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 parseMaterial(v.second);
648
649
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 8 times.
 9 if (v.first == "texture")
650
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 parseTexture(v.second);
651
652
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
 9 if (v.first == "hfield")
653 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "hfields are not supported yet");
654 }
655 7 }
656
657 43 void MjcfGraph::parseDefault(ptree & el, const ptree & parent, const std::string & parentTag)
658 {
659 43 boost::optional<std::string> nameClass;
660 // Classes
661
7/12
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 116 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 159 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 116 times.
✓ Branch 16 taken 43 times.
 159 for (ptree::value_type & v : el)
662 {
663
2/2
✓ Branch 1 taken 39 times.
✓ Branch 2 taken 77 times.
 116 if (v.first == "<xmlattr>")
664 {
665
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 39 nameClass = v.second.get_optional<std::string>("class");
666
1/2
✓ Branch 0 taken 39 times.
✗ Branch 1 not taken.
 39 if (nameClass)
667 {
668
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 MjcfClass classDefault;
669
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 39 classDefault.className = *nameClass;
670
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 updateClassElement(el, parent);
671
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 classDefault.classElement = el;
672
3/6
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
 39 mapOfClasses.insert(std::make_pair(*nameClass, classDefault));
673 39 }
674 else
675 ✗ PINOCCHIO_THROW_PRETTY(
676 std::invalid_argument, "Class does not have a name. Cannot parse model.");
677 }
678
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 48 times.
 77 else if (v.first == "default")
679
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 parseDefault(v.second, el, v.first);
680
5/6
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 34 times.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 14 times.
✓ Branch 7 taken 34 times.
 48 else if (parentTag == "mujoco" && v.first != "<xmlattr>")
681 {
682
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 MjcfClass classDefault;
683
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 classDefault.className = "mujoco_default";
684
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 classDefault.classElement = el;
685
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 14 mapOfClasses.insert(std::make_pair("mujoco_default", classDefault));
686 14 }
687 }
688 43 }
689
690 16 void MjcfGraph::parseCompiler(const ptree & el)
691 {
692 // get autolimits
693
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto auto_s = el.get_optional<std::string>("<xmlattr>.autolimits");
694
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 11 times.
 16 if (auto_s)
695
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 if (*auto_s == "true")
696 5 compilerInfo.autolimits = true;
697
698 // strip path
699
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto strip_s = el.get_optional<std::string>("<xmlattr>.strippath");
700
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 15 times.
 16 if (strip_s)
701
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 if (*strip_s == "true")
702 1 compilerInfo.strippath = true;
703
704 // get dir to mesh and texture
705
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto dir = el.get_optional<std::string>("<xmlattr>.assetdir");
706
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (dir)
707 {
708 ✗ compilerInfo.meshdir = *dir;
709 ✗ compilerInfo.texturedir = *dir;
710 }
711
712
4/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 7 times.
✓ Branch 10 taken 9 times.
 32 if ((dir = el.get_optional<std::string>("<xmlattr>.meshdir")))
713
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 compilerInfo.meshdir = *dir;
714
715
4/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 3 times.
✓ Branch 10 taken 13 times.
 32 if ((dir = el.get_optional<std::string>("<xmlattr>.texturedir")))
716
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 compilerInfo.texturedir = *dir;
717
718
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 auto value_v = el.get_optional<double>("<xmlattr>.boundmass");
719
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (value_v)
720 ✗ compilerInfo.boundMass = *value_v;
721
722
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 16 times.
 32 if ((value_v = el.get_optional<double>("<xmlattr>.boundinertia")))
723 ✗ compilerInfo.boundInertia = *value_v;
724
725
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto in_g = el.get_optional<std::string>("<xmlattr>.inertiafromgeom");
726
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 12 times.
 16 if (in_g)
727 {
728
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 if (*in_g == "true")
729 4 compilerInfo.inertiafromgeom = true;
730 ✗ else if (*in_g == "false")
731 ✗ compilerInfo.inertiafromgeom = false;
732 }
733
734 // angle radian or degree
735
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto angle_s = el.get_optional<std::string>("<xmlattr>.angle");
736
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 7 times.
 16 if (angle_s)
737
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 if (*angle_s == "radian")
738 9 compilerInfo.angle_converter = 1;
739
740
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 auto eulerS = el.get_optional<std::string>("<xmlattr>.eulerseq");
741
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 14 times.
 16 if (eulerS)
742 {
743
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 std::string eulerseq = *eulerS;
744
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 2 if (eulerseq.find_first_not_of("xyzXYZ") != std::string::npos || eulerseq.size() != 3)
745 {
746 ✗ PINOCCHIO_THROW_PRETTY(
747 std::invalid_argument, "Model tried to use euler angles but euler sequence is wrong");
748 }
749 else
750 {
751 // get index combination
752
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
 8 for (std::size_t i = 0; i < eulerseq.size(); i++)
753 {
754 6 auto ci = static_cast<Eigen::Index>(i);
755
4/9
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 switch (eulerseq[i])
756 {
757 2 case 'x':
758
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitX();
759 2 break;
760 ✗ case 'X':
761 ✗ compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitX();
762 ✗ break;
763 2 case 'y':
764
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitY();
765 2 break;
766 ✗ case 'Y':
767 ✗ compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitY();
768 ✗ break;
769 2 case 'z':
770
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitZ();
771 2 break;
772 ✗ case 'Z':
773 ✗ compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitZ();
774 ✗ break;
775 ✗ default:
776 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Euler Axis does not exist");
777 break;
778 }
779 }
780 }
781 2 }
782 16 }
783
784 3 void MjcfGraph::parseKeyFrame(const ptree & el)
785 {
786
7/12
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 3 times.
✓ Branch 16 taken 3 times.
 6 for (const ptree::value_type & v : el)
787 {
788
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 if (v.first == "key")
789 {
790
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 auto nameKey = v.second.get_optional<std::string>("<xmlattr>.name");
791
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (nameKey)
792 {
793
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 auto configVectorS = v.second.get_optional<std::string>("<xmlattr>.qpos");
794
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (configVectorS)
795 {
796 Eigen::VectorXd configVector =
797
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 internal::getUnknownSizeVectorFromStream(*configVectorS);
798
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 mapOfConfigs.insert(std::make_pair(*nameKey, configVector));
799 3 }
800 3 }
801 3 }
802 }
803 3 }
804
805 11 void MjcfGraph::parseEquality(const ptree & el)
806 {
807
7/12
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 35 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 24 times.
✓ Branch 16 taken 11 times.
 35 for (const ptree::value_type & v : el)
808 {
809
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 std::string type = v.first;
810 // List of supported constraints from mjcf description
811 // equality -> connect
812
813 // The constraints below are not supported and will be ignored with the following
814 // warning: joint, flex, distance, weld
815
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 14 times.
 24 if (type != "connect")
816 {
817 // TODO(jcarpent): support extra constraint types such as joint, flex, distance, weld.
818 10 continue;
819 }
820
821
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 MjcfEquality eq;
822
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 eq.type = type;
823
824 // get the name of first body
825
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 auto body1 = v.second.get_optional<std::string>("<xmlattr>.body1");
826
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 if (body1)
827
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 14 eq.body1 = *body1;
828 else
829 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Equality constraint needs a first body");
830
831 // get the name of second body
832
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 auto body2 = v.second.get_optional<std::string>("<xmlattr>.body2");
833
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 if (body2)
834
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 14 eq.body2 = *body2;
835
836 // get the name of the constraint (if it exists)
837
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 auto name = v.second.get_optional<std::string>("<xmlattr>.name");
838
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 14 if (name)
839 ✗ eq.name = *name;
840 else
841
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 14 eq.name = eq.body1 + "_" + eq.body2 + "_constraint";
842
843 // get the anchor position
844
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 auto anchor = v.second.get_optional<std::string>("<xmlattr>.anchor");
845
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 10 times.
 14 if (anchor)
846
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 eq.anchor = internal::getVectorFromStream<3>(*anchor);
847
848
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 14 mapOfEqualities.insert(std::make_pair(eq.name, eq));
849
2/2
✓ Branch 6 taken 14 times.
✓ Branch 7 taken 10 times.
 24 }
850 11 }
851
852 38 void MjcfGraph::parseGraph()
853 {
854
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 boost::property_tree::ptree el;
855
3/6
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
 76 if (pt.get_child_optional("mujoco"))
856
3/6
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
 38 el = pt.get_child("mujoco");
857 else
858 ✗ PINOCCHIO_THROW_PRETTY(
859 std::invalid_argument, "This is not a standard mujoco model. Cannot parse it.");
860
861
7/12
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 125 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 163 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 125 times.
✓ Branch 16 taken 38 times.
 163 for (const ptree::value_type & v : el)
862 {
863 // get model name
864
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 87 times.
 125 if (v.first == "<xmlattr>")
865 {
866
2/4
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
 76 auto n_s = v.second.get_optional<std::string>("model");
867
1/2
✓ Branch 0 taken 38 times.
✗ Branch 1 not taken.
 38 if (n_s)
868
2/4
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
 38 modelName = *n_s;
869 else
870 ✗ PINOCCHIO_THROW_PRETTY(
871 std::invalid_argument, "Model is missing a name. Cannot parse it");
872 38 }
873
874
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 110 times.
 125 if (v.first == "compiler")
875
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 15 parseCompiler(el.get_child("compiler"));
876
877
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 112 times.
 125 if (v.first == "default")
878
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
 13 parseDefault(el.get_child("default"), el, "mujoco");
879
880
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 118 times.
 125 if (v.first == "asset")
881
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 parseAsset(el.get_child("asset"));
882
883
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 122 times.
 125 if (v.first == "keyframe")
884
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 parseKeyFrame(el.get_child("keyframe"));
885
886
2/2
✓ Branch 1 taken 35 times.
✓ Branch 2 taken 90 times.
 125 if (v.first == "worldbody")
887 {
888 35 boost::optional<std::string> childClass;
889
6/12
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 35 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 35 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 35 times.
✗ Branch 18 not taken.
 35 parseJointAndBody(el.get_child("worldbody").get_child("body"), childClass);
890 35 }
891
892
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 114 times.
 125 if (v.first == "equality")
893 {
894
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 parseEquality(el.get_child("equality"));
895 }
896 }
897 38 }
898
899 38 void MjcfGraph::parseGraphFromXML(const std::string & xmlStr)
900 {
901
1/2
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
 38 boost::property_tree::read_xml(xmlStr, pt);
902 38 parseGraph();
903 38 }
904
905 template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned>
906 30 JointModel MjcfGraph::createJoint(const Eigen::Vector3d & axis)
907 {
908
4/6
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 9 times.
 30 if (axis.isApprox(Eigen::Vector3d::UnitX()))
909
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 24 return TypeX();
910
4/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 5 times.
 18 else if (axis.isApprox(Eigen::Vector3d::UnitY()))
911
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 16 return TypeY();
912
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 10 else if (axis.isApprox(Eigen::Vector3d::UnitZ()))
913
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 20 return TypeZ();
914 else
915 ✗ return TypeUnaligned(axis.normalized());
916 }
917
918 164 void MjcfGraph::addSoloJoint(
919 const MjcfJoint & joint, const MjcfBody & currentBody, SE3 & bodyInJoint)
920 {
921
922 164 FrameIndex parentFrameId = 0;
923
2/2
✓ Branch 1 taken 161 times.
✓ Branch 2 taken 3 times.
 164 if (!currentBody.bodyParent.empty())
924
1/2
✓ Branch 1 taken 161 times.
✗ Branch 2 not taken.
 161 parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent);
925
926 // get body pose in body parent
927
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 const SE3 bodyPose = currentBody.bodyPlacement;
928
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 Inertia inert = currentBody.bodyInertia;
929
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 SE3 jointInParent = bodyPose * joint.jointPlacement;
930
2/4
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164 times.
✗ Branch 5 not taken.
 164 bodyInJoint = joint.jointPlacement.inverse();
931 UrdfVisitor::JointType jType;
932
933
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 RangeJoint range;
934
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 160 times.
 164 if (joint.jointType == "free")
935 {
936
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 urdfVisitor << "Free Joint " << '\n';
937
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 range = joint.range.setDimension<7, 6>();
938 4 jType = UrdfVisitor::FLOATING;
939 }
940
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 154 times.
 160 else if (joint.jointType == "slide")
941 {
942
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 urdfVisitor << "joint prismatic with axis " << joint.axis << '\n';
943
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 range = joint.range;
944 6 jType = UrdfVisitor::PRISMATIC;
945 }
946
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 145 times.
 154 else if (joint.jointType == "ball")
947 {
948
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 urdfVisitor << "Sphere Joint " << '\n';
949
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 range = joint.range.setDimension<4, 3>();
950 9 jType = UrdfVisitor::SPHERICAL;
951 }
952
1/2
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
 145 else if (joint.jointType == "hinge")
953 {
954
3/6
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 145 times.
✗ Branch 8 not taken.
 145 urdfVisitor << "joint revolute with axis " << joint.axis << '\n';
955
1/2
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
 145 range = joint.range;
956 145 jType = UrdfVisitor::REVOLUTE;
957 }
958 else
959 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Unknown joint type");
960
961
6/12
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 164 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 164 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 164 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 164 times.
✗ Branch 17 not taken.
 328 urdfVisitor.addJointAndBody(
962 164 jType, joint.axis, parentFrameId, jointInParent, joint.jointName, inert, bodyInJoint,
963
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 currentBody.bodyName, range.maxEffort, range.maxVel, range.minConfig, range.maxConfig,
964 range.friction, range.damping);
965
966 // Add armature info
967
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 JointIndex j_id = urdfVisitor.getJointId(joint.jointName);
968
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 urdfVisitor.model.armature.segment(
969
2/4
✓ Branch 3 taken 164 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 164 times.
✗ Branch 7 not taken.
 328 urdfVisitor.model.joints[j_id].idx_v(), urdfVisitor.model.joints[j_id].nv()) =
970
1/2
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
 164 range.armature;
971 164 }
972
973 203 void MjcfGraph::fillModel(const std::string & nameOfBody)
974 {
975 typedef UrdfVisitor::SE3 SE3;
976
977
2/4
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
 203 MjcfBody currentBody = mapOfBodies.at(nameOfBody);
978
979 // get parent body frame
980 203 FrameIndex parentFrameId = 0;
981
2/2
✓ Branch 1 taken 176 times.
✓ Branch 2 taken 27 times.
 203 if (!currentBody.bodyParent.empty())
982
1/2
✓ Branch 1 taken 176 times.
✗ Branch 2 not taken.
 176 parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent);
983
984
1/2
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 203 Frame frame = urdfVisitor.model.frames[parentFrameId];
985 // get body pose in body parent
986
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 203 const SE3 bodyPose = currentBody.bodyPlacement;
987
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 203 Inertia inert = currentBody.bodyInertia;
988
989 // Fixed Joint
990
2/2
✓ Branch 1 taken 32 times.
✓ Branch 2 taken 171 times.
 203 if (currentBody.jointChildren.size() == 0)
991 {
992
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 8 times.
 32 if (currentBody.bodyParent.empty())
993 24 return;
994
995
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 std::string jointName = nameOfBody + "_fixed";
996
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 urdfVisitor << jointName << " being parsed." << '\n';
997
998
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 urdfVisitor.addFixedJointAndBody(parentFrameId, bodyPose, jointName, inert, nameOfBody);
999 8 return;
1000 8 }
1001
1002 171 bool composite = false;
1003
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 171 SE3 jointPlacement, firstJointPlacement, prevJointPlacement = SE3::Identity();
1004
1005
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 RangeJoint rangeCompo;
1006
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 JointModelComposite jointM;
1007 171 std::string jointName;
1008
1009
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 164 times.
 171 if (currentBody.jointChildren.size() > 1)
1010 {
1011 7 composite = true;
1012
1013
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 MjcfJoint firstOne = currentBody.jointChildren.at(0);
1014
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 jointName = "Composite_" + firstOne.jointName;
1015 7 }
1016
1017
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 fillReferenceConfig(currentBody);
1018
1019 171 bool isFirst = true;
1020
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 SE3 bodyInJoint;
1021
1022
2/2
✓ Branch 0 taken 164 times.
✓ Branch 1 taken 7 times.
 171 if (!composite)
1023 {
1024
2/4
✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164 times.
✗ Branch 5 not taken.
 164 addSoloJoint(currentBody.jointChildren.at(0), currentBody, bodyInJoint);
1025 }
1026 else
1027 {
1028
2/2
✓ Branch 5 taken 16 times.
✓ Branch 6 taken 7 times.
 23 for (const auto & joint : currentBody.jointChildren)
1029 {
1030
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 if (joint.jointType == "free")
1031 ✗ PINOCCHIO_THROW_PRETTY(
1032 std::invalid_argument, "Joint Composite trying to be created with a freeFlyer");
1033
1034
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 SE3 jointInParent = bodyPose * joint.jointPlacement;
1035
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 bodyInJoint = joint.jointPlacement.inverse();
1036
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 9 times.
 16 if (isFirst)
1037 {
1038
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 firstJointPlacement = jointInParent;
1039
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 jointPlacement = SE3::Identity();
1040 7 isFirst = false;
1041 }
1042 else
1043
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 jointPlacement = prevJointPlacement.inverse() * jointInParent;
1044
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 10 times.
 16 if (joint.jointType == "slide")
1045 {
1046
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 jointM.addJoint(
1047 6 createJoint<JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned>(
1048
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 joint.axis),
1049 jointPlacement);
1050
1051
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 rangeCompo = rangeCompo.concatenate<1, 1>(joint.range);
1052 }
1053
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 9 times.
 10 else if (joint.jointType == "ball")
1054 {
1055
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 jointM.addJoint(JointModelSpherical(), jointPlacement);
1056
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 rangeCompo = rangeCompo.concatenate<4, 3>(joint.range.setDimension<4, 3>());
1057 }
1058
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 else if (joint.jointType == "hinge")
1059 {
1060
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 jointM.addJoint(
1061 9 createJoint<JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned>(
1062
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 joint.axis),
1063 jointPlacement);
1064
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 rangeCompo = rangeCompo.concatenate<1, 1>(joint.range);
1065 }
1066 else
1067 ✗ PINOCCHIO_THROW_PRETTY(
1068 std::invalid_argument, "Unknown joint type trying to be parsed.");
1069
1070
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 prevJointPlacement = jointInParent;
1071 }
1072 JointIndex joint_id;
1073
1074
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 joint_id = urdfVisitor.model.addJoint(
1075
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 frame.parentJoint, jointM, frame.placement * firstJointPlacement, jointName,
1076 rangeCompo.maxEffort, rangeCompo.maxVel, rangeCompo.minConfig, rangeCompo.maxConfig,
1077 rangeCompo.friction, rangeCompo.damping);
1078
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 FrameIndex jointFrameId = urdfVisitor.model.addJointFrame(joint_id, (int)parentFrameId);
1079
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody);
1080
1081
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 urdfVisitor.model.armature.segment(
1082
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 14 urdfVisitor.model.joints[joint_id].idx_v(), urdfVisitor.model.joints[joint_id].nv()) =
1083
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 rangeCompo.armature;
1084 }
1085
1086
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 FrameIndex bodyId = urdfVisitor.model.getFrameId(nameOfBody, BODY);
1087
1/2
✓ Branch 2 taken 171 times.
✗ Branch 3 not taken.
 171 frame = urdfVisitor.model.frames[bodyId];
1088
2/2
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 171 times.
 173 for (const auto & site : currentBody.siteChildren)
1089 {
1090
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 SE3 placement = bodyInJoint * site.sitePlacement;
1091
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 urdfVisitor.model.addFrame(
1092
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME));
1093 }
1094
4/4
✓ Branch 4 taken 171 times.
✓ Branch 5 taken 32 times.
✓ Branch 7 taken 171 times.
✓ Branch 8 taken 32 times.
 235 }
1095
1096 198 void MjcfGraph::fillReferenceConfig(const MjcfBody & currentBody)
1097 {
1098
2/2
✓ Branch 5 taken 183 times.
✓ Branch 6 taken 198 times.
 381 for (const auto & joint : currentBody.jointChildren)
1099 {
1100
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 176 times.
 183 if (joint.jointType == "free")
1101 {
1102
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 referenceConfig.conservativeResize(referenceConfig.size() + 7);
1103
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 referenceConfig.tail(7) << Eigen::Matrix<double, 7, 1>::Zero();
1104 }
1105
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 166 times.
 176 else if (joint.jointType == "ball")
1106 {
1107
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 referenceConfig.conservativeResize(referenceConfig.size() + 4);
1108
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 referenceConfig.tail(4) << Eigen::Vector4d::Zero();
1109 }
1110
4/6
✓ Branch 1 taken 154 times.
✓ Branch 2 taken 12 times.
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 166 times.
✗ Branch 7 not taken.
 166 else if (joint.jointType == "slide" || joint.jointType == "hinge")
1111 {
1112
1/2
✓ Branch 2 taken 166 times.
✗ Branch 3 not taken.
 166 referenceConfig.conservativeResize(referenceConfig.size() + 1);
1113
2/4
✓ Branch 1 taken 166 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 166 times.
✗ Branch 5 not taken.
 166 referenceConfig.tail(1) << joint.posRef;
1114 }
1115 }
1116 198 }
1117
1118 3 void MjcfGraph::addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName)
1119 {
1120 // Check config vectors and add them if size is right
1121 3 const int model_nq = urdfVisitor.model.nq;
1122
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 if (keyframe.size() == model_nq)
1123 {
1124
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Eigen::VectorXd qpos(model_nq);
1125
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3 times.
 9 for (std::size_t i = 1; i < urdfVisitor.model.joints.size(); i++)
1126 {
1127 6 const auto & joint = urdfVisitor.model.joints[i];
1128
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 int idx_q = joint.idx_q();
1129
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 int nq = joint.nq();
1130
1131
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Eigen::VectorXd qpos_j = keyframe.segment(idx_q, nq);
1132
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Eigen::VectorXd q_ref = referenceConfig.segment(idx_q, nq);
1133
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 5 times.
 6 if (joint.shortname() == "JointModelFreeFlyer")
1134 {
1135
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 Eigen::Vector4d new_quat(qpos_j(4), qpos_j(5), qpos_j(6), qpos_j(3));
1136
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 qpos_j.tail(4) = new_quat;
1137 }
1138
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 4 times.
 5 else if (joint.shortname() == "JointModelSpherical")
1139 {
1140
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 Eigen::Vector4d new_quat(qpos_j(1), qpos_j(2), qpos_j(3), qpos_j(0));
1141
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 qpos_j = new_quat;
1142 }
1143
3/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 3 times.
 4 else if (joint.shortname() == "JointModelComposite")
1144 {
1145
3/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 1 times.
 3 for (const auto & joint_ : boost::get<JointModelComposite>(joint.toVariant()).joints)
1146 {
1147
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 int idx_q_ = joint_.idx_q() - idx_q;
1148
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 int nq_ = joint_.nq();
1149
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 if (joint_.shortname() == "JointModelSpherical")
1150 {
1151 Eigen::Vector4d new_quat(
1152 ✗ qpos_j(idx_q_ + 1), qpos_j(idx_q_ + 2), qpos_j(idx_q_ + 3), qpos_j(idx_q_));
1153 ✗ qpos_j.segment(idx_q_, nq_) = new_quat;
1154 }
1155 else
1156
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 qpos_j.segment(idx_q_, nq_) -= q_ref.segment(idx_q_, nq_);
1157 }
1158 }
1159 else
1160
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 qpos_j -= q_ref;
1161
1162
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 qpos.segment(idx_q, nq) = qpos_j;
1163 6 }
1164
1165
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 urdfVisitor.model.referenceConfigurations.insert(std::make_pair(keyName, qpos));
1166 3 }
1167 else
1168 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Keyframe size does not match model size");
1169 3 }
1170
1171 16 void MjcfGraph::parseContactInformation(
1172 const Model & model,
1173 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models)
1174 {
1175
2/2
✓ Branch 5 taken 8 times.
✓ Branch 6 taken 16 times.
 24 for (const auto & entry : mapOfEqualities)
1176 {
1177 8 const MjcfEquality & eq = entry.second;
1178
1179
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 SE3 jointPlacement;
1180
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 jointPlacement.setIdentity();
1181
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 jointPlacement.translation() = eq.anchor;
1182
1183 // Get Joint Indices from the model
1184
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 const JointIndex body1 = urdfVisitor.getParentId(eq.body1);
1185
1186 // when body2 is not specified, we link to the world
1187
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (eq.body2 == "")
1188 {
1189 ✗ RigidConstraintModel rcm(CONTACT_3D, model, body1, jointPlacement, LOCAL);
1190 ✗ contact_models.push_back(rcm);
1191 }
1192 else
1193 {
1194
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 const JointIndex body2 = urdfVisitor.getParentId(eq.body2);
1195 RigidConstraintModel rcm(
1196
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 CONTACT_3D, model, body1, jointPlacement, body2, jointPlacement.inverse(), LOCAL);
1197
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 contact_models.push_back(rcm);
1198 8 }
1199 }
1200 16 }
1201
1202 27 void MjcfGraph::parseRootTree()
1203 {
1204
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 urdfVisitor.setName(modelName);
1205 // get name and inertia of first root link
1206
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 std::string rootLinkName = bodiesList.at(0);
1207
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
 27 MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second;
1208
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 3 times.
 27 if (rootBody.jointChildren.size() == 0)
1209
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 urdfVisitor.addRootJoint(rootBody.bodyInertia, rootLinkName);
1210
1211
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 fillReferenceConfig(rootBody);
1212
2/2
✓ Branch 5 taken 203 times.
✓ Branch 6 taken 27 times.
 230 for (const auto & entry : bodiesList)
1213 {
1214
1/2
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
 203 fillModel(entry);
1215 }
1216
1217
2/2
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 27 times.
 30 for (const auto & entry : mapOfConfigs)
1218 {
1219
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 addKeyFrame(entry.second, entry.first);
1220 }
1221 27 }
1222 } // namespace details
1223 } // namespace mjcf
1224 } // namespace pinocchio
1225