contact-phase.hpp 30.6 KB
Newer Older
1
2
3
4
5
#ifndef __multicontact_api_scenario_contact_phase_hpp__
#define __multicontact_api_scenario_contact_phase_hpp__

#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
6
#include "multicontact-api/geometry/curve-map.hpp"
7
8
9
10
11

#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/spatial.hpp"

12
13
#include <curves/fwd.h>
#include <curves/piecewise_curve.h>
14
#include <curves/serialization/curves.hpp>
15
#include <map>
16
#include <vector>
17
#include <set>
18
19
#include <string>
#include <sstream>
20
#include <boost/serialization/string.hpp>
21
#include <boost/serialization/map.hpp>
22
#include <boost/serialization/vector.hpp>
23

Pierre Fernbach's avatar
Pierre Fernbach committed
24
25
namespace multicontact_api {
namespace scenario {
26

Pierre Fernbach's avatar
Pierre Fernbach committed
27
28
template <typename _Scalar>
struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Scalar> > {
29
30
31
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
32

33
  // Eigen types
34
35
36
37
38
39
  typedef curves::pointX_t pointX_t;
  typedef curves::point3_t point3_t;
  typedef curves::point6_t point6_t;
  typedef curves::t_point3_t t_point3_t;
  typedef curves::t_pointX_t t_pointX_t;
  typedef curves::transform_t transform_t;
40
41

  // Curves types
42
  typedef curves::curve_abc_t curve_t;
Pierre Fernbach's avatar
Pierre Fernbach committed
43
  // typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
44
  typedef curves::curve_SE3_t curve_SE3_t;
45
  typedef curves::curve_ptr_t curve_ptr;
Pierre Fernbach's avatar
Pierre Fernbach committed
46
  // typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
47
  typedef curves::curve_SE3_ptr_t curve_SE3_ptr;
48
49
50
51
  typedef curves::piecewise3_t piecewise3_t;
  typedef curves::piecewise_t piecewise_t;
  typedef piecewise_t::t_time_t t_time_t;

52
  typedef std::vector<std::string> t_strings;
53
54
  typedef ContactPatchTpl<Scalar> ContactPatch;
  typedef typename ContactPatch::SE3 SE3;
Pierre Fernbach's avatar
Pierre Fernbach committed
55
  typedef std::map<std::string, ContactPatch> ContactPatchMap;
56
57
  typedef CurveMap<curve_ptr> CurveMap_t;
  typedef CurveMap<curve_SE3_ptr> CurveSE3Map_t;
58
59
60

  /// \brief Default constructor
  ContactPhaseTpl()
Pierre Fernbach's avatar
Pierre Fernbach committed
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
      : m_c_init(point3_t::Zero()),
        m_dc_init(point3_t::Zero()),
        m_ddc_init(point3_t::Zero()),
        m_L_init(point3_t::Zero()),
        m_dL_init(point3_t::Zero()),
        m_q_init(),
        m_c_final(point3_t::Zero()),
        m_dc_final(point3_t::Zero()),
        m_ddc_final(point3_t::Zero()),
        m_L_final(point3_t::Zero()),
        m_dL_final(point3_t::Zero()),
        m_q_final(),
        m_q(),
        m_dq(),
        m_ddq(),
        m_tau(),
        m_c(),
        m_dc(),
        m_ddc(),
        m_L(),
        m_dL(),
        m_wrench(),
        m_zmp(),
        m_root(),
        m_contact_forces(),
        m_contact_normal_force(),
        m_effector_trajectories(),
        m_effector_in_contact(),
        m_contact_patches(),
        m_t_init(-1),
        m_t_final(-1) {}
92
93
94

  /**
   * @brief ContactPhaseTpl Constructor with time interval
Pierre Fernbach's avatar
Pierre Fernbach committed
95
   * @param t_init the time at the beginning of this contact phase
96
   * @param t_final the time at the end of this contact phase
Pierre Fernbach's avatar
Pierre Fernbach committed
97
   * @throw invalid_argument if t_final < t_init
98
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
99
  ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
Pierre Fernbach's avatar
Pierre Fernbach committed
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
      : m_c_init(point3_t::Zero()),
        m_dc_init(point3_t::Zero()),
        m_ddc_init(point3_t::Zero()),
        m_L_init(point3_t::Zero()),
        m_dL_init(point3_t::Zero()),
        m_q_init(),
        m_c_final(point3_t::Zero()),
        m_dc_final(point3_t::Zero()),
        m_ddc_final(point3_t::Zero()),
        m_L_final(point3_t::Zero()),
        m_dL_final(point3_t::Zero()),
        m_q_final(),
        m_q(),
        m_dq(),
        m_ddq(),
        m_tau(),
        m_c(),
        m_dc(),
        m_ddc(),
        m_L(),
        m_dL(),
        m_wrench(),
        m_zmp(),
        m_root(),
        m_contact_forces(),
        m_contact_normal_force(),
        m_effector_trajectories(),
        m_effector_in_contact(),
        m_contact_patches(),
        m_t_init(t_init),
        m_t_final(t_final) {
    if (t_final < t_init) throw std::invalid_argument("t_final cannot be inferior to t_initial");
Pierre Fernbach's avatar
Pierre Fernbach committed
132
  }
133
134

  /// \brief Copy constructor
Pierre Fernbach's avatar
Pierre Fernbach committed
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
  template <typename S2>
  ContactPhaseTpl(const ContactPhaseTpl<S2>& other)
      : m_c_init(other.m_c_init),
        m_dc_init(other.m_dc_init),
        m_ddc_init(other.m_ddc_init),
        m_L_init(other.m_L_init),
        m_dL_init(other.m_dL_init),
        m_q_init(other.m_q_init),
        m_c_final(other.m_c_final),
        m_dc_final(other.m_dc_final),
        m_ddc_final(other.m_ddc_final),
        m_L_final(other.m_L_final),
        m_dL_final(other.m_dL_final),
        m_q_final(other.m_q_final),
        m_q(other.m_q),
        m_dq(other.m_dq),
        m_ddq(other.m_ddq),
        m_tau(other.m_tau),
        m_c(other.m_c),
        m_dc(other.m_dc),
        m_ddc(other.m_ddc),
        m_L(other.m_L),
        m_dL(other.m_dL),
        m_wrench(other.m_wrench),
        m_zmp(other.m_zmp),
        m_root(other.m_root),
        m_contact_forces(other.m_contact_forces),
        m_contact_normal_force(other.m_contact_normal_force),
        m_effector_trajectories(other.m_effector_trajectories),
        m_effector_in_contact(other.m_effector_in_contact),
        m_contact_patches(other.m_contact_patches),
        m_t_init(other.m_t_init),
        m_t_final(other.m_t_final) {}
168
169
170

  template <typename S2>
  bool operator==(const ContactPhaseTpl<S2>& other) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
    return m_c_init == other.m_c_init && m_dc_init == other.m_dc_init && m_ddc_init == other.m_ddc_init &&
           m_L_init == other.m_L_init && m_dL_init == other.m_dL_init &&
           (m_q_init.rows() == other.m_q_init.rows() && m_q_init.cols() == other.m_q_init.cols() &&
            m_q_init == other.m_q_init) &&
           m_c_final == other.m_c_final && m_dc_final == other.m_dc_final && m_ddc_final == other.m_ddc_final &&
           m_L_final == other.m_L_final && m_dL_final == other.m_dL_final &&
           (m_q_final.rows() == other.m_q_final.rows() && m_q_final.cols() == other.m_q_final.cols() &&
            m_q_final == other.m_q_final) &&
           (m_q == other.m_q || (m_q && other.m_q && m_q->isApprox(other.m_q.get()))) &&
           (m_dq == other.m_dq || (m_dq && other.m_dq && m_dq->isApprox(other.m_dq.get()))) &&
           (m_ddq == other.m_ddq || (m_ddq && other.m_ddq && m_ddq->isApprox(other.m_ddq.get()))) &&
           (m_tau == other.m_tau || (m_tau && other.m_tau && m_tau->isApprox(other.m_tau.get()))) &&
           (m_c == other.m_c || (m_c && other.m_c && m_c->isApprox(other.m_c.get()))) &&
           (m_dc == other.m_dc || (m_dc && other.m_dc && m_dc->isApprox(other.m_dc.get()))) &&
           (m_ddc == other.m_ddc || (m_ddc && other.m_ddc && m_ddc->isApprox(other.m_ddc.get()))) &&
           (m_L == other.m_L || (m_L && other.m_L && m_L->isApprox(other.m_L.get()))) &&
           (m_dL == other.m_dL || (m_dL && other.m_dL && m_dL->isApprox(other.m_dL.get()))) &&
           (m_wrench == other.m_wrench || (m_wrench && other.m_wrench && m_wrench->isApprox(other.m_wrench.get()))) &&
           (m_zmp == other.m_zmp || (m_zmp && other.m_zmp && m_zmp->isApprox(other.m_zmp.get()))) &&
           (m_root == other.m_root || (m_root && other.m_root && m_root->isApprox(other.m_root.get()))) &&
           m_contact_forces == other.m_contact_forces && m_contact_normal_force == other.m_contact_normal_force &&
           m_effector_trajectories == other.m_effector_trajectories &&
           m_effector_in_contact == other.m_effector_in_contact && m_contact_patches == other.m_contact_patches &&
           m_t_init == other.m_t_init && m_t_final == other.m_t_final;
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
  }

  template <typename S2>
  bool operator!=(const ContactPhaseTpl<S2>& other) const {
    return !(*this == other);
  }

  // public members :
  /// \brief Initial position of the center of mass for this contact phase
  point3_t m_c_init;
  /// \brief Initial velocity of the center of mass for this contact phase
  point3_t m_dc_init;
  /// \brief Initial acceleration of the center of mass for this contact phase
  point3_t m_ddc_init;
  /// \brief Initial angular momentum for this contact phase
  point3_t m_L_init;
  /// \brief Initial angular momentum derivative for this contact phase
  point3_t m_dL_init;
  /// \brief Initial whole body configuration of this phase
  pointX_t m_q_init;
  /// \brief Final position of the center of mass for this contact phase
216
  point3_t m_c_final;
217
  /// \brief Final velocity of the center of mass for this contact phase
218
  point3_t m_dc_final;
219
  /// \brief Final acceleration of the center of mass for this contact phase
220
  point3_t m_ddc_final;
221
  /// \brief Final angular momentum for this contact phase
222
  point3_t m_L_final;
223
  /// \brief Final angular momentum derivative for this contact phase
224
  point3_t m_dL_final;
225
  /// \brief Final whole body configuration of this phase
226
  pointX_t m_q_final;
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249

  /// \brief trajectory for the joint positions
  curve_ptr m_q;
  /// \brief trajectory for the joint velocities
  curve_ptr m_dq;
  /// \brief trajectory for the joint accelerations
  curve_ptr m_ddq;
  /// \brief trajectory for the joint torques
  curve_ptr m_tau;
  /// \brief trajectory for the center of mass position
  curve_ptr m_c;
  /// \brief trajectory for the center of mass velocity
  curve_ptr m_dc;
  /// \brief trajectory for the center of mass acceleration
  curve_ptr m_ddc;
  /// \brief trajectory for the angular momentum
  curve_ptr m_L;
  /// \brief trajectory for the angular momentum derivative
  curve_ptr m_dL;
  /// \brief trajectory for the centroidal wrench
  curve_ptr m_wrench;
  /// \brief trajectory for the zmp
  curve_ptr m_zmp;
Pierre Fernbach's avatar
Pierre Fernbach committed
250
251
  /// \brief SE3 trajectory of the root of the robot
  curve_SE3_ptr m_root;
252
253

  // getter and setter for the timings
Pierre Fernbach's avatar
Pierre Fernbach committed
254
255
256
257
258
  Scalar timeInitial() const { return m_t_init; }
  void timeInitial(const Scalar t) { m_t_init = t; }
  Scalar timeFinal() const { return m_t_final; }
  void timeFinal(const Scalar t) {
    if (t < m_t_init) throw std::invalid_argument("t_final cannot be inferior to t_initial");
259
    m_t_final = t;
260
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
261
262
263
  Scalar duration() const { return m_t_final - m_t_init; }
  void duration(const Scalar d) {
    if (d <= 0) throw std::invalid_argument("Duration of the phase cannot be negative.");
Pierre Fernbach's avatar
Pierre Fernbach committed
264
265
    m_t_final = m_t_init + d;
  }
266
267

  // getter for the map trajectories
Pierre Fernbach's avatar
Pierre Fernbach committed
268
269
270
  CurveMap_t contactForces() const { return m_contact_forces; }
  CurveMap_t contactNormalForces() const { return m_contact_normal_force; }
  CurveSE3Map_t effectorTrajectories() const { return m_effector_trajectories; }
271
  curve_ptr contactForces(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
272
273
274
275
    if (m_contact_forces.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any contact force trajectory for the effector " +
                                  eeName);
    } else {
276
277
278
      return m_contact_forces.at(eeName);
    }
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
279
280
281
282
283
  curve_ptr contactNormalForces(const std::string& eeName) {
    if (m_contact_normal_force.count(eeName) == 0) {
      throw std::invalid_argument(
          "This contact phase do not contain any normal contact force trajectory for the effector " + eeName);
    } else {
284
285
286
287
      return m_contact_normal_force.at(eeName);
    }
  }
  curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
288
289
290
291
    if (m_effector_trajectories.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any effector trajectory for the effector " +
                                  eeName);
    } else {
292
293
294
295
296
297
298
299
300
301
302
      return m_effector_trajectories.at(eeName);
    }
  }
  /**
   * @brief addContactForceTrajectory add a trajectory to the map of contact forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is not defined in contact for this phase
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
303
304
305
306
  bool addContactForceTrajectory(const std::string& eeName, const curve_ptr trajectory) {
    if (!isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add a contact force trajectory for effector " + eeName +
                                  " as it is not in contact for the current phase.");
307
    bool alreadyExist(m_contact_forces.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
308
309
    if (alreadyExist) m_contact_forces.erase(eeName);
    m_contact_forces.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
310
311
312
313
314
315
316
317
318
319
320
    return !alreadyExist;
  }
  /**
   * @brief addContactNormalForceTrajectory add a trajectory to the map of contact normal forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is not defined in contact for this phase
   * @throw invalid_argument if trajectory is not of dimension 1
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
321
322
323
324
325
  bool addContactNormalForceTrajectory(const std::string& eeName, const curve_ptr trajectory) {
    if (!isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add a contact normal trajectory for effector " + eeName +
                                  " as it is not in contact for the current phase.");
    if (trajectory->dim() != 1) throw std::invalid_argument("Contact normal force trajectory must be of dimension 1");
326
    bool alreadyExist(m_contact_normal_force.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
327
328
    if (alreadyExist) m_contact_normal_force.erase(eeName);
    m_contact_normal_force.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
329
330
331
332
333
334
335
336
337
338
    return !alreadyExist;
  }
  /**
   * @brief adEffectorTrajectory add a trajectory to the map of contact forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is defined in contact for this phase
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
339
340
341
342
  bool addEffectorTrajectory(const std::string& eeName, const curve_SE3_ptr trajectory) {
    if (isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add an effector trajectory for effector " + eeName +
                                  " as it is in contact for the current phase.");
343
    bool alreadyExist(m_effector_trajectories.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
344
345
    if (alreadyExist) m_effector_trajectories.erase(eeName);
    m_effector_trajectories.insert(std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
346
347
348
    return !alreadyExist;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
349
  ContactPatchMap contactPatches() const { return m_contact_patches; }
350
  ContactPatch& contactPatch(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
351
352
353
    if (m_contact_patches.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any contact patch for the effector " + eeName);
    } else {
354
355
356
357
358
359
360
361
362
363
364
      return m_contact_patches.at(eeName);
    }
  }
  /**
   * @brief addContact add a new contact patch to this contact phase
   * If a contact phase already exist for this effector, it is overwritted.
   * If an end effector trajectory exist for this contact, it is removed.
   * @param eeName the name of the effector in contact
   * @param patch the contact patch
   * @return false if a contact for this effector already existed (and have been overwrited) true otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
365
  bool addContact(const std::string& eeName, const ContactPatch& patch) {
366
    bool alreadyExist(isEffectorInContact(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
367
    if (m_contact_patches.count(eeName))
368
      m_contact_patches.erase(eeName);
369
370
    else
      m_effector_in_contact.push_back(eeName);
Pierre Fernbach's avatar
Pierre Fernbach committed
371
    m_contact_patches.insert(std::pair<std::string, ContactPatch>(eeName, patch));
372
373
374
375
376
377
378
379
380
381
    m_effector_trajectories.erase(eeName);
    return !alreadyExist;
  }

  /**
   * @brief removeContact remove the given contact
   * This will also remove the contact_patch all the contact_forces and contact_normal_forces related to this contact
   * @param eeName the name of the effector to remove
   * @return true if the effector was in contact, false otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
382
  bool removeContact(const std::string& eeName) {
383
    bool existed(isEffectorInContact(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
384
    if (existed)
385
      m_effector_in_contact.erase(std::find(m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
386
387
388
389
390
391
    m_contact_patches.erase(eeName);
    m_contact_forces.erase(eeName);
    m_contact_normal_force.erase(eeName);
    return existed;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
392
  std::size_t numContacts() const { return m_effector_in_contact.size(); }
393

Pierre Fernbach's avatar
Pierre Fernbach committed
394
  t_strings effectorsInContact() const { return m_effector_in_contact; }
395

Pierre Fernbach's avatar
Pierre Fernbach committed
396
  bool isEffectorInContact(const std::string& eeName) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
397
    if (m_effector_in_contact.empty())
398
399
      return false;
    else
Pierre Fernbach's avatar
Pierre Fernbach committed
400
401
      return (std::find(m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName) !=
              m_effector_in_contact.end());
Pierre Fernbach's avatar
Pierre Fernbach committed
402
403
  }

404
405
406
407
  /**
   * @brief effectorsWithTrajectory return a set of all effectors for which an effector trajectory have been defined
   * @return a set of all effectors for which an effector trajectory have been defined
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
408
  t_strings effectorsWithTrajectory() const {
409
    t_strings effectors;
Pierre Fernbach's avatar
Pierre Fernbach committed
410
411
    for (typename CurveSE3Map_t::const_iterator mit = m_effector_trajectories.begin();
         mit != m_effector_trajectories.end(); ++mit) {
412
      effectors.push_back(mit->first);
413
414
415
416
417
418
419
420
421
    }
    return effectors;
  }

  /**
   * @brief effectorHaveAtrajectory check if an end effector trajectory have been defined for a given effector
   * @param eeName the effector name
   * @return true if there is a trajectory defined, false otherwise
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
422
  bool effectorHaveAtrajectory(const std::string& eeName) const { return m_effector_trajectories.count(eeName); }
423
424
425

  /* Helpers */

426
427
428
429
  /**
   * @brief isConsistent check if all the members of the phase are consistent together:
   * - There is a contact patch defined for all effector in contact
   * - There is only contact forces for the effector defined in contact
430
   * - If a trajectory is defined, it is defined between t_begin and t_final
431
432
433
434
435
   * - If init/end values are defined and a trajectory for this values is also defined, check that they match
   * - The times are positives and tbegin <= tmax
   * @param throw_if_inconsistent if true, throw an runtime_error if not consistent
   * @return true if consistent, false otherwise
   */
436
  bool isConsistent(const bool throw_if_inconsistent = false) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
437
    std::cout << "WARNING : not implemented yet, return True" << std::endl;
438
    (void)throw_if_inconsistent;
439
440
441
    return true;
  }

442
443
444
  /**
   * @brief setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete
   * COM positions, velocity and accelerations.
445
   * The trajectories are build with first order polynomials connecting each discrete points given.
446
   * this method also set the initial/final values for c, dc and ddc from the first and last discrete point given.
447
448
449
450
451
   * @param points list of discrete CoM positions
   * @param points_derivative list of discrete CoM velocities
   * @param points_second_derivative list of discrete CoM accelerations
   * @param time_points list of times corresponding to each discrete point given.
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
452
453
  void setCOMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                  const t_pointX_t& points_second_derivative, const t_time_t& time_points) {
454
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
455
456
457
    piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, points_second_derivative, time_points);
    if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
458
459
460
    m_c = curve_ptr(new piecewise_t(c_t));
    m_dc = curve_ptr(c_t.compute_derivate_ptr(1));
    m_ddc = curve_ptr(c_t.compute_derivate_ptr(2));
461
    */
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
462
463
464
465
    m_c = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dc = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
466
    m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
467
468
        points_second_derivative, time_points)));
    if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
469
      throw std::invalid_argument("Dimension of the points must be 3.");
470

471
472
473
474
475
476
    m_c_init = point3_t(points.front());
    m_c_final = point3_t(points.back());
    m_dc_init = point3_t(points_derivative.front());
    m_dc_final = point3_t(points_derivative.back());
    m_ddc_init = point3_t(points_second_derivative.front());
    m_ddc_final = point3_t(points_second_derivative.back());
477
478
479
480
481
482
    return;
  }

  /**
   * @brief setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete
   * Angular velocity values and their derivatives
483
   * The trajectories are build with first order polynomials connecting each discrete points given.
484
   * This method also set the initial/final values for L, and dL from the first and last discrete point given.
485
486
487
488
   * @param points list of discrete Angular Momentum values
   * @param points_derivative list of discrete Angular momentum derivative
   * @param time_points list of times corresponding to each discrete point given.
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
489
490
  void setAMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                 const t_time_t& time_points) {
491
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
492
493
494
    piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, time_points);
    if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
495
496
    m_L = curve_ptr(new piecewise_t(L_t));
    m_dL = curve_ptr(L_t.compute_derivate_ptr(1));
497
    */
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
498
499
500
501
502
    m_L = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dL = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
    if (m_L->dim() != 3 || m_dL->dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
503

504
505
506
507
    m_L_init = point3_t(points.front());
    m_L_final = point3_t(points.back());
    m_dL_init = point3_t(points_derivative.front());
    m_dL_final = point3_t(points_derivative.back());
508
509
510
511
512
513
    return;
  }

  /**
   * @brief setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete
   * joints positions, velocity and accelerations.
514
   * The trajectories are build with first order polynomials connecting each discrete points given.
515
   * This method also set initial/final values for q from the first and last discrete point given.
516
517
518
519
520
   * @param points list of discrete joints positions
   * @param points_derivative list of discrete joints velocities
   * @param points_second_derivative list of discrete joints accelerations
   * @param time_points list of times corresponding to each discrete point given.
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
521
522
  void setJointsTrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                     const t_pointX_t& points_second_derivative, const t_time_t& time_points) {
523
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
524
525
    piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, points_second_derivative, time_points);
526
527
528
    m_q = curve_ptr(new piecewise_t(q_t));
    m_dq = curve_ptr(q_t.compute_derivate_ptr(1));
    m_ddq = curve_ptr(q_t.compute_derivate_ptr(2));
529
    */
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
530
531
532
533
    m_q = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dq = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
534
    m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
535
        points_second_derivative, time_points)));
536
537
    m_q_init = points.front();
    m_q_final = points.back();
538
539
540
    return;
  }

541
542
543
544
545
546
547
  /**
   * @brief getContactsBroken return the list of effectors in contact in '*this' but not in contact in 'to'
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsBroken(const ContactPhase& to) const {
    t_strings res;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
548
549
    for (std::string eeName : m_effector_in_contact) {
      if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
550
551
552
553
554
555
556
557
558
559
560
    }
    return res;
  }

  /**
   * @brief getContactsCreated return the list of effectors in contact in 'to' but not in contact in '*this'
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsCreated(const ContactPhase& to) const {
    t_strings res;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
561
562
    for (std::string eeName : to.effectorsInContact()) {
      if (!isEffectorInContact(eeName)) res.push_back(eeName);
563
564
565
566
567
568
569
570
571
572
573
574
575
    }
    return res;
  }

  /**
   * @brief getContactsRepositioned return the list of effectors in contact both in 'to' and '*this'
   * but not at the same placement
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsRepositioned(const ContactPhase& to) const {
    t_strings res;
    const ContactPatchMap& to_patch_map = to.contactPatches();
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
576
577
578
    for (std::string eeName : m_effector_in_contact) {
      if (to.isEffectorInContact(eeName))
        if (m_contact_patches.at(eeName).placement() != to_patch_map.at(eeName).placement()) res.push_back(eeName);
579
580
581
582
583
584
585
586
587
588
    }
    return res;
  }

  /**
   * @brief getContactsVariations return the list of all the effectors whose contacts differ between *this and to
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsVariations(const ContactPhase& to) const {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
589
590
    std::set<std::string> set_res;  // use intermediate set to guarantee uniqueness of element
    for (std::string eeName : getContactsBroken(to)) {
591
592
      set_res.insert(eeName);
    }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
593
    for (std::string eeName : getContactsCreated(to)) {
594
595
      set_res.insert(eeName);
    }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
596
    for (std::string eeName : getContactsRepositioned(to)) {
597
598
599
600
601
      set_res.insert(eeName);
    }
    return t_strings(set_res.begin(), set_res.end());
  }

602
603
  /* End Helpers */

604
  void disp(std::ostream& os) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
    Eigen::Matrix<Scalar, 3, 5> state0(Eigen::Matrix<Scalar, 3, 5>::Zero());
    Eigen::Matrix<Scalar, 3, 5> state1(Eigen::Matrix<Scalar, 3, 5>::Zero());
    state0.block(0, 0, 3, 1) = m_c_init;
    state0.block(0, 1, 3, 1) = m_dc_init;
    state0.block(0, 2, 3, 1) = m_ddc_init;
    state0.block(0, 3, 3, 1) = m_L_init;
    state0.block(0, 4, 3, 1) = m_dL_init;
    state1.block(0, 0, 3, 1) = m_c_final;
    state1.block(0, 1, 3, 1) = m_dc_final;
    state1.block(0, 2, 3, 1) = m_ddc_final;
    state1.block(0, 3, 3, 1) = m_L_final;
    state1.block(0, 4, 3, 1) = m_dL_final;

    os << "Contact phase defined for t \\in [" << m_t_init << ";" << m_t_final << "]" << std::endl
       << "Conecting (c0,dc0,ddc0,L0,dL0) = " << std::endl
       << state0 << std::endl
       << "to        (c0,dc0,ddc0,L0,dL0) = " << std::endl
       << state1 << std::endl;
    os << "Effectors in contact " << m_effector_in_contact.size() << " : " << std::endl;
    for (t_strings::const_iterator ee = m_effector_in_contact.begin(); ee != m_effector_in_contact.end(); ++ee) {
      os << "______________________________________________" << std::endl
         << "Effector " << *ee << " contact patch:" << std::endl
         << m_contact_patches.at(*ee) << std::endl
         << "Has contact force trajectory : " << bool(m_contact_forces.count(*ee)) << std::endl
         << "Has contact normal force trajectory : " << bool(m_contact_normal_force.count(*ee)) << std::endl;
630
631
632
633
634
635
636
637
638
    }
  }

  template <typename S2>
  friend std::ostream& operator<<(std::ostream& os, const ContactPhaseTpl<S2>& cp) {
    cp.disp(os);
    return os;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
639
 protected:
640
641
  // private members
  /// \brief map with keys : effector name containing the contact forces
642
  CurveMap_t m_contact_forces;
643
  /// \brief map with keys : effector name containing the contact normal force
644
  CurveMap_t m_contact_normal_force;
645
  /// \brief map with keys : effector name containing the end effector trajectory
646
  CurveSE3Map_t m_effector_trajectories;
647
  /// \brief set of the name of all effector in contact for this phase
648
  t_strings m_effector_in_contact;
649
650
651
  /// \brief map effector name : contact patches. All the patches are actives
  ContactPatchMap m_contact_patches;
  /// \brief time at the beginning of the contact phase
652
  Scalar m_t_init;
653
  /// \brief time at the end of the contact phase
654
  Scalar m_t_final;
655

Pierre Fernbach's avatar
Pierre Fernbach committed
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void serialize(Archive& ar, const unsigned int /*version*/) {
    // ar& boost::serialization::make_nvp("placement", m_placement);
    curves::serialization::register_types<Archive>(ar);
    ar& boost::serialization::make_nvp("c_init", m_c_init);
    ar& boost::serialization::make_nvp("dc_init", m_dc_init);
    ar& boost::serialization::make_nvp("ddc_init", m_ddc_init);
    ar& boost::serialization::make_nvp("L_init", m_L_init);
    ar& boost::serialization::make_nvp("dL_init", m_dL_init);
    ar& boost::serialization::make_nvp("q_init", m_q_init);
    ar& boost::serialization::make_nvp("c_final", m_c_final);
    ar& boost::serialization::make_nvp("dc_final", m_dc_final);
    ar& boost::serialization::make_nvp("ddc_final", m_ddc_final);
    ar& boost::serialization::make_nvp("L_final", m_L_final);
    ar& boost::serialization::make_nvp("dL_final", m_dL_final);
    ar& boost::serialization::make_nvp("q_final", m_q_final);
    ar& boost::serialization::make_nvp("q", m_q);
    ar& boost::serialization::make_nvp("dq", m_dq);
    ar& boost::serialization::make_nvp("ddq", m_ddq);
    ar& boost::serialization::make_nvp("tau", m_tau);
    ar& boost::serialization::make_nvp("c", m_c);
    ar& boost::serialization::make_nvp("dc", m_dc);
    ar& boost::serialization::make_nvp("ddc", m_ddc);
    ar& boost::serialization::make_nvp("L", m_L);
    ar& boost::serialization::make_nvp("dL", m_dL);
    ar& boost::serialization::make_nvp("wrench", m_wrench);
    ar& boost::serialization::make_nvp("zmp", m_zmp);
    ar& boost::serialization::make_nvp("root", m_root);
    ar& boost::serialization::make_nvp("contact_forces", m_contact_forces);
    ar& boost::serialization::make_nvp("contact_normal_force", m_contact_normal_force);
    ar& boost::serialization::make_nvp("effector_trajectories", m_effector_trajectories);
    ar& boost::serialization::make_nvp("effector_in_contact", m_effector_in_contact);
    ar& boost::serialization::make_nvp("contact_patches", m_contact_patches);
    ar& boost::serialization::make_nvp("t_init", m_t_init);
    ar& boost::serialization::make_nvp("t_final", m_t_final);
  }

};  // struct contact phase

}  // namespace scenario
}  // namespace multicontact_api

#endif  // CONTACTPHASE_HPP