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 |