Commit b80915fe authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

WIP refactor contact-phase ongoing

parent 3021da0d
......@@ -10,5 +10,17 @@ void exposeContactPhase() {
ContactPhasePythonVisitor<multicontact_api::scenario::ContactPhase4>::expose("ContactPhase4");
//ContactPhaseHumanoidPythonVisitor<multicontact_api::scenario::ContactPhaseHumanoid>::expose("ContactPhaseHumanoid");
}
} // namespace python
}
CurveMap ContactPhaseTpl::contact_forces() const
{
return m_contact_forces;
}
void ContactPhaseTpl::setContact_forces(const CurveMap &contact_forces)
{
m_contact_forces = contact_forces;
}
// namespace python
} // namespace multicontact_api
#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"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include <curves/curve_abc.h>
#include <map>
#include <set>
#include <string>
#include <sstream>
#include <boost/serialization/map.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/shared_ptr.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 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> pointX_t;
typedef Eigen::Matrix<Scalar,3, 1> point3_t;
typedef Eigen::Matrix<Scalar, 6, 1> point6_t;
typedef Eigen::Transform<Scalar, 3, Eigen::Affine> transform_t;
// Curves types
typedef curves::curve_abc<Scalar, Scalar, true, pointX_t> curve_t;
//typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
typedef curves::curve_abc<Scalar, Scalar, true, transform_t,point6_t> curve_SE3_t;
typedef boost::shared_ptr<curve_t> curve_ptr;
//typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr;
typedef ContactPatchTpl<Scalar> ContactPatch;
typedef typename ContactPatch::SE3 SE3;
typedef std::map< std::string, ContactPatch > ContactPatchMap;
typedef std::map< std::string, curve_ptr > CurveMap;
typedef std::map< std::string, curve_SE3_ptr > CurveSE3Map;
/// \brief Default constructor
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_end(point3_t::Zero()),
m_dc_end(point3_t::Zero()),
m_ddc_end(point3_t::Zero()),
m_L_end(point3_t::Zero()),
m_dL_end(point3_t::Zero()),
m_q_end(),
m_q(),
m_dq(),
m_ddq(),
m_tau(),
m_c(),
m_dc(),
m_ddc(),
m_L(),
m_dL(),
m_wrench(),
m_zmp(),
m_contact_forces(),
m_contact_normal_force(),
m_effector_trajectories(),
m_effector_in_contact(),
m_contact_patches(),
m_t_begin(-1),
m_t_end(-1)
{}
/**
* @brief ContactPhaseTpl Constructor with time interval
* @param t_begin the time at the beginning of this contact phase
* @param t_end the time at the end of this contact phase
*/
ContactPhaseTpl(const Scalar t_begin, const Scalar t_end)
: 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_end(point3_t::Zero()),
m_dc_end(point3_t::Zero()),
m_ddc_end(point3_t::Zero()),
m_L_end(point3_t::Zero()),
m_dL_end(point3_t::Zero()),
m_q_end(),
m_q(),
m_dq(),
m_ddq(),
m_tau(),
m_c(),
m_dc(),
m_ddc(),
m_L(),
m_dL(),
m_wrench(),
m_zmp(),
m_contact_forces(),
m_contact_normal_force(),
m_effector_trajectories(),
m_effector_in_contact(),
m_contact_patches(),
m_t_begin(t_begin),
m_t_end(t_end)
{}
/// \brief Copy constructor
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_end(other.m_c_end),
m_dc_end(other.m_dc_end),
m_ddc_end(other.m_ddc_end),
m_L_end(other.m_L_end),
m_dL_end(other.m_dL_end),
m_q_end(other.m_q_end),
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_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_begin(other.m_t_begin),
m_t_end(other.m_t_end)
{}
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 == other.m_q_init &&
m_c_end == other.m_c_end &&
m_dc_end == other.m_dc_end &&
m_ddc_end == other.m_ddc_end &&
m_L_end == other.m_L_end &&
m_dL_end == other.m_dL_end &&
m_q_end == other.m_q_end &&
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_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_begin == other.m_t_begin &&
m_t_end == other.m_t_end;
}
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
point3_t m_c_end;
/// \brief Final velocity of the center of mass for this contact phase
point3_t m_dc_end;
/// \brief Final acceleration of the center of mass for this contact phase
point3_t m_ddc_end;
/// \brief Final angular momentum for this contact phase
point3_t m_L_end;
/// \brief Final angular momentum derivative for this contact phase
point3_t m_dL_end;
/// \brief Final whole body configuration of this phase
pointX_t m_q_end;
/// \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;
// getter and setter for the timings
Scalar timeBegin() const {return m_t_begin;}
void timeBegin(const Scalar t){m_t_begin = t;}
Scalar timeEnd() const {return m_t_end;}
void timeEnd(const Scalar t){
if(t<=m_t_begin)
throw std::invalid_argument("t_end cannot be inferior to t_begin");
m_t_end = t;
}
Scalar duration() const {return m_t_end - m_t_begin;}
void duration(const Scalar d){m_t_end = m_t_begin + d;}
// getter for the map trajectories
CurveMap contactForces() const{return m_contact_forces;}
CurveMap contactNormalForces() const{return m_contact_normal_force;}
CurveSE3Map 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);
}
}
/**
* @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
*/
bool addContactForceTrajectory(const std::string& eeName, const curve_ptr trajectory){
if(m_effector_in_contact.count(eeName) == 0)
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(m_contact_forces.count(eeName))
m_contact_forces.erase(eeName);
m_contact_forces.insert(std::pair<std::string,curve_ptr>(eeName,trajectory));
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
*/
bool addContactNormalForceTrajectory(const std::string& eeName, const curve_ptr trajectory){
if(m_effector_in_contact.count(eeName)==0)
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(m_contact_normal_force.count(eeName))
m_contact_normal_force.erase(eeName);
m_contact_normal_force.insert(std::pair<std::string,curve_ptr>(eeName,trajectory));
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
*/
bool adEffectorTrajectory(const std::string& eeName, const curve_SE3_ptr trajectory){
if(m_effector_in_contact.count(eeName) > 0)
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(m_effector_trajectories.count(eeName))
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);
}
}
/**
* @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
*/
bool addContact(const std::string& eeName, const ContactPatch& patch){
bool alreadyExist(m_contact_patches.count(eeName));
if(m_contact_patches.count(eeName))
m_contact_patches.erase(eeName);
m_contact_patches.insert(std::pair<std::string,ContactPatch>(eeName,patch));
m_effector_in_contact.insert(eeName);
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
*/
bool removeContact(const std::string& eeName){
bool existed(m_effector_in_contact.count(eeName));
m_effector_in_contact.erase(eeName);
m_contact_patches.erase(eeName);
m_contact_forces.erase(eeName);
m_contact_normal_force.erase(eeName);
return existed;
}
/**
* @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
* - If a trajectory is defined, it is defined between t_begin and t_end
* - 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
*/
bool isConsistent(const bool throw_if_inconsistent = false){
//TODO
return true;
}
protected:
// private members
/// \brief map with keys : effector name containing the contact forces
CurveMap m_contact_forces;
/// \brief map with keys : effector name containing the contact normal force
CurveMap m_contact_normal_force;
/// \brief map with keys : effector name containing the end effector trajectory
CurveSE3Map m_effector_trajectories;
/// \brief set of the name of all effector in contact for this phase
std::set<std::string> m_effector_in_contact;
/// \brief map effector name : contact patches. All the patches are actives
ContactPatchMap m_contact_patches;
/// \brief time at the beginning of the contact phase
Scalar m_t_begin;
/// \brief time at the end of the contact phase
Scalar m_t_end;
}; //struct contact phase
} //scenario
}// multicontact-api
#endif // CONTACTPHASE_HPP
......@@ -9,7 +9,7 @@
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
//#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-phase.hpp"
//#include "multicontact-api/scenario/contact-sequence.hpp"
using namespace multicontact_api::scenario;
......@@ -100,27 +100,11 @@ BOOST_AUTO_TEST_CASE(contact_patch) {
BOOST_CHECK(cp3 == cp_from_bin);
}
//BOOST_AUTO_TEST_CASE(contact_phase)
//{
// ContactPhase4 cp, cp_test;
// for(ContactPhase4::ContactPatchMap::iterator it = cp.contact_patches().begin();
// it != cp.contact_patches().end(); ++it)
// {
// it->second.contactModel().m_mu = 0.3;
// it->second.contactModel().m_ZMP_radius = 0.01;
// it->second.placement().setRandom();
// it->second.contactModelPlacement().setRandom();
// it->second.worldContactModelPlacement().setRandom();
// }
// ContactPhase4 cp2(cp);
// BOOST_CHECK(cp == cp);
// BOOST_CHECK(cp == cp2);
// // test serialization
// cp.saveAsText("serialization_cp_test.test");
// cp_test.loadFromText("serialization_cp_test.test");
// remove("serialization_cp_test.test");
// BOOST_CHECK(cp == cp_test);
//}
BOOST_AUTO_TEST_CASE(contact_phase)
{
}
//BOOST_AUTO_TEST_CASE(contact_sequence)
//{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment