GCC Code Coverage Report

Directory: ./
File: include/multicontact-api/scenario/contact-phase.hpp
Date: 2025-04-05 01:06:26
Exec Total Coverage
Lines: 342 346 98.8%
Branches: 417 632 66.0%
Line Branch Exec Source
1 #ifndef __multicontact_api_scenario_contact_phase_hpp__
2 #define __multicontact_api_scenario_contact_phase_hpp__
3
4 #include <ndcurves/fwd.h>
5 #include <ndcurves/piecewise_curve.h>
6
7 #include <boost/serialization/map.hpp>
8 #include <boost/serialization/string.hpp>
9 #include <boost/serialization/vector.hpp>
10 #include <map>
11 #include <ndcurves/serialization/curves.hpp>
12 #include <set>
13 #include <sstream>
14 #include <string>
15 #include <vector>
16
17 #include "multicontact-api/geometry/curve-map.hpp"
18 #include "multicontact-api/scenario/contact-patch.hpp"
19 #include "multicontact-api/scenario/fwd.hpp"
20 #include "multicontact-api/serialization/archive.hpp"
21 #include "multicontact-api/serialization/eigen-matrix.hpp"
22 #include "multicontact-api/serialization/spatial.hpp"
23
24 namespace multicontact_api {
25 namespace scenario {
26
27 template <typename _Scalar>
28 struct ContactPhaseTpl
29 : public serialization::Serializable<ContactPhaseTpl<_Scalar> > {
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 typedef _Scalar Scalar;
33
34 // Eigen types
35 typedef ndcurves::pointX_t pointX_t;
36 typedef ndcurves::point3_t point3_t;
37 typedef ndcurves::point6_t point6_t;
38 typedef ndcurves::t_point3_t t_point3_t;
39 typedef ndcurves::t_pointX_t t_pointX_t;
40 typedef ndcurves::transform_t transform_t;
41
42 // Curves types
43 typedef ndcurves::curve_abc_t curve_t;
44 // typedef ndcurves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
45 typedef ndcurves::curve_SE3_t curve_SE3_t;
46 typedef ndcurves::curve_ptr_t curve_ptr;
47 // typedef std::shared_ptr<curve_3_t> curve_3_ptr;
48 typedef ndcurves::curve_SE3_ptr_t curve_SE3_ptr;
49 typedef ndcurves::piecewise3_t piecewise3_t;
50 typedef ndcurves::piecewise_t piecewise_t;
51 typedef piecewise_t::t_time_t t_time_t;
52
53 typedef std::vector<std::string> t_strings;
54 typedef ContactPatchTpl<Scalar> ContactPatch;
55 typedef typename ContactPatch::SE3 SE3;
56 typedef std::map<std::string, ContactPatch> ContactPatchMap;
57 typedef CurveMap<curve_ptr> CurveMap_t;
58 typedef CurveMap<curve_SE3_ptr> CurveSE3Map_t;
59
60 /// \brief Default constructor
61 145 ContactPhaseTpl()
62
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 : m_c_init(point3_t::Zero()),
63
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 m_dc_init(point3_t::Zero()),
64
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 m_ddc_init(point3_t::Zero()),
65
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 m_L_init(point3_t::Zero()),
66
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 m_dL_init(point3_t::Zero()),
67 145 m_q_init(),
68
2/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
 145 m_c_final(point3_t::Zero()),
69
2/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
 145 m_dc_final(point3_t::Zero()),
70
2/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
 145 m_ddc_final(point3_t::Zero()),
71
2/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
 145 m_L_final(point3_t::Zero()),
72
2/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 145 times.
✗ Branch 5 not taken.
 145 m_dL_final(point3_t::Zero()),
73
1/2
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
 145 m_q_final(),
74 145 m_q(),
75 145 m_dq(),
76 145 m_ddq(),
77 145 m_tau(),
78 145 m_c(),
79 145 m_dc(),
80 145 m_ddc(),
81 145 m_L(),
82 145 m_dL(),
83 145 m_wrench(),
84 145 m_zmp(),
85 145 m_root(),
86 145 m_contact_forces(),
87 145 m_contact_normal_force(),
88 145 m_effector_trajectories(),
89 145 m_effector_in_contact(),
90 145 m_contact_patches(),
91 145 m_t_init(-1),
92 145 m_t_final(-1) {}
93
94 /**
95 * @brief ContactPhaseTpl Constructor with time interval
96 * @param t_init the time at the beginning of this contact phase
97 * @param t_final the time at the end of this contact phase
98 * @throw invalid_argument if t_final < t_init
99 */
100 134 ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
101
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 : m_c_init(point3_t::Zero()),
102
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 m_dc_init(point3_t::Zero()),
103
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 m_ddc_init(point3_t::Zero()),
104
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 m_L_init(point3_t::Zero()),
105
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 m_dL_init(point3_t::Zero()),
106 134 m_q_init(),
107
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 m_c_final(point3_t::Zero()),
108
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 m_dc_final(point3_t::Zero()),
109
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 m_ddc_final(point3_t::Zero()),
110
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 m_L_final(point3_t::Zero()),
111
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 m_dL_final(point3_t::Zero()),
112
1/2
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
 134 m_q_final(),
113 134 m_q(),
114 134 m_dq(),
115 134 m_ddq(),
116 134 m_tau(),
117 134 m_c(),
118 134 m_dc(),
119 134 m_ddc(),
120 134 m_L(),
121 134 m_dL(),
122 134 m_wrench(),
123 134 m_zmp(),
124 134 m_root(),
125 134 m_contact_forces(),
126 134 m_contact_normal_force(),
127 134 m_effector_trajectories(),
128 134 m_effector_in_contact(),
129 134 m_contact_patches(),
130 134 m_t_init(t_init),
131 134 m_t_final(t_final) {
132
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 131 times.
 134 if (t_final < t_init)
133
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 throw std::invalid_argument("t_final cannot be inferior to t_initial");
134 188 }
135
136 /// \brief Copy constructor
137 template <typename S2>
138 ContactPhaseTpl(const ContactPhaseTpl<S2>& other)
139 : m_c_init(other.m_c_init),
140 m_dc_init(other.m_dc_init),
141 m_ddc_init(other.m_ddc_init),
142 m_L_init(other.m_L_init),
143 m_dL_init(other.m_dL_init),
144 m_q_init(other.m_q_init),
145 m_c_final(other.m_c_final),
146 m_dc_final(other.m_dc_final),
147 m_ddc_final(other.m_ddc_final),
148 m_L_final(other.m_L_final),
149 m_dL_final(other.m_dL_final),
150 m_q_final(other.m_q_final),
151 m_q(other.m_q),
152 m_dq(other.m_dq),
153 m_ddq(other.m_ddq),
154 m_tau(other.m_tau),
155 m_c(other.m_c),
156 m_dc(other.m_dc),
157 m_ddc(other.m_ddc),
158 m_L(other.m_L),
159 m_dL(other.m_dL),
160 m_wrench(other.m_wrench),
161 m_zmp(other.m_zmp),
162 m_root(other.m_root),
163 m_contact_forces(other.m_contact_forces),
164 m_contact_normal_force(other.m_contact_normal_force),
165 m_effector_trajectories(other.m_effector_trajectories),
166 m_effector_in_contact(other.m_effector_in_contact),
167 m_contact_patches(other.m_contact_patches),
168 m_t_init(other.m_t_init),
169 m_t_final(other.m_t_final) {}
170
171 template <typename S2>
172 326 bool operator==(const ContactPhaseTpl<S2>& other) const {
173
2/2
✓ Branch 2 taken 317 times.
✓ Branch 3 taken 2 times.
 645 return m_c_init == other.m_c_init && m_dc_init == other.m_dc_init &&
174
4/4
✓ Branch 1 taken 315 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 313 times.
✓ Branch 5 taken 2 times.
 317 m_ddc_init == other.m_ddc_init && m_L_init == other.m_L_init &&
175
4/4
✓ Branch 1 taken 311 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 310 times.
✓ Branch 4 taken 1 times.
 624 m_dL_init == other.m_dL_init &&
176
1/2
✓ Branch 2 taken 310 times.
✗ Branch 3 not taken.
 621 (m_q_init.rows() == other.m_q_init.rows() &&
177 310 m_q_init.cols() == other.m_q_init.cols() &&
178
2/2
✓ Branch 1 taken 309 times.
✓ Branch 2 taken 1 times.
 310 m_q_init == other.m_q_init) &&
179
4/4
✓ Branch 1 taken 307 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 305 times.
✓ Branch 5 taken 2 times.
 309 m_c_final == other.m_c_final && m_dc_final == other.m_dc_final &&
180
4/4
✓ Branch 1 taken 303 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 301 times.
✓ Branch 5 taken 2 times.
 305 m_ddc_final == other.m_ddc_final && m_L_final == other.m_L_final &&
181
4/4
✓ Branch 1 taken 299 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 298 times.
✓ Branch 4 taken 1 times.
 600 m_dL_final == other.m_dL_final &&
182
1/2
✓ Branch 2 taken 298 times.
✗ Branch 3 not taken.
 597 (m_q_final.rows() == other.m_q_final.rows() &&
183 298 m_q_final.cols() == other.m_q_final.cols() &&
184
4/4
✓ Branch 1 taken 297 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 79 times.
✓ Branch 4 taken 218 times.
 595 m_q_final == other.m_q_final) &&
185
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 376 (m_q == other.m_q ||
186
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 216 times.
 453 (m_q && other.m_q && m_q->isApprox(other.m_q.get()))) &&
187
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 374 (m_dq == other.m_dq ||
188
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 214 times.
 451 (m_dq && other.m_dq && m_dq->isApprox(other.m_dq.get()))) &&
189
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 372 (m_ddq == other.m_ddq ||
190
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 212 times.
 449 (m_ddq && other.m_ddq && m_ddq->isApprox(other.m_ddq.get()))) &&
191
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 370 (m_tau == other.m_tau ||
192
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 116 times.
✓ Branch 11 taken 173 times.
 447 (m_tau && other.m_tau && m_tau->isApprox(other.m_tau.get()))) &&
193
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 405 (m_c == other.m_c ||
194
6/6
✓ Branch 2 taken 115 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 114 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 208 times.
 519 (m_c && other.m_c && m_c->isApprox(other.m_c.get()))) &&
195
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 366 (m_dc == other.m_dc ||
196
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 206 times.
 443 (m_dc && other.m_dc && m_dc->isApprox(other.m_dc.get()))) &&
197
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 364 (m_ddc == other.m_ddc ||
198
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 204 times.
 441 (m_ddc && other.m_ddc && m_ddc->isApprox(other.m_ddc.get()))) &&
199
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 362 (m_L == other.m_L ||
200
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 202 times.
 439 (m_L && other.m_L && m_L->isApprox(other.m_L.get()))) &&
201
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 360 (m_dL == other.m_dL ||
202
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 200 times.
 437 (m_dL && other.m_dL && m_dL->isApprox(other.m_dL.get()))) &&
203
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 358 (m_wrench == other.m_wrench ||
204
2/2
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
 158 (m_wrench && other.m_wrench &&
205
4/4
✓ Branch 4 taken 77 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 79 times.
✓ Branch 7 taken 198 times.
 355 m_wrench->isApprox(other.m_wrench.get()))) &&
206
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 356 (m_zmp == other.m_zmp ||
207
6/6
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 79 times.
✓ Branch 11 taken 196 times.
 433 (m_zmp && other.m_zmp && m_zmp->isApprox(other.m_zmp.get()))) &&
208
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 354 (m_root == other.m_root ||
209
4/4
✓ Branch 2 taken 78 times.
✓ Branch 3 taken 1 times.
✓ Branch 8 taken 77 times.
✓ Branch 9 taken 1 times.
 158 (m_root && other.m_root && m_root->isApprox(other.m_root.get()))) &&
210
2/2
✓ Branch 1 taken 268 times.
✓ Branch 2 taken 5 times.
 273 m_contact_forces == other.m_contact_forces &&
211
2/2
✓ Branch 1 taken 264 times.
✓ Branch 2 taken 4 times.
 268 m_contact_normal_force == other.m_contact_normal_force &&
212
2/2
✓ Branch 1 taken 260 times.
✓ Branch 2 taken 4 times.
 264 m_effector_trajectories == other.m_effector_trajectories &&
213
2/2
✓ Branch 1 taken 255 times.
✓ Branch 2 taken 5 times.
 260 m_effector_in_contact == other.m_effector_in_contact &&
214
2/2
✓ Branch 1 taken 254 times.
✓ Branch 2 taken 1 times.
 255 m_contact_patches == other.m_contact_patches &&
215
6/6
✓ Branch 0 taken 319 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 253 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 244 times.
✓ Branch 5 taken 9 times.
 645 m_t_init == other.m_t_init && m_t_final == other.m_t_final;
216 }
217
218 template <typename S2>
219 75 bool operator!=(const ContactPhaseTpl<S2>& other) const {
220 75 return !(*this == other);
221 }
222
223 // public members :
224 /// \brief Initial position of the center of mass for this contact phase
225 point3_t m_c_init;
226 /// \brief Initial velocity of the center of mass for this contact phase
227 point3_t m_dc_init;
228 /// \brief Initial acceleration of the center of mass for this contact phase
229 point3_t m_ddc_init;
230 /// \brief Initial angular momentum for this contact phase
231 point3_t m_L_init;
232 /// \brief Initial angular momentum derivative for this contact phase
233 point3_t m_dL_init;
234 /// \brief Initial whole body configuration of this phase
235 pointX_t m_q_init;
236 /// \brief Final position of the center of mass for this contact phase
237 point3_t m_c_final;
238 /// \brief Final velocity of the center of mass for this contact phase
239 point3_t m_dc_final;
240 /// \brief Final acceleration of the center of mass for this contact phase
241 point3_t m_ddc_final;
242 /// \brief Final angular momentum for this contact phase
243 point3_t m_L_final;
244 /// \brief Final angular momentum derivative for this contact phase
245 point3_t m_dL_final;
246 /// \brief Final whole body configuration of this phase
247 pointX_t m_q_final;
248
249 /// \brief trajectory for the joint positions
250 curve_ptr m_q;
251 /// \brief trajectory for the joint velocities
252 curve_ptr m_dq;
253 /// \brief trajectory for the joint accelerations
254 curve_ptr m_ddq;
255 /// \brief trajectory for the joint torques
256 curve_ptr m_tau;
257 /// \brief trajectory for the center of mass position
258 curve_ptr m_c;
259 /// \brief trajectory for the center of mass velocity
260 curve_ptr m_dc;
261 /// \brief trajectory for the center of mass acceleration
262 curve_ptr m_ddc;
263 /// \brief trajectory for the angular momentum
264 curve_ptr m_L;
265 /// \brief trajectory for the angular momentum derivative
266 curve_ptr m_dL;
267 /// \brief trajectory for the centroidal wrench
268 curve_ptr m_wrench;
269 /// \brief trajectory for the zmp
270 curve_ptr m_zmp;
271 /// \brief SE3 trajectory of the root of the robot
272 curve_SE3_ptr m_root;
273
274 // getter and setter for the timings
275 241 Scalar timeInitial() const { return m_t_init; }
276 22 void timeInitial(const Scalar t) { m_t_init = t; }
277 230 Scalar timeFinal() const { return m_t_final; }
278 7 void timeFinal(const Scalar t) {
279
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 5 times.
 7 if (t < m_t_init)
280
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument("t_final cannot be inferior to t_initial");
281 5 m_t_final = t;
282 5 }
283 15 Scalar duration() const { return m_t_final - m_t_init; }
284 27 void duration(const Scalar d) {
285
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 25 times.
 27 if (d <= 0)
286
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument("Duration of the phase cannot be negative.");
287 25 m_t_final = m_t_init + d;
288 25 }
289
290 // getter for the map trajectories
291 41 CurveMap_t contactForces() const { return m_contact_forces; }
292 28 CurveMap_t contactNormalForces() const { return m_contact_normal_force; }
293 44 CurveSE3Map_t effectorTrajectories() const { return m_effector_trajectories; }
294 47 curve_ptr contactForces(const std::string& eeName) {
295
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 46 times.
 47 if (m_contact_forces.count(eeName) == 0) {
296
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 throw std::invalid_argument(
297 "This contact phase do not contain any contact force trajectory for "
298 "the effector " +
299 eeName);
300 } else {
301 46 return m_contact_forces.at(eeName);
302 }
303 }
304 44 curve_ptr contactNormalForces(const std::string& eeName) {
305
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 43 times.
 44 if (m_contact_normal_force.count(eeName) == 0) {
306
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 throw std::invalid_argument(
307 "This contact phase do not contain any normal contact force "
308 "trajectory for the effector " +
309 eeName);
310 } else {
311 43 return m_contact_normal_force.at(eeName);
312 }
313 }
314 49 curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
315
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 48 times.
 49 if (m_effector_trajectories.count(eeName) == 0) {
316
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 throw std::invalid_argument(
317 "This contact phase do not contain any effector trajectory for the "
318 "effector " +
319 eeName);
320 } else {
321 48 return m_effector_trajectories.at(eeName);
322 }
323 }
324 /**
325 * @brief addContactForceTrajectory add a trajectory to the map of contact
326 * forces. If a trajectory already exist for this effector, it is overwritted.
327 * @param eeName the name of the effector (key of the map)
328 * @param trajectory the trajectory to add
329 * @throw invalid_argument if eeName is not defined in contact for this phase
330 * @return false if a trajectory already existed (and have been overwrited)
331 * true otherwise
332 */
333 213 bool addContactForceTrajectory(const std::string& eeName,
334 const curve_ptr trajectory) {
335
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 211 times.
 213 if (!isEffectorInContact(eeName))
336
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 throw std::invalid_argument(
337 "Cannot add a contact force trajectory for effector " + eeName +
338 " as it is not in contact for the current phase.");
339 211 bool alreadyExist(m_contact_forces.count(eeName));
340
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 206 times.
 211 if (alreadyExist) m_contact_forces.erase(eeName);
341
1/2
✓ Branch 1 taken 211 times.
✗ Branch 2 not taken.
 211 m_contact_forces.insert(
342 422 std::pair<std::string, curve_ptr>(eeName, trajectory));
343 211 return !alreadyExist;
344 }
345 /**
346 * @brief addContactNormalForceTrajectory add a trajectory to the map of
347 * contact normal forces. If a trajectory already exist for this effector, it
348 * is overwritted.
349 * @param eeName the name of the effector (key of the map)
350 * @param trajectory the trajectory to add
351 * @throw invalid_argument if eeName is not defined in contact for this phase
352 * @throw invalid_argument if trajectory is not of dimension 1
353 * @return false if a trajectory already existed (and have been overwrited)
354 * true otherwise
355 */
356 212 bool addContactNormalForceTrajectory(const std::string& eeName,
357 const curve_ptr trajectory) {
358
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 210 times.
 212 if (!isEffectorInContact(eeName))
359
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 throw std::invalid_argument(
360 "Cannot add a contact normal trajectory for effector " + eeName +
361 " as it is not in contact for the current phase.");
362
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 208 times.
 210 if (trajectory->dim() != 1)
363
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
364 "Contact normal force trajectory must be of dimension 1");
365 208 bool alreadyExist(m_contact_normal_force.count(eeName));
366
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 204 times.
 208 if (alreadyExist) m_contact_normal_force.erase(eeName);
367
1/2
✓ Branch 1 taken 208 times.
✗ Branch 2 not taken.
 208 m_contact_normal_force.insert(
368 416 std::pair<std::string, curve_ptr>(eeName, trajectory));
369 208 return !alreadyExist;
370 }
371 /**
372 * @brief adEffectorTrajectory add a trajectory to the map of contact forces.
373 * If a trajectory already exist for this effector, it is overwritted.
374 * @param eeName the name of the effector (key of the map)
375 * @param trajectory the trajectory to add
376 * @throw invalid_argument if eeName is defined in contact for this phase
377 * @return false if a trajectory already existed (and have been overwrited)
378 * true otherwise
379 */
380 212 bool addEffectorTrajectory(const std::string& eeName,
381 const curve_SE3_ptr trajectory) {
382
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 210 times.
 212 if (isEffectorInContact(eeName))
383
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 throw std::invalid_argument(
384 "Cannot add an effector trajectory for effector " + eeName +
385 " as it is in contact for the current phase.");
386 210 bool alreadyExist(m_effector_trajectories.count(eeName));
387
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 207 times.
 210 if (alreadyExist) m_effector_trajectories.erase(eeName);
388
1/2
✓ Branch 1 taken 210 times.
✗ Branch 2 not taken.
 210 m_effector_trajectories.insert(
389 420 std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
390 210 return !alreadyExist;
391 }
392
393 88 ContactPatchMap contactPatches() const { return m_contact_patches; }
394 114 ContactPatch& contactPatch(const std::string& eeName) {
395
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 112 times.
 114 if (m_contact_patches.count(eeName) == 0) {
396
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 throw std::invalid_argument(
397 "This contact phase do not contain any contact patch for the "
398 "effector " +
399 eeName);
400 } else {
401 112 return m_contact_patches.at(eeName);
402 }
403 }
404 /**
405 * @brief addContact add a new contact patch to this contact phase
406 * If a contact phase already exist for this effector, it is overwritted.
407 * If an end effector trajectory exist for this contact, it is removed.
408 * @param eeName the name of the effector in contact
409 * @param patch the contact patch
410 * @return false if a contact for this effector already existed (and have been
411 * overwrited) true otherwise
412 */
413 262 bool addContact(const std::string& eeName, const ContactPatch& patch) {
414 262 bool alreadyExist(isEffectorInContact(eeName));
415
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 260 times.
 262 if (m_contact_patches.count(eeName))
416 2 m_contact_patches.erase(eeName);
417 else
418 260 m_effector_in_contact.push_back(eeName);
419
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
 262 m_contact_patches.insert(
420 524 std::pair<std::string, ContactPatch>(eeName, patch));
421 262 m_effector_trajectories.erase(eeName);
422 262 return !alreadyExist;
423 }
424
425 /**
426 * @brief removeContact remove the given contact
427 * This will also remove the contact_patch all the contact_forces and
428 * contact_normal_forces related to this contact
429 * @param eeName the name of the effector to remove
430 * @return true if the effector was in contact, false otherwise
431 */
432 13 bool removeContact(const std::string& eeName) {
433 13 bool existed(isEffectorInContact(eeName));
434
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 3 times.
 13 if (existed)
435
1/2
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 m_effector_in_contact.erase(std::find(
436 m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
437 13 m_contact_patches.erase(eeName);
438 13 m_contact_forces.erase(eeName);
439 13 m_contact_normal_force.erase(eeName);
440 13 return existed;
441 }
442
443 10 std::size_t numContacts() const { return m_effector_in_contact.size(); }
444
445 86 t_strings effectorsInContact() const { return m_effector_in_contact; }
446
447 1036 bool isEffectorInContact(const std::string& eeName) const {
448
2/2
✓ Branch 1 taken 155 times.
✓ Branch 2 taken 881 times.
 1036 if (m_effector_in_contact.empty())
449 155 return false;
450 else
451
1/2
✓ Branch 3 taken 881 times.
✗ Branch 4 not taken.
 881 return (std::find(m_effector_in_contact.begin(),
452 m_effector_in_contact.end(),
453 1762 eeName) != m_effector_in_contact.end());
454 }
455
456 /**
457 * @brief effectorsWithTrajectory return a set of all effectors for which an
458 * effector trajectory have been defined
459 * @return a set of all effectors for which an effector trajectory have been
460 * defined
461 */
462 4 t_strings effectorsWithTrajectory() const {
463 4 t_strings effectors;
464 4 for (typename CurveSE3Map_t::const_iterator mit =
465 4 m_effector_trajectories.begin();
466
2/2
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 4 times.
 11 mit != m_effector_trajectories.end(); ++mit) {
467
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 effectors.push_back(mit->first);
468 }
469 4 return effectors;
470 }
471
472 /**
473 * @brief effectorHaveAtrajectory check if an end effector trajectory have
474 * been defined for a given effector
475 * @param eeName the effector name
476 * @return true if there is a trajectory defined, false otherwise
477 */
478 50 bool effectorHaveAtrajectory(const std::string& eeName) const {
479 50 return m_effector_trajectories.count(eeName);
480 }
481
482 /* Helpers */
483
484 /**
485 * @brief isConsistent check if all the members of the phase are consistent
486 * together:
487 * - There is a contact patch defined for all effector in contact
488 * - There is only contact forces for the effector defined in contact
489 * - If a trajectory is defined, it is defined between t_begin and t_final
490 * - If init/end values are defined and a trajectory for this values is also
491 * defined, check that they match
492 * - The times are positives and tbegin <= tmax
493 * @param throw_if_inconsistent if true, throw an runtime_error if not
494 * consistent
495 * @return true if consistent, false otherwise
496 */
497 ✗ bool isConsistent(const bool throw_if_inconsistent = false) const {
498 ✗ std::cout << "WARNING : not implemented yet, return True" << std::endl;
499 (void)throw_if_inconsistent;
500 ✗ return true;
501 }
502
503 /**
504 * @brief setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list
505 * of discrete COM positions, velocity and accelerations. The trajectories are
506 * build with first order polynomials connecting each discrete points given.
507 * this method also set the initial/final values for c, dc and ddc from the
508 * first and last discrete point given.
509 * @param points list of discrete CoM positions
510 * @param points_derivative list of discrete CoM velocities
511 * @param points_second_derivative list of discrete CoM accelerations
512 * @param time_points list of times corresponding to each discrete point
513 * given.
514 */
515 10 void setCOMtrajectoryFromPoints(const t_pointX_t& points,
516 const t_pointX_t& points_derivative,
517 const t_pointX_t& points_second_derivative,
518 const t_time_t& time_points) {
519 /*
520 piecewise_t c_t =
521 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
522 points, points_derivative, points_second_derivative, time_points);
523 if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points
524 must be 3."); m_c = curve_ptr(new piecewise_t(c_t)); m_dc =
525 curve_ptr(c_t.compute_derivate_ptr(1)); m_ddc =
526 curve_ptr(c_t.compute_derivate_ptr(2));
527 */
528
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 24 m_c = curve_ptr(
529
3/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✓ Branch 5 taken 2 times.
 10 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
530 ndcurves::polynomial_t>(points, time_points)));
531
5/8
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 11 m_dc = curve_ptr(new piecewise_t(
532 piecewise_t::convert_discrete_points_to_polynomial<
533 ndcurves::polynomial_t>(points_derivative, time_points)));
534
5/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
 10 m_ddc = curve_ptr(new piecewise_t(
535 piecewise_t::convert_discrete_points_to_polynomial<
536 ndcurves::polynomial_t>(points_second_derivative, time_points)));
537
8/8
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 2 times.
✓ Branch 6 taken 3 times.
✓ Branch 7 taken 1 times.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 2 times.
✓ Branch 12 taken 4 times.
✓ Branch 13 taken 2 times.
 6 if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
538
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 throw std::invalid_argument("Dimension of the points must be 3.");
539
540 2 m_c_init = point3_t(points.front());
541 2 m_c_final = point3_t(points.back());
542 2 m_dc_init = point3_t(points_derivative.front());
543 2 m_dc_final = point3_t(points_derivative.back());
544 2 m_ddc_init = point3_t(points_second_derivative.front());
545 2 m_ddc_final = point3_t(points_second_derivative.back());
546 2 return;
547 }
548
549 /**
550 * @brief setAMtrajectoryFromPoints set the L and d_L curves from a list of
551 * discrete Angular velocity values and their derivatives The trajectories are
552 * build with first order polynomials connecting each discrete points given.
553 * This method also set the initial/final values for L, and dL from the first
554 * and last discrete point given.
555 * @param points list of discrete Angular Momentum values
556 * @param points_derivative list of discrete Angular momentum derivative
557 * @param time_points list of times corresponding to each discrete point
558 * given.
559 */
560 8 void setAMtrajectoryFromPoints(const t_pointX_t& points,
561 const t_pointX_t& points_derivative,
562 const t_time_t& time_points) {
563 /*
564 piecewise_t L_t =
565 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
566 points, points_derivative, time_points);
567 if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points
568 must be 3."); m_L = curve_ptr(new piecewise_t(L_t)); m_dL =
569 curve_ptr(L_t.compute_derivate_ptr(1));
570 */
571
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 20 m_L = curve_ptr(
572
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 2 times.
 8 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
573 ndcurves::polynomial_t>(points, time_points)));
574
5/8
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
 9 m_dL = curve_ptr(new piecewise_t(
575 piecewise_t::convert_discrete_points_to_polynomial<
576 ndcurves::polynomial_t>(points_derivative, time_points)));
577
6/6
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 2 times.
 5 if (m_L->dim() != 3 || m_dL->dim() != 3)
578
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 throw std::invalid_argument("Dimension of the points must be 3.");
579
580 2 m_L_init = point3_t(points.front());
581 2 m_L_final = point3_t(points.back());
582 2 m_dL_init = point3_t(points_derivative.front());
583 2 m_dL_final = point3_t(points_derivative.back());
584 2 return;
585 }
586
587 /**
588 * @brief setJointsTrajectoryFromPoints set the q,dq and ddq curves from a
589 * list of discrete joints positions, velocity and accelerations. The
590 * trajectories are build with first order polynomials connecting each
591 * discrete points given. This method also set initial/final values for q from
592 * the first and last discrete point given.
593 * @param points list of discrete joints positions
594 * @param points_derivative list of discrete joints velocities
595 * @param points_second_derivative list of discrete joints accelerations
596 * @param time_points list of times corresponding to each discrete point
597 * given.
598 */
599 6 void setJointsTrajectoryFromPoints(const t_pointX_t& points,
600 const t_pointX_t& points_derivative,
601 const t_pointX_t& points_second_derivative,
602 const t_time_t& time_points) {
603 /*
604 piecewise_t q_t =
605 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
606 points, points_derivative, points_second_derivative, time_points);
607 m_q = curve_ptr(new piecewise_t(q_t));
608 m_dq = curve_ptr(q_t.compute_derivate_ptr(1));
609 m_ddq = curve_ptr(q_t.compute_derivate_ptr(2));
610 */
611
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 16 m_q = curve_ptr(
612
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 2 times.
 6 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
613 ndcurves::polynomial_t>(points, time_points)));
614
5/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 7 m_dq = curve_ptr(new piecewise_t(
615 piecewise_t::convert_discrete_points_to_polynomial<
616 ndcurves::polynomial_t>(points_derivative, time_points)));
617
5/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 5 m_ddq = curve_ptr(new piecewise_t(
618 piecewise_t::convert_discrete_points_to_polynomial<
619 ndcurves::polynomial_t>(points_second_derivative, time_points)));
620 2 m_q_init = points.front();
621 2 m_q_final = points.back();
622 2 return;
623 }
624
625 /**
626 * @brief getContactsBroken return the list of effectors in contact in '*this'
627 * but not in contact in 'to'
628 * @param to the other ContactPhase
629 * @return a list of string containing the effectors names
630 */
631 10 t_strings getContactsBroken(const ContactPhase& to) const {
632 10 t_strings res;
633
3/4
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 16 times.
✓ Branch 10 taken 10 times.
 26 for (std::string eeName : m_effector_in_contact) {
634
4/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 12 times.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 16 if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
635 }
636 10 return res;
637 }
638
639 /**
640 * @brief getContactsCreated return the list of effectors in contact in 'to'
641 * but not in contact in '*this'
642 * @param to the other ContactPhase
643 * @return a list of string containing the effectors names
644 */
645 24 t_strings getContactsCreated(const ContactPhase& to) const {
646 24 t_strings res;
647
4/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 23 times.
✓ Branch 13 taken 24 times.
 47 for (std::string eeName : to.effectorsInContact()) {
648
4/6
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✓ Branch 4 taken 12 times.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 23 if (!isEffectorInContact(eeName)) res.push_back(eeName);
649 }
650 24 return res;
651 }
652
653 /**
654 * @brief getContactsRepositioned return the list of effectors in contact both
655 * in 'to' and '*this' but not at the same placement
656 * @param to the other ContactPhase
657 * @return a list of string containing the effectors names
658 */
659 10 t_strings getContactsRepositioned(const ContactPhase& to) const {
660 10 t_strings res;
661
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 const ContactPatchMap& to_patch_map = to.contactPatches();
662
3/4
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 18 times.
✓ Branch 10 taken 10 times.
 28 for (std::string eeName : m_effector_in_contact) {
663
3/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 2 times.
 18 if (to.isEffectorInContact(eeName))
664
4/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 12 times.
✓ Branch 8 taken 4 times.
 16 if (m_contact_patches.at(eeName).placement() !=
665
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 to_patch_map.at(eeName).placement())
666
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 res.push_back(eeName);
667 }
668 20 return res;
669 10 }
670
671 /**
672 * @brief getContactsVariations return the list of all the effectors whose
673 * contacts differ between *this and to
674 * @param to the other ContactPhase
675 * @return a list of string containing the effectors names
676 */
677 6 t_strings getContactsVariations(const ContactPhase& to) const {
678 std::set<std::string>
679 6 set_res; // use intermediate set to guarantee uniqueness of element
680
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 2 times.
✓ Branch 13 taken 6 times.
 8 for (std::string eeName : getContactsBroken(to)) {
681
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 set_res.insert(eeName);
682 }
683
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 2 times.
✓ Branch 13 taken 6 times.
 8 for (std::string eeName : getContactsCreated(to)) {
684
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 set_res.insert(eeName);
685 }
686
4/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 4 times.
✓ Branch 13 taken 6 times.
 10 for (std::string eeName : getContactsRepositioned(to)) {
687
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 set_res.insert(eeName);
688 }
689
1/2
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 return t_strings(set_res.begin(), set_res.end());
690 6 }
691
692 /* End Helpers */
693
694 2 void disp(std::ostream& os) const {
695
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::Matrix<Scalar, 3, 5> state0(Eigen::Matrix<Scalar, 3, 5>::Zero());
696
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::Matrix<Scalar, 3, 5> state1(Eigen::Matrix<Scalar, 3, 5>::Zero());
697
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state0.block(0, 0, 3, 1) = m_c_init;
698
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state0.block(0, 1, 3, 1) = m_dc_init;
699
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state0.block(0, 2, 3, 1) = m_ddc_init;
700
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state0.block(0, 3, 3, 1) = m_L_init;
701
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state0.block(0, 4, 3, 1) = m_dL_init;
702
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state1.block(0, 0, 3, 1) = m_c_final;
703
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state1.block(0, 1, 3, 1) = m_dc_final;
704
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state1.block(0, 2, 3, 1) = m_ddc_final;
705
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state1.block(0, 3, 3, 1) = m_L_final;
706
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 state1.block(0, 4, 3, 1) = m_dL_final;
707
708
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 os << "Contact phase defined for t \\in [" << m_t_init << ";" << m_t_final
709
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << "]" << std::endl
710
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << "Conecting (c0,dc0,ddc0,L0,dL0) = " << std::endl
711
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << state0 << std::endl
712
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << "to (c0,dc0,ddc0,L0,dL0) = " << std::endl
713
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << state1 << std::endl;
714
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 os << "Effectors in contact " << m_effector_in_contact.size() << " : "
715
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 << std::endl;
716 2 for (t_strings::const_iterator ee = m_effector_in_contact.begin();
717
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 ee != m_effector_in_contact.end(); ++ee) {
718
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "______________________________________________" << std::endl
719
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
 4 << "Effector " << *ee << " contact patch:" << std::endl
720
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 << m_contact_patches.at(*ee) << std::endl
721
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << "Has contact force trajectory : "
722
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 << bool(m_contact_forces.count(*ee)) << std::endl
723
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << "Has contact normal force trajectory : "
724
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 << bool(m_contact_normal_force.count(*ee)) << std::endl;
725 }
726 2 }
727
728 template <typename S2>
729 2 friend std::ostream& operator<<(std::ostream& os,
730 const ContactPhaseTpl<S2>& cp) {
731 2 cp.disp(os);
732 2 return os;
733 }
734
735 protected:
736 // private members
737 /// \brief map with keys : effector name containing the contact forces
738 CurveMap_t m_contact_forces;
739 /// \brief map with keys : effector name containing the contact normal force
740 CurveMap_t m_contact_normal_force;
741 /// \brief map with keys : effector name containing the end effector
742 /// trajectory
743 CurveSE3Map_t m_effector_trajectories;
744 /// \brief set of the name of all effector in contact for this phase
745 t_strings m_effector_in_contact;
746 /// \brief map effector name : contact patches. All the patches are actives
747 ContactPatchMap m_contact_patches;
748 /// \brief time at the beginning of the contact phase
749 Scalar m_t_init;
750 /// \brief time at the end of the contact phase
751 Scalar m_t_final;
752
753 private:
754 // Serialization of the class
755 friend class boost::serialization::access;
756
757 template <class Archive>
758 324 void serialize(Archive& ar, const unsigned int version) {
759 // ar& boost::serialization::make_nvp("placement", m_placement);
760 unsigned int curve_version; // Curves API version related to the archive
761 // multicontact-api API version
762
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 162 times.
 324 if (version < 2) {
763 ✗ curve_version = 0;
764 } else {
765 324 curve_version = 1;
766 }
767 324 ndcurves::serialization::register_types<Archive>(ar, curve_version);
768
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("c_init", m_c_init);
769
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dc_init", m_dc_init);
770
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("ddc_init", m_ddc_init);
771
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("L_init", m_L_init);
772
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dL_init", m_dL_init);
773
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("q_init", m_q_init);
774
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("c_final", m_c_final);
775
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dc_final", m_dc_final);
776
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("ddc_final", m_ddc_final);
777
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("L_final", m_L_final);
778
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dL_final", m_dL_final);
779
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("q_final", m_q_final);
780
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("q", m_q);
781
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dq", m_dq);
782
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("ddq", m_ddq);
783
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("tau", m_tau);
784
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("c", m_c);
785
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dc", m_dc);
786
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("ddc", m_ddc);
787
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("L", m_L);
788
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("dL", m_dL);
789
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("wrench", m_wrench);
790
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("zmp", m_zmp);
791
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("root", m_root);
792
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("contact_forces", m_contact_forces);
793
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 ar& boost::serialization::make_nvp("contact_normal_force",
794 324 m_contact_normal_force);
795
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 ar& boost::serialization::make_nvp("effector_trajectories",
796 324 m_effector_trajectories);
797
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 ar& boost::serialization::make_nvp("effector_in_contact",
798 324 m_effector_in_contact);
799
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("contact_patches", m_contact_patches);
800
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("t_init", m_t_init);
801
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 ar& boost::serialization::make_nvp("t_final", m_t_final);
802 324 }
803
804 }; // struct contact phase
805
806 } // namespace scenario
807 } // namespace multicontact_api
808
809 MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(
810 typename Scalar, multicontact_api::scenario::ContactPhaseTpl<Scalar>)
811
812 #endif // CONTACTPHASE_HPP
813