contact-phase.hpp

Go to the documentation of this file.

#ifndef __multicontact_api_scenario_contact_phase_hpp__
#define __multicontact_api_scenario_contact_phase_hpp__

#include <ndcurves/fwd.h>
#include <ndcurves/piecewise_curve.h>

#include <boost/serialization/map.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/vector.hpp>
#include <map>
#include <ndcurves/serialization/curves.hpp>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include "multicontact-api/geometry/curve-map.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/spatial.hpp"

namespace multicontact_api {
namespace scenario {

template <typename _Scalar>
struct ContactPhaseTpl
 : public serialization::Serializable<ContactPhaseTpl<_Scalar> > {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;

 // Eigen types
 typedef ndcurves::pointX_t pointX_t;
 typedef ndcurves::point3_t point3_t;
 typedef ndcurves::point6_t point6_t;
 typedef ndcurves::t_point3_t t_point3_t;
 typedef ndcurves::t_pointX_t t_pointX_t;
 typedef ndcurves::transform_t transform_t;

 // Curves types
 typedef ndcurves::curve_abc_t curve_t;
 // typedef ndcurves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
 typedef ndcurves::curve_SE3_t curve_SE3_t;
 typedef ndcurves::curve_ptr_t curve_ptr;
 // typedef std::shared_ptr<curve_3_t> curve_3_ptr;
 typedef ndcurves::curve_SE3_ptr_t curve_SE3_ptr;
 typedef ndcurves::piecewise3_t piecewise3_t;
 typedef ndcurves::piecewise_t piecewise_t;
 typedef piecewise_t::t_time_t t_time_t;

 typedef std::vector<std::string> t_strings;
 typedef ContactPatchTpl<Scalar> ContactPatch;
 typedef typename ContactPatch::SE3 SE3;
 typedef std::map<std::string, ContactPatch> ContactPatchMap;
 typedef CurveMap<curve_ptr> CurveMap_t;
 typedef CurveMap<curve_SE3_ptr> CurveSE3Map_t;

 ContactPhaseTpl()
 : 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) {}

 ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
 : 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");
 }

 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) {}

 template <typename S2>
 bool operator==(const ContactPhaseTpl<S2>& other) const {
 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;
 }

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

 // public members :
 point3_t m_c_init;
 point3_t m_dc_init;
 point3_t m_ddc_init;
 point3_t m_L_init;
 point3_t m_dL_init;
 pointX_t m_q_init;
 point3_t m_c_final;
 point3_t m_dc_final;
 point3_t m_ddc_final;
 point3_t m_L_final;
 point3_t m_dL_final;
 pointX_t m_q_final;

 curve_ptr m_q;
 curve_ptr m_dq;
 curve_ptr m_ddq;
 curve_ptr m_tau;
 curve_ptr m_c;
 curve_ptr m_dc;
 curve_ptr m_ddc;
 curve_ptr m_L;
 curve_ptr m_dL;
 curve_ptr m_wrench;
 curve_ptr m_zmp;
 curve_SE3_ptr m_root;

 // getter and setter for the timings
 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");
 m_t_final = t;
 }
 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.");
 m_t_final = m_t_init + d;
 }

 // getter for the map trajectories
 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; }
 curve_ptr contactForces(const std::string& eeName) {
 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 {
 return m_contact_forces.at(eeName);
 }
 }
 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 {
 return m_contact_normal_force.at(eeName);
 }
 }
 curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
 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 {
 return m_effector_trajectories.at(eeName);
 }
 }

 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.");
 bool alreadyExist(m_contact_forces.count(eeName));
 if (alreadyExist) m_contact_forces.erase(eeName);
 m_contact_forces.insert(
 std::pair<std::string, curve_ptr>(eeName, trajectory));
 return !alreadyExist;
 }

 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");
 bool alreadyExist(m_contact_normal_force.count(eeName));
 if (alreadyExist) m_contact_normal_force.erase(eeName);
 m_contact_normal_force.insert(
 std::pair<std::string, curve_ptr>(eeName, trajectory));
 return !alreadyExist;
 }

 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.");
 bool alreadyExist(m_effector_trajectories.count(eeName));
 if (alreadyExist) m_effector_trajectories.erase(eeName);
 m_effector_trajectories.insert(
 std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
 return !alreadyExist;
 }

 ContactPatchMap contactPatches() const { return m_contact_patches; }
 ContactPatch& contactPatch(const std::string& eeName) {
 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 {
 return m_contact_patches.at(eeName);
 }
 }

 bool addContact(const std::string& eeName, const ContactPatch& patch) {
 bool alreadyExist(isEffectorInContact(eeName));
 if (m_contact_patches.count(eeName))
 m_contact_patches.erase(eeName);
 else
 m_effector_in_contact.push_back(eeName);
 m_contact_patches.insert(
 std::pair<std::string, ContactPatch>(eeName, patch));
 m_effector_trajectories.erase(eeName);
 return !alreadyExist;
 }

 bool removeContact(const std::string& eeName) {
 bool existed(isEffectorInContact(eeName));
 if (existed)
 m_effector_in_contact.erase(std::find(
 m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
 m_contact_patches.erase(eeName);
 m_contact_forces.erase(eeName);
 m_contact_normal_force.erase(eeName);
 return existed;
 }

 std::size_t numContacts() const { return m_effector_in_contact.size(); }

 t_strings effectorsInContact() const { return m_effector_in_contact; }

 bool isEffectorInContact(const std::string& eeName) const {
 if (m_effector_in_contact.empty())
 return false;
 else
 return (std::find(m_effector_in_contact.begin(),
 m_effector_in_contact.end(),
 eeName) != m_effector_in_contact.end());
 }

 t_strings effectorsWithTrajectory() const {
 t_strings effectors;
 for (typename CurveSE3Map_t::const_iterator mit =
 m_effector_trajectories.begin();
 mit != m_effector_trajectories.end(); ++mit) {
 effectors.push_back(mit->first);
 }
 return effectors;
 }

 bool effectorHaveAtrajectory(const std::string& eeName) const {
 return m_effector_trajectories.count(eeName);
 }

 /* Helpers */

 bool isConsistent(const bool throw_if_inconsistent = false) const {
 std::cout << "WARNING : not implemented yet, return True" << std::endl;
 (void)throw_if_inconsistent;
 return true;
 }

 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) {
 /*
 piecewise_t c_t =
 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::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."); 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));
 */
 m_c = curve_ptr(
 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points, time_points)));
 m_dc = curve_ptr(new piecewise_t(
 piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points_derivative, time_points)));
 m_ddc = curve_ptr(new piecewise_t(
 piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::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.");

 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());
 return;
 }

 void setAMtrajectoryFromPoints(const t_pointX_t& points,
 const t_pointX_t& points_derivative,
 const t_time_t& time_points) {
 /*
 piecewise_t L_t =
 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
 points, points_derivative, time_points);
 if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points
 must be 3."); m_L = curve_ptr(new piecewise_t(L_t)); m_dL =
 curve_ptr(L_t.compute_derivate_ptr(1));
 */
 m_L = curve_ptr(
 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points, time_points)));
 m_dL = curve_ptr(new piecewise_t(
 piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::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.");

 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());
 return;
 }

 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) {
 /*
 piecewise_t q_t =
 piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(
 points, points_derivative, points_second_derivative, time_points);
 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));
 */
 m_q = curve_ptr(
 new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points, time_points)));
 m_dq = curve_ptr(new piecewise_t(
 piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points_derivative, time_points)));
 m_ddq = curve_ptr(new piecewise_t(
 piecewise_t::convert_discrete_points_to_polynomial<
 ndcurves::polynomial_t>(points_second_derivative, time_points)));
 m_q_init = points.front();
 m_q_final = points.back();
 return;
 }

 t_strings getContactsBroken(const ContactPhase& to) const {
 t_strings res;
 for (std::string eeName : m_effector_in_contact) {
 if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
 }
 return res;
 }

 t_strings getContactsCreated(const ContactPhase& to) const {
 t_strings res;
 for (std::string eeName : to.effectorsInContact()) {
 if (!isEffectorInContact(eeName)) res.push_back(eeName);
 }
 return res;
 }

 t_strings getContactsRepositioned(const ContactPhase& to) const {
 t_strings res;
 const ContactPatchMap& to_patch_map = to.contactPatches();
 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);
 }
 return res;
 }

 t_strings getContactsVariations(const ContactPhase& to) const {
 std::set<std::string>
 set_res; // use intermediate set to guarantee uniqueness of element
 for (std::string eeName : getContactsBroken(to)) {
 set_res.insert(eeName);
 }
 for (std::string eeName : getContactsCreated(to)) {
 set_res.insert(eeName);
 }
 for (std::string eeName : getContactsRepositioned(to)) {
 set_res.insert(eeName);
 }
 return t_strings(set_res.begin(), set_res.end());
 }

 /* End Helpers */

 void disp(std::ostream& os) const {
 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;
 }
 }

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

 protected:
 // private members
 CurveMap_t m_contact_forces;
 CurveMap_t m_contact_normal_force;
 CurveSE3Map_t m_effector_trajectories;
 t_strings m_effector_in_contact;
 ContactPatchMap m_contact_patches;
 Scalar m_t_init;
 Scalar m_t_final;

 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);
 unsigned int curve_version; // Curves API version related to the archive
 // multicontact-api API version
 if (version < 2) {
 curve_version = 0;
 } else {
 curve_version = 1;
 }
 ndcurves::serialization::register_types<Archive>(ar, curve_version);
 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

MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(
 typename Scalar, multicontact_api::scenario::ContactPhaseTpl<Scalar>)

#endif // CONTACTPHASE_HPP