contact-sequence.hpp 11.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
#ifndef __multicontact_api_scenario_contact_sequence_hpp__
#define __multicontact_api_scenario_contact_sequence_hpp__

#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-phase.hpp"

#include "multicontact-api/serialization/archive.hpp"

#include <vector>
#include <boost/serialization/vector.hpp>

Pierre Fernbach's avatar
Pierre Fernbach committed
12
13
namespace multicontact_api {
namespace scenario {
14

Pierre Fernbach's avatar
Pierre Fernbach committed
15
16
template <class _ContactPhase>
struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTpl<_ContactPhase> > {
17
18
19
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _ContactPhase ContactPhase;
  typedef typename ContactPhase::Scalar Scalar;
20
  typedef std::vector<ContactPhase> ContactPhaseVector;
21

Pierre Fernbach's avatar
Pierre Fernbach committed
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
  ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {}

  /// \brief Copy contructor
  ContactSequenceTpl(const ContactSequenceTpl& other) : m_contact_phases(other.m_contact_phases) {}

  size_t size() const { return m_contact_phases.size(); }

  bool operator==(const ContactSequenceTpl& other) const { return m_contact_phases == other.m_contact_phases; }

  bool operator!=(const ContactSequenceTpl& other) const { return !(*this == other); }

  void resize(const size_t size) { m_contact_phases.resize(size); }

  /* Accessors to the contact Phases */

  /**
   * @brief append Add the given Phase at the end of the sequence
   * @param contactPhase the phase to end
   * @return The id of the phase added in the sequence
   */
  size_t append(const ContactPhase& contactPhase) {
    m_contact_phases.push_back(contactPhase);
    return m_contact_phases.size() - 1;
  }

  /**
   * @brief contactPhases return a Const copy of the contact phase vector in this sequence.
   * Prefer accessing the contact phases through the contactPhase(id) as this one create a copy
   * @return a Const copy of the contact phase vector in this sequence
   */
  const ContactPhaseVector contactPhases() const { return m_contact_phases; }

  /**
   * @brief contactPhase return a reference to the contactPhase stored at the given Id
   * @param id the desired Id in the contact sequence
   * @return a reference to the ContactPhase
   */
  ContactPhase& contactPhase(const size_t id) {
    if (id >= m_contact_phases.size())
      // throw std::invalid_argument("Contact Sequence size is "+m_contact_phases.size()+" given Id is "+id);
      throw std::invalid_argument("Given Id is greater than the vector size");
    return m_contact_phases.at(id);
  }

  /**
   * @brief removePhase remove the given contactPhase from the sequence
   * @param id the Id of the phase to remove
   */
  void removePhase(const size_t id) {
    if (id >= m_contact_phases.size()) throw std::invalid_argument("Given Id is greater than the vector size");
    m_contact_phases.erase(m_contact_phases.begin() + id);
  }

  /* End Accessors to the contact Phases */

  /* Helpers */
78
79
80
81
82
83
84
85
86
87
88
  /**
   * @brief breakContact Add a new contactPhase at the end of the current ContactSequence,
   * The new ContactPhase have the same ContactPatchs as the last phase of the sequence,
   * with the exeption of the given contact removed.
   * It copy all the 'final' values of the last phase as 'initial' values of the new phase.
   * It also set the duration of the previous last phase.
   * @param eeName the name of the effector to remove from contact
   * @param phaseDuration if provided, the duration of the previous last phase of the sequence is set to this value
   * (it is thus the duration BEFORE breaking the contact)
   * @return true if the last phase had eeName in contact, false otherwise
   * @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
89
   * @throw invalid_argument if eeName is not in contact in the last phase of the sequence
90
   */
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
  void breakContact(const std::string& eeName, const double phaseDuration = -1){
    ContactPhase& lastPhase = m_contact_phases.back();
    if(!lastPhase.isEffectorInContact(eeName))
      throw std::invalid_argument("In breakContact : effector is not currently in contact : "+eeName);
    if(phaseDuration > 0){
      if(lastPhase.timeInitial() < 0){
        throw std::invalid_argument("In breakContact : duration is specified but current phase interval in not initialised.");
      }
      else {
        lastPhase.duration(phaseDuration);
      }
    }
    ContactPhase phase;
    // set initial values from last values of previous phase :
    phase.timeInitial(lastPhase.timeFinal());
    phase.m_c_init = lastPhase.m_c_final;
    phase.m_dc_init = lastPhase.m_dc_final;
    phase.m_ddc_init = lastPhase.m_ddc_final;
    phase.m_L_init = lastPhase.m_L_final;
    phase.m_dL_init = lastPhase.m_dL_final;
    phase.m_q_init = lastPhase.m_q_final;

    // copy contact patchs :
    for(std::string name : lastPhase.effectorsInContact()){
      if(name != eeName){
        phase.addContact(name,lastPhase.contactPatch(name));
      }
    }
    // add new phase at the end of the sequence
    append(phase);
    return;
122
123
124
125
126
127
128
129
130
131
132
133
134
135
  }

  /**
   * @brief createContact Add a new contactPhase at the end of the current ContactSequence,
   * The new ContactPhase have the same ContactPatchs as the last phase of the sequence,
   * with the exeption of the given contact added.
   * @param eeName the name of the effector used to create contact
   * @param patch the ContactPatch of the new contact
   * @param phaseDuration if provided, the duration of the previous last phase of the sequence is set to this value
   * (it is thus the duration BEFORE creating the contact)
   * @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
   * @throw invalid_argument if eeName is already in contact in the last phase of the sequence
   */
  void createContact(const std::string& eeName, const ContactPatch& patch, const double phaseDuration = -1){
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
    ContactPhase& lastPhase = m_contact_phases.back();
    if(lastPhase.isEffectorInContact(eeName))
      throw std::invalid_argument("In createContact : effector is already in contact : "+eeName);
    if(phaseDuration > 0){
      if(lastPhase.timeInitial() < 0){
        throw std::invalid_argument("In createContact : duration is specified but current phase interval in not initialised.");
      }
      else {
        lastPhase.duration(phaseDuration);
      }
    }
    ContactPhase phase;
    // set initial values from last values of previous phase :
    phase.timeInitial(lastPhase.timeFinal());
    phase.m_c_init = lastPhase.m_c_final;
    phase.m_dc_init = lastPhase.m_dc_final;
    phase.m_ddc_init = lastPhase.m_ddc_final;
    phase.m_L_init = lastPhase.m_L_final;
    phase.m_dL_init = lastPhase.m_dL_final;
    phase.m_q_init = lastPhase.m_q_final;

    // copy contact patchs :
    for(std::string name : lastPhase.effectorsInContact()){
        phase.addContact(name,lastPhase.contactPatch(name));
    }
    // add new contact to new phase :
    phase.addContact(eeName,patch);
    // add new phase at the end of the sequence
    append(phase);
    return;
166
167
  }

168

169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
  /**
   * @brief moveEffectorToPlacement Add two new phases at the end of the current ContactSequence,
   *  - it break the contact with eeName
   *  - it create the contact with eeName at the given placement.
   * It copy all the 'final' values of the last phase as 'initial' values of the new phase.
   * It also set the duration of the previous last phase.
   * @param eeName the name of the effector used to create contact
   * @param placement the new placement for the contact of eeName
   * @param durationBreak the duration of the previous last phase of the sequence
   *  (it is thus the duration BEFORE breaking the contact)
   * @param durationCreate the duration of the first new ContactPhase
   *  (it is thus the duration BEFORE creating the contact)
   * @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
   * @throw invalid_argument if eeName is not in contact in the last phase of the sequence
   */
  void moveEffectorToPlacement(const std::string& eeName, const ContactPatch::SE3& placement,
   const double durationBreak = -1, const double durationCreate = -1 ){
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
    if(!m_contact_phases.back().isEffectorInContact(eeName))
      throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : "+eeName);
    ContactPatch target(m_contact_phases.back().contactPatch(eeName));
    target.placement() = placement;
    breakContact(eeName,durationBreak);
    // copy all "init" value to "final" for the current last phase :
    ContactPhase& lastPhase = m_contact_phases.back();
    lastPhase.m_c_final = lastPhase.m_c_init;
    lastPhase.m_dc_final = lastPhase.m_dc_init;
    lastPhase.m_ddc_final = lastPhase.m_ddc_init;
    lastPhase.m_L_final = lastPhase.m_L_init;
    lastPhase.m_dL_final = lastPhase.m_dL_init;
    lastPhase.m_q_final = lastPhase.m_q_init;
    createContact(eeName,target,durationCreate);
    return;
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
  }

    /**
   * @brief moveEffectorOf similar to moveEffectorToPlacement
   * exept that the new placement is defined from the previous placement and a given transform applied.
   * @param eeName the name of the effector used to create contact
   * @param transform the new placement for the contact of eeName
   * @param durationBreak the duration of the previous last phase of the sequence
   *  (it is thus the duration BEFORE breaking the contact)
   * @param durationCreate the duration of the first new ContactPhase
   *  (it is thus the duration BEFORE creating the contact)
   * @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
   * @throw invalid_argument if eeName is not in contact in the last phase of the sequence
   */
  void moveEffectorOf(const std::string& eeName, const ContactPatch::SE3& transform,
   const double durationBreak = -1, const double durationCreate = -1 ){
217
218
219
220
221
    if(!m_contact_phases.back().isEffectorInContact(eeName))
      throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : "+eeName);
    ContactPatch::SE3 previous(m_contact_phases.back().contactPatch(eeName).placement());
    ContactPatch::SE3 target = previous.act(transform);
    return moveEffectorToPlacement(eeName,target,durationBreak,durationCreate);
222
223
224
  }


Pierre Fernbach's avatar
Pierre Fernbach committed
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
  /* End Helpers */

  /*Public Attributes*/
  ContactPhaseVector m_contact_phases;
  /*Public Attributes*/

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    const size_t m_size = size();
    ar& boost::serialization::make_nvp("size", m_size);
    for (typename ContactPhaseVector::const_iterator it = m_contact_phases.begin(); it != m_contact_phases.end();
         ++it) {
      ar& boost::serialization::make_nvp("contact_phase", *it);
242
    }
Pierre Fernbach's avatar
Pierre Fernbach committed
243
244
245
246
247
248
249
250
251
252
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    size_t m_size;
    ar >> boost::serialization::make_nvp("size", m_size);
    assert(m_size > 0);
    resize(m_size);
    for (typename ContactPhaseVector::iterator it = m_contact_phases.begin(); it != m_contact_phases.end(); ++it) {
      ar >> boost::serialization::make_nvp("contact_phase", *it);
253
    }
Pierre Fernbach's avatar
Pierre Fernbach committed
254
  }
255

Pierre Fernbach's avatar
Pierre Fernbach committed
256
  BOOST_SERIALIZATION_SPLIT_MEMBER()
257

Pierre Fernbach's avatar
Pierre Fernbach committed
258
};  // end class ContactSequence
259

Pierre Fernbach's avatar
Pierre Fernbach committed
260
261
262
}  // namespace scenario
}  // namespace multicontact_api
#endif  // __multicontact_api_scenario_contact_sequence_hpp__