contact-phase.hpp 29.2 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
#include <curves/fwd.h>
13
#include <curves/curve_abc.h>
14
#include <curves/piecewise_curve.h>
15
#include <map>
16
#include <vector>
17
18
#include <string>
#include <sstream>
19
20
#include <boost/shared_ptr.hpp>
#include <boost/serialization/shared_ptr.hpp>
21
#include <boost/serialization/string.hpp>
22
#include <boost/serialization/map.hpp>
23
#include <boost/serialization/vector.hpp>
24
#include <curves/serialization/registeration.hpp>
25

Pierre Fernbach's avatar
Pierre Fernbach committed
26
27
namespace multicontact_api {
namespace scenario {
28

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

  typedef _Scalar Scalar;
34

35
  // Eigen types
36
37
38
39
40
41
  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;
42
43

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

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

  /// \brief Default constructor
  ContactPhaseTpl()
Pierre Fernbach's avatar
Pierre Fernbach committed
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
92
93
      : 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) {}
94
95
96

  /**
   * @brief ContactPhaseTpl Constructor with time interval
Pierre Fernbach's avatar
Pierre Fernbach committed
97
   * @param t_init the time at the beginning of this contact phase
98
   * @param t_final the time at the end of this contact phase
Pierre Fernbach's avatar
Pierre Fernbach committed
99
   * @throw invalid_argument if t_final < t_init
100
   */
Pierre Fernbach's avatar
Pierre Fernbach committed
101
  ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
Pierre Fernbach's avatar
Pierre Fernbach committed
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
132
133
      : 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
134
  }
135
136

  /// \brief Copy constructor
Pierre Fernbach's avatar
Pierre Fernbach committed
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
168
169
  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) {}
170
171
172

  template <typename S2>
  bool operator==(const ContactPhaseTpl<S2>& other) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
    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;
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
  }

  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
218
  point3_t m_c_final;
219
  /// \brief Final velocity of the center of mass for this contact phase
220
  point3_t m_dc_final;
221
  /// \brief Final acceleration of the center of mass for this contact phase
222
  point3_t m_ddc_final;
223
  /// \brief Final angular momentum for this contact phase
224
  point3_t m_L_final;
225
  /// \brief Final angular momentum derivative for this contact phase
226
  point3_t m_dL_final;
227
  /// \brief Final whole body configuration of this phase
228
  pointX_t m_q_final;
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251

  /// \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
252
253
  /// \brief SE3 trajectory of the root of the robot
  curve_SE3_ptr m_root;
254
255

  // getter and setter for the timings
Pierre Fernbach's avatar
Pierre Fernbach committed
256
257
258
259
260
  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");
261
    m_t_final = t;
262
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
263
264
265
  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
266
267
    m_t_final = m_t_init + d;
  }
268
269

  // getter for the map trajectories
Pierre Fernbach's avatar
Pierre Fernbach committed
270
271
272
  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; }
273
  curve_ptr contactForces(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
274
275
276
277
    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 {
278
279
280
      return m_contact_forces.at(eeName);
    }
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
281
282
283
284
285
  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 {
286
287
288
289
      return m_contact_normal_force.at(eeName);
    }
  }
  curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
290
291
292
293
    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 {
294
295
296
297
298
299
300
301
302
303
304
      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
305
306
307
308
  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.");
309
    bool alreadyExist(m_contact_forces.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
310
311
    if (alreadyExist) m_contact_forces.erase(eeName);
    m_contact_forces.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
312
313
314
315
316
317
318
319
320
321
322
    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
323
324
325
326
327
  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");
328
    bool alreadyExist(m_contact_normal_force.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
329
330
    if (alreadyExist) m_contact_normal_force.erase(eeName);
    m_contact_normal_force.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
331
332
333
334
335
336
337
338
339
340
    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
341
342
343
344
  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.");
345
    bool alreadyExist(m_effector_trajectories.count(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
346
347
    if (alreadyExist) m_effector_trajectories.erase(eeName);
    m_effector_trajectories.insert(std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
348
349
350
    return !alreadyExist;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
351
  ContactPatchMap contactPatches() const { return m_contact_patches; }
352
  ContactPatch& contactPatch(const std::string& eeName) {
Pierre Fernbach's avatar
Pierre Fernbach committed
353
354
355
    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 {
356
357
358
359
360
361
362
363
364
365
366
      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
367
  bool addContact(const std::string& eeName, const ContactPatch& patch) {
368
    bool alreadyExist(isEffectorInContact(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
369
    if (m_contact_patches.count(eeName))
370
      m_contact_patches.erase(eeName);
371
372
    else
      m_effector_in_contact.push_back(eeName);
Pierre Fernbach's avatar
Pierre Fernbach committed
373
    m_contact_patches.insert(std::pair<std::string, ContactPatch>(eeName, patch));
374
375
376
377
378
379
380
381
382
383
    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
384
  bool removeContact(const std::string& eeName) {
385
    bool existed(isEffectorInContact(eeName));
Pierre Fernbach's avatar
Pierre Fernbach committed
386
    if (existed)
387
      m_effector_in_contact.erase(std::find(m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
388
389
390
391
392
393
    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
394
  std::size_t numContacts() const { return m_effector_in_contact.size(); }
395

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

Pierre Fernbach's avatar
Pierre Fernbach committed
398
  bool isEffectorInContact(const std::string& eeName) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
399
    if (m_effector_in_contact.empty())
400
401
      return false;
    else
Pierre Fernbach's avatar
Pierre Fernbach committed
402
403
      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
404
405
  }

406
407
408
409
  /**
   * @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
410
  t_strings effectorsWithTrajectory() const {
411
    t_strings effectors;
Pierre Fernbach's avatar
Pierre Fernbach committed
412
413
    for (typename CurveSE3Map_t::const_iterator mit = m_effector_trajectories.begin();
         mit != m_effector_trajectories.end(); ++mit) {
414
      effectors.push_back(mit->first);
415
416
417
418
419
420
421
422
423
    }
    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
424
  bool effectorHaveAtrajectory(const std::string& eeName) const { return m_effector_trajectories.count(eeName); }
425
426
427

  /* Helpers */

428
429
430
431
  /**
   * @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
432
   * - If a trajectory is defined, it is defined between t_begin and t_final
433
434
435
436
437
   * - 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
   */
438
  bool isConsistent(const bool throw_if_inconsistent = false) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
439
    std::cout << "WARNING : not implemented yet, return True" << std::endl;
440
    (void)throw_if_inconsistent;
441
442
443
    return true;
  }

444
445
446
  /**
   * @brief setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete
   * COM positions, velocity and accelerations.
447
   * The trajectories are build with first order polynomials connecting each discrete points given.
448
449
450
451
452
453
454
455
   * If the initial/final values for c, dc and ddc are not set,
   * this method also set them from the first and last discrete point given.
   * Otherwise it do not modify them.
   * @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
456
457
  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) {
458
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
459
460
461
    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.");
462
463
464
    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));
465
466
467
468
469
470
471
472
473
    */
    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)));
    m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
                                        points_second_derivative, time_points)));
    if(m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
      throw std::invalid_argument("Dimension of the points must be 3.");
474

Pierre Fernbach's avatar
Pierre Fernbach committed
475
476
477
478
479
480
    if (m_c_init.isZero()) m_c_init = point3_t(points.front());
    if (m_c_final.isZero()) m_c_final = point3_t(points.back());
    if (m_dc_init.isZero()) m_dc_init = point3_t(points_derivative.front());
    if (m_dc_final.isZero()) m_dc_final = point3_t(points_derivative.back());
    if (m_ddc_init.isZero()) m_ddc_init = point3_t(points_second_derivative.front());
    if (m_ddc_final.isZero()) m_ddc_final = point3_t(points_second_derivative.back());
481
482
483
484
485
486
    return;
  }

  /**
   * @brief setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete
   * Angular velocity values and their derivatives
487
488
   * The trajectories are build with first order polynomials connecting each discrete points given.
   * If the initial/final values for L, and dL are not set,
489
490
491
492
493
494
   * this method also set them from the first and last discrete point given.
   * Otherwise it do not modify them.
   * @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
495
496
  void setAMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                 const t_time_t& time_points) {
497
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
498
499
500
    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.");
501
502
    m_L = curve_ptr(new piecewise_t(L_t));
    m_dL = curve_ptr(L_t.compute_derivate_ptr(1));
503
504
505
506
507
508
509
    */
    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.");
510

Pierre Fernbach's avatar
Pierre Fernbach committed
511
512
513
514
    if (m_L_init.isZero()) m_L_init = point3_t(points.front());
    if (m_L_final.isZero()) m_L_final = point3_t(points.back());
    if (m_dL_init.isZero()) m_dL_init = point3_t(points_derivative.front());
    if (m_dL_final.isZero()) m_dL_final = point3_t(points_derivative.back());
515
516
517
518
519
520
    return;
  }

  /**
   * @brief setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete
   * joints positions, velocity and accelerations.
521
   * The trajectories are build with first order polynomials connecting each discrete points given.
522
523
524
525
526
527
528
529
   * If the initial/final values for q are not set,
   * this method also set them from the first and last discrete point given.
   * Otherwise it do not modify them.
   * @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
530
531
  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) {
532
    /*
Pierre Fernbach's avatar
Pierre Fernbach committed
533
534
    piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, points_second_derivative, time_points);
535
536
537
    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));
538
539
540
541
542
543
544
    */
    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)));
    m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
                                        points_second_derivative, time_points)));
Pierre Fernbach's avatar
Pierre Fernbach committed
545
546
    if (m_q_init.isZero()) m_q_init = points.front();
    if (m_q_final.isZero()) m_q_final = points.back();
547
548
549
550
551
    return;
  }

  /* End Helpers */

552
  void disp(std::ostream& os) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
    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;
578
579
580
581
582
583
584
585
586
    }
  }

  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
587
 protected:
588
589
  // private members
  /// \brief map with keys : effector name containing the contact forces
590
  CurveMap_t m_contact_forces;
591
  /// \brief map with keys : effector name containing the contact normal force
592
  CurveMap_t m_contact_normal_force;
593
  /// \brief map with keys : effector name containing the end effector trajectory
594
  CurveSE3Map_t m_effector_trajectories;
595
  /// \brief set of the name of all effector in contact for this phase
596
  t_strings m_effector_in_contact;
597
598
599
  /// \brief map effector name : contact patches. All the patches are actives
  ContactPatchMap m_contact_patches;
  /// \brief time at the beginning of the contact phase
600
  Scalar m_t_init;
601
  /// \brief time at the end of the contact phase
602
  Scalar m_t_final;
603

Pierre Fernbach's avatar
Pierre Fernbach committed
604
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
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
 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