Commit a887ef40 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'v2' into 'master'

V2

See merge request loco-3d/multicontact-api!11
parents be639a5c cb7fa82e
Pipeline #8374 passed with stage
in 43 minutes and 35 seconds
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_constraint_planar_hpp__
#define __multicontact_api_scenario_contact_constraint_planar_hpp__
#include "multicontact-api/scenario/contact-constraint.hpp"
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include <pinocchio/spatial/force.hpp>
namespace multicontact_api {
namespace scenario {
template <typename _Scalar>
struct traits<ContactConstraintPlanarTpl<_Scalar> > {
typedef _Scalar Scalar;
typedef pinocchio::ForceTpl<Scalar, 0> Force;
typedef ContactModelPlanarTpl<Scalar> ContactModel;
enum { dim_in = 6, dim_out = 2 };
typedef Force ArgumentType;
typedef Eigen::Matrix<Scalar, dim_out, 1> ReturnType;
};
template <typename _Scalar>
struct ContactConstraintPlanarTpl : public ContactConstraintBase<ContactConstraintPlanarTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef ContactConstraintBase<ContactConstraintPlanarTpl<Scalar> > Base;
typedef ContactModelPlanarTpl<Scalar> ContactModel;
typedef Eigen::DenseIndex Index;
typedef typename traits<ContactConstraintPlanarTpl>::ArgumentType ArgumentType;
typedef typename traits<ContactConstraintPlanarTpl>::ReturnType ReturnType;
using Base::contactModel;
/// \brief Default constructor
ContactConstraintPlanarTpl() : Base() {}
ContactConstraintPlanarTpl(const ContactModel& contact_model) : Base(contact_model) {}
static Index inputSize() { return traits<ContactConstraintPlanarTpl>::dim_in; }
static Index outputSize() { return traits<ContactConstraintPlanarTpl>::dim_out; }
static Index neq() { return 0; }
ReturnType value(const ArgumentType& f) const {
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
const ContactModel& contact_model = contactModel();
const Scalar& mu = contact_model.m_mu;
const Scalar& ZMP_radius = contact_model.m_ZMP_radius;
const Scalar& fx = f.linear()[0];
const Scalar& fy = f.linear()[1];
const Scalar& fz = f.linear()[2];
const Scalar& tx = f.angular()[0];
const Scalar& ty = f.angular()[1];
const Scalar& tz = f.angular()[2];
const Scalar mu_fz = mu * fz;
const Scalar ZMP_radius_fx = fx * ZMP_radius;
const Scalar ZMP_radius_fy = fy * ZMP_radius;
const Scalar ZMP_radius_fz = fz * ZMP_radius;
ReturnType res;
res[0] = mu_fz * mu_fz - f.linear().template head<2>().squaredNorm();
res[1] = ZMP_radius_fz * ZMP_radius_fz - f.angular().template head<2>().squaredNorm();
const Scalar& tmin =
-4. * mu * ZMP_radius_fz + std::fabs(ZMP_radius * fx - mu * tx) + std::fabs(ZMP_radius * fy - mu * ty);
const Scalar& tmax =
4. * mu * ZMP_radius_fz - std::fabs(ZMP_radius * fx + mu * tx) - std::fabs(ZMP_radius * fy + mu * ty);
// res[2] = tz - tmin;
// res[3] = -tz + tmax;
return res;
}
ReturnType residu(const ArgumentType& f) const { return value(f); }
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_constraint_planar_hpp__
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_constraint_hpp__
#define __multicontact_api_scenario_contact_constraint_hpp__
#include "multicontact-api/scenario/constraint.hpp"
namespace multicontact_api {
namespace scenario {
template <typename Derived>
struct ContactConstraintBase : ConstraintBase<Derived> {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename traits<Derived>::ContactModel ContactModel;
ContactConstraintBase() : m_contact_model(ContactModel()) {}
ContactConstraintBase(const ContactModel& contact_model) : m_contact_model(contact_model) {}
void setContactModel(const ContactModel& contact_model) { m_contact_model = contact_model; }
const ContactModel& contactModel() const { return m_contact_model; }
ContactModel& contactModel() { return m_contact_model; }
protected:
ContactModel m_contact_model;
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_constraint_hpp__
......@@ -19,10 +19,10 @@ struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPl
typedef _Scalar Scalar;
/// \brief Default constructor.
/// \brief Default constructor.
ContactModelPlanarTpl() : m_mu(-1.), m_ZMP_radius(-1.) {}
/// \brief Default constructor.
/// \brief Default constructor.
ContactModelPlanarTpl(const Scalar mu, const Scalar ZMP_radius) : m_mu(mu), m_ZMP_radius(ZMP_radius) {}
/// \brief Copy constructor
......@@ -37,7 +37,7 @@ struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPl
template <typename S2>
bool operator!=(const ContactModelPlanarTpl<S2>& other) const {
return !(*this != other);
return !(*this == other);
}
void disp(std::ostream& os) const {
......@@ -52,7 +52,6 @@ struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPl
/// \brief Friction coefficient.
Scalar m_mu;
/// \brief ZMP radius.
Scalar m_ZMP_radius;
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_patch_hpp__
#define __multicontact_api_scenario_contact_patch_hpp__
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/geometry/linear-cone.hpp"
#include <pinocchio/spatial/se3.hpp>
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include <pinocchio/spatial/se3.hpp>
#include <iostream>
namespace multicontact_api {
namespace scenario {
......@@ -19,69 +13,40 @@ namespace scenario {
template <typename _Scalar>
struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
typedef geometry::WrenchConeTpl<Scalar> LinearWrenchCone;
typedef ContactModelPlanarTpl<Scalar> ContactModel;
/// \brief Default constructor.
ContactPatchTpl()
: m_placement(SE3::Identity()), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
ContactPatchTpl() : m_placement(SE3::Identity()), m_mu(-1.) {}
/// \brief Init contact patch from a given placement.
explicit ContactPatchTpl(const SE3& placement)
: m_placement(placement), m_active(false), m_contact_model_placement(SE3::Identity()), m_lwc(0) {}
explicit ContactPatchTpl(const SE3& placement) : m_placement(placement), m_mu(-1.) {}
/// \brief Init contact patch from a given placement and a friction coefficient
ContactPatchTpl(const SE3& placement, const Scalar mu) : m_placement(placement), m_mu(mu) {}
/// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other)
: m_placement(other.m_placement),
m_active(other.m_active),
m_contact_model_placement(other.m_contact_model_placement),
m_oMcm(other.m_oMcm),
m_contact_model(other.m_contact_model),
m_lwc(other.m_lwc) {}
// ContactPatchTpl & operator=(const ContactPatchTpl & other)
// {
// std::cout << "Copy op" << std::endl;
// }
ContactPatchTpl(const ContactPatchTpl& other) : m_placement(other.m_placement), m_mu(other.m_mu) {}
const SE3& placement() const { return m_placement; }
SE3& placement() { return m_placement; }
bool active() const { return m_active; }
bool& active() { return m_active; }
const SE3& contactModelPlacement() const { return m_contact_model_placement; }
SE3& contactModelPlacement() { return m_contact_model_placement; }
const SE3& worldContactModelPlacement() const { return m_oMcm; }
SE3& worldContactModelPlacement() { return m_oMcm; }
const ContactModel& contactModel() const { return m_contact_model; }
ContactModel& contactModel() { return m_contact_model; }
const LinearWrenchCone& lwc() const { return m_lwc; }
LinearWrenchCone& lwc() { return m_lwc; }
const Scalar& friction() const { return m_mu; }
Scalar& friction() { return m_mu; }
template <typename S2>
bool operator==(const ContactPatchTpl<S2>& other) const {
return m_placement == other.m_placement && m_active == other.m_active &&
m_contact_model_placement == other.m_contact_model_placement && m_contact_model == other.m_contact_model &&
m_lwc == other.m_lwc;
return m_placement == other.m_placement && m_mu == other.m_mu;
}
template <typename S2>
bool operator!=(const ContactPatchTpl<S2>& other) const {
return !(*this != other);
return !(*this == other);
}
void disp(std::ostream& os) const {
os << "placement:\n"
<< m_placement << std::endl
<< "contact model placement:\n"
<< m_contact_model_placement << std::endl
<< "active: " << (m_active ? "True" : "False") << std::endl;
os << "Placement:\n" << m_placement << std::endl << "Friction coefficient : " << m_mu << std::endl;
}
template <typename S2>
......@@ -93,19 +58,8 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief Is the contact patch active?
bool m_active;
// Relative to contact model
/// \brief Placement of the contact model w.r.t the contact patch
SE3 m_contact_model_placement;
/// \brief Placement of the contact model w.r.t the world
SE3 m_oMcm;
/// \brief Contact model (Planar,Bilateral)
ContactModel m_contact_model;
LinearWrenchCone m_lwc;
/// \brief friction coefficient for this contact
Scalar m_mu;
private:
// Serialization of the class
......@@ -114,24 +68,20 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("placement", m_placement);
ar& boost::serialization::make_nvp("active", m_active);
ar& boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar& boost::serialization::make_nvp("contact_model", m_contact_model);
ar& boost::serialization::make_nvp("lwc", m_lwc);
ar& boost::serialization::make_nvp("mu", m_mu);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("placement", m_placement);
ar >> boost::serialization::make_nvp("active", m_active);
ar >> boost::serialization::make_nvp("contact_model_placement", m_contact_model_placement);
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
ar >> boost::serialization::make_nvp("lwc", m_lwc);
ar >> boost::serialization::make_nvp("mu", m_mu);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
BOOST_SERIALIZATION_SPLIT_MEMBER() // why is it required ? using only serialize() lead to compilation error,
// probably because of the SE3
}; // struct ContactPatchTpl
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_patch_hpp__
#endif // __multicontact_api_scenario_contact_patch_hpp__
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
#ifndef __multicontact_api_scenario_contact_phase_humanoid_hpp__
#define __multicontact_api_scenario_contact_phase_humanoid_hpp__
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/serialization/aligned-vector.hpp"
#include "multicontact-api/trajectories/cubic-hermite-spline.hpp"
#include <pinocchio/container/aligned-vector.hpp>
#include <pinocchio/spatial/force.hpp>
#include <boost/serialization/vector.hpp>
#include <vector>
#include <Eigen/StdVector>
namespace multicontact_api {
namespace scenario {
template <typename _Scalar>
struct ContactPhaseHumanoidTpl : public ContactPhaseTpl<_Scalar, 4>,
public serialization::Serializable<ContactPhaseHumanoidTpl<_Scalar> > {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef ContactPhaseTpl<_Scalar, 4> Base;
typedef _Scalar Scalar;
typedef pinocchio::ForceTpl<Scalar> Force;
enum {
state_dim = 9,
control_dim = 6,
com_position_id = 0,
com_velocity_id = 3,
com_acceleration_id = 3,
am_id = 6,
am_variations_id = 6,
linear_control_id = 0,
angular_control_id = 3
};
typedef Eigen::Matrix<Scalar, state_dim, 1> StateVector;
typedef Eigen::Matrix<Scalar, control_dim, 1> ControlVector;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Map<Vector3> MapVector3;
typedef Eigen::Map<StateVector> MapStateVector;
typedef Eigen::Map<ControlVector> MapControlVector;
typedef pinocchio::container::aligned_vector<StateVector> VectorStateVector;
// typedef pinocchio::container::aligned_vector<Force> VectorForce;
typedef std::vector<Force, Eigen::aligned_allocator<Force> > VectorForce;
typedef pinocchio::container::aligned_vector<ControlVector> VectorControlVector;
typedef std::vector<Scalar> VectorScalar;
typedef VectorScalar TimeVector;
typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, 1> ConfigurationVector;
typedef pinocchio::container::aligned_vector<ConfigurationVector> VectorConfigurationVector;
typedef boost::array<VectorForce, 4> ForceVectorArray;
typedef trajectories::CubicHermiteSplineTpl<Scalar, 3> CubicHermiteSpline3;
typedef trajectories::CubicHermiteSplineTpl<Scalar, Eigen::Dynamic> CubicHermiteSpline;
using Base::dim;
using Base::operator==;
using Base::m_contact_patches;
using typename Base::ContactPatch;
/// \brief default constructor
ContactPhaseHumanoidTpl()
: Base(),
RF_patch(m_contact_patches[0]),
LF_patch(m_contact_patches[1]),
RH_patch(m_contact_patches[2]),
LH_patch(m_contact_patches[3]),
m_init_state(),
m_final_state(),
m_state_trajectory(0),
m_dot_state_trajectory(0),
m_control_trajectory(0),
m_time_trajectory(0),
m_objective_trajectory(0),
m_raw_control_trajectory(0),
m_angular_momentum_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero())),
m_com_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero())),
m_vcom_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero())),
m_forces_ref(CubicHermiteSpline::Constant(CubicHermiteSpline::VectorX::Zero(24))),
m_reference_configurations(0) {}
/// \brief copy constructor
ContactPhaseHumanoidTpl(const ContactPhaseHumanoidTpl& other)
: Base(other),
RF_patch(m_contact_patches[0]),
LF_patch(m_contact_patches[1]),
RH_patch(m_contact_patches[2]),
LH_patch(m_contact_patches[3]),
m_init_state(other.m_init_state),
m_final_state(other.m_final_state),
m_state_trajectory(other.m_state_trajectory),
m_dot_state_trajectory(other.m_dot_state_trajectory),
m_control_trajectory(other.m_control_trajectory),
m_time_trajectory(other.m_time_trajectory),
m_objective_trajectory(other.m_objective_trajectory),
m_contact_forces_trajectories(other.m_contact_forces_trajectories),
m_raw_control_trajectory(other.m_raw_control_trajectory),
m_angular_momentum_ref(other.m_angular_momentum_ref),
m_com_ref(other.m_com_ref),
m_vcom_ref(other.m_vcom_ref),
m_forces_ref(other.m_forces_ref),
m_reference_configurations(other.m_reference_configurations) {}
/// \brief copy operator
// template<typename S2>
ContactPhaseHumanoidTpl& operator=(const ContactPhaseHumanoidTpl& other) {
Base::operator=(other);
m_init_state = other.m_init_state;
m_final_state = other.m_final_state;
m_state_trajectory = other.m_state_trajectory;
m_dot_state_trajectory = other.m_dot_state_trajectory;
m_control_trajectory = other.m_control_trajectory;
m_time_trajectory = other.m_time_trajectory;
m_objective_trajectory = other.m_objective_trajectory;
m_contact_forces_trajectories = other.m_contact_forces_trajectories;
m_raw_control_trajectory = other.m_raw_control_trajectory;
m_angular_momentum_ref = other.m_angular_momentum_ref;
m_com_ref = other.m_com_ref;
m_vcom_ref = other.m_vcom_ref;
m_forces_ref = other.m_forces_ref;
m_reference_configurations = other.m_reference_configurations;
return *this;
}
ContactPatch& RF_patch;
ContactPatch& LF_patch;
ContactPatch& RH_patch;
ContactPatch& LH_patch;
StateVector m_init_state;
StateVector m_final_state;
VectorStateVector m_state_trajectory;
VectorStateVector m_dot_state_trajectory;
VectorControlVector m_control_trajectory;
TimeVector m_time_trajectory;
VectorScalar m_objective_trajectory;
VectorConfigurationVector m_reference_configurations;
ForceVectorArray m_contact_forces_trajectories;
VectorConfigurationVector m_raw_control_trajectory;
CubicHermiteSpline3 m_angular_momentum_ref;
CubicHermiteSpline3 m_com_ref;
CubicHermiteSpline3 m_vcom_ref;
CubicHermiteSpline m_forces_ref;
private:
// Serialization of the class
friend class boost::serialization::access;
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& boost::serialization::make_nvp("init_state", m_init_state);
ar& boost::serialization::make_nvp("final_state", m_final_state);
ar& boost::serialization::make_nvp("state_trajectory", m_state_trajectory);
ar& boost::serialization::make_nvp("dot_state_trajectory", m_dot_state_trajectory);
ar& boost::serialization::make_nvp("control_trajectory", m_control_trajectory);
ar& boost::serialization::make_nvp("time_trajectory", m_time_trajectory);
ar& boost::serialization::make_nvp("objective_trajectory", m_objective_trajectory);
size_t reference_configurations_size = m_reference_configurations.size();
ar& boost::serialization::make_nvp("reference_configurations_size", reference_configurations_size);
for (typename VectorConfigurationVector::const_iterator it = m_reference_configurations.begin();
it != m_reference_configurations.end(); ++it)
ar& boost::serialization::make_nvp("reference_configuration", *it);
for (typename ForceVectorArray::const_iterator it = m_contact_forces_trajectories.begin();
it != m_contact_forces_trajectories.end(); ++it)
ar& boost::serialization::make_nvp("contact_force_trajectory", *it);
// const typename VectorConfigurationVector::vector_base & m_raw_control_trajectory_ =
// static_cast<const typename VectorConfigurationVector::vector_base&> (m_raw_control_trajectory);
ar& boost::serialization::make_nvp("raw_control", m_raw_control_trajectory);
ar& boost::serialization::make_nvp("angular_momentum_ref", m_angular_momentum_ref);
ar& boost::serialization::make_nvp("com_ref", m_com_ref);
ar& boost::serialization::make_nvp("vcom_ref", m_vcom_ref);
ar& boost::serialization::make_nvp("forces_ref", m_forces_ref);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar >> boost::serialization::make_nvp("init_state", m_init_state);
ar >> boost::serialization::make_nvp("final_state", m_final_state);
ar >> boost::serialization::make_nvp("state_trajectory", m_state_trajectory);
ar >> boost::serialization::make_nvp("dot_state_trajectory", m_dot_state_trajectory);
ar >> boost::serialization::make_nvp("control_trajectory", m_control_trajectory);
ar >> boost::serialization::make_nvp("time_trajectory", m_time_trajectory);
ar >> boost::serialization::make_nvp("objective_trajectory", m_objective_trajectory);
size_t reference_configurations_size = 0;
ar >> boost::serialization::make_nvp("reference_configurations_size", reference_configurations_size);
m_reference_configurations.resize(reference_configurations_size);
for (typename VectorConfigurationVector::iterator it = m_reference_configurations.begin();
it != m_reference_configurations.end(); ++it)
ar >> boost::serialization::make_nvp("reference_configuration", *it);
for (typename ForceVectorArray::iterator it = m_contact_forces_trajectories.begin();
it != m_contact_forces_trajectories.end(); ++it)
ar >> boost::serialization::make_nvp("contact_force_trajectory", *it);
// typename VectorConfigurationVector::vector_base & m_raw_control_trajectory_ =
// static_cast<typename VectorConfigurationVector::vector_base&> (m_raw_control_trajectory);
ar >> boost::serialization::make_nvp("raw_control", m_raw_control_trajectory);
ar >> boost::serialization::make_nvp("angular_momentum_ref", m_angular_momentum_ref);
ar >> boost::serialization::make_nvp("com_ref", m_com_ref);
ar >> boost::serialization::make_nvp("vcom_ref", m_vcom_ref);
ar >> boost::serialization::make_nvp("forces_ref", m_forces_ref);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
};
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_contact_phase_humanoid_hpp__
......@@ -17,44 +17,20 @@ template <typename _Scalar>
struct ContactPatchTpl;
typedef ContactPatchTpl<double> ContactPatch;
template <typename _Scalar, int _dim>
struct ContactPhaseTpl;
typedef ContactPhaseTpl<double, 4> ContactPhase4;
template <typename _Scalar>
struct ContactPhaseHumanoidTpl;
typedef ContactPhaseHumanoidTpl<double> ContactPhaseHumanoid;
struct ContactPhaseTpl;
typedef ContactPhaseTpl<double> ContactPhase;
template <class _ContactPhase>
template <typename _ContactPhase>
struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase4> ContactSequence4;
typedef ContactSequenceTpl<ContactPhaseHumanoid> ContactSequenceHumanoid;
template <class SOC>
struct ContactConstraintSOC;
typedef ContactConstraintSOC<geometry::SOC6d> ContactConstraintSOC6;
typedef ContactSequenceTpl<ContactPhase> ContactSequence;
template <typename Scalar>
struct ContactModelPlanarTpl;
typedef ContactModelPlanarTpl<double> ContactModelPlanar;
template <typename Scalar>
struct ContactConstraintPlanarTpl;
typedef ContactConstraintPlanarTpl<double> ContactConstraintPlanar;
template <class GMM>
struct ContactConstraintGMM;
enum HumanoidPhaseType {
SINGLE_SUPPORT,
DOUBLE_SUPPORT,
TRIPLE_SUPPORT,
QUADRUPLE_SUPPORT,
NO_SUPPORT,
HUMANOID_PHASE_UNDEFINED
};