GCC Code Coverage Report

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