Commit 79f130a5 authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

[C++] move definition of operator == between CurveMap in other file

Needed to avoid conflict
parent 5c775fba
// Copyright (c) 2019-2020, CNRS
// Authors: Pierre Fernbach <pfernbac@laas.fr>
#ifndef __multicontact_api_geometry_curve_map_hpp__
#define __multicontact_api_geometry_curve_map_hpp__
#include <curves/curve_abc.h>
#include <map>
#include <string>
#include "multicontact-api/serialization/archive.hpp"
#include <boost/serialization/access.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/base_object.hpp>
#include <curves/serialization/registeration.hpp>
template <typename Curve>
struct CurveMap : public std::map<std::string,Curve> {
typedef CurveMap<Curve> CurveMap_t;
typedef std::map<std::string,Curve> Parent;
// define operator == for map of shared ptr: start by checking if the ptr are same, otherwise check if the values are the sames
bool operator==(const CurveMap_t& other) const {
if(this->size() != other.size())
return false;
for(typename Parent::const_iterator it = this->begin() ; it != this->end() ; ++it){
if(other.count(it->first) < 1)
return false;
if((it->second != other.at(it->first)) && !(it->second->isApprox(other.at(it->first).get())))
return false;
}
return true;
}
bool operator!=(const CurveMap_t& other)const {
return !(*this == other);
}
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Parent);
}
};
#endif // CURVEMAP_HPP
......@@ -4,6 +4,7 @@
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
#include "multicontact-api/geometry/curve-map.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
......@@ -48,8 +49,8 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc
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;
typedef CurveMap<curve_ptr> CurveMap_t;
typedef CurveMap<curve_SE3_ptr> CurveSE3Map_t;
/// \brief Default constructor
......@@ -279,9 +280,9 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc
}
// 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;}
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);
......@@ -422,7 +423,7 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc
*/
t_strings effectorsWithTrajectory() const{
t_strings effectors;
for(typename CurveSE3Map::const_iterator mit = m_effector_trajectories.begin() ; mit != m_effector_trajectories.end(); ++mit)
for(typename CurveSE3Map_t::const_iterator mit = m_effector_trajectories.begin() ; mit != m_effector_trajectories.end(); ++mit)
{
effectors.push_back(mit->first);
}
......@@ -490,11 +491,11 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc
protected:
// private members
/// \brief map with keys : effector name containing the contact forces
CurveMap m_contact_forces;
CurveMap_t m_contact_forces;
/// \brief map with keys : effector name containing the contact normal force
CurveMap m_contact_normal_force;
CurveMap_t m_contact_normal_force;
/// \brief map with keys : effector name containing the end effector trajectory
CurveSE3Map m_effector_trajectories;
CurveSE3Map_t m_effector_trajectories;
/// \brief set of the name of all effector in contact for this phase
t_strings m_effector_in_contact;
/// \brief map effector name : contact patches. All the patches are actives
......@@ -543,36 +544,10 @@ private:
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
// define operator == for map of shared ptr: start by checking if the ptr are same, otherwise check if the values are the sames
bool operator==(const ContactPhase::CurveMap& a,const ContactPhase::CurveMap& b){
if(a.size() != b.size())
return false;
for(ContactPhase::CurveMap::const_iterator it = a.begin() ; it != a.end() ; ++it){
if(b.count(it->first) < 1)
return false;
if((it->second != b.at(it->first)) && !(it->second->isApprox(b.at(it->first).get())))
return false;
}
return true;
}
bool operator==(const ContactPhase::CurveSE3Map& a,const ContactPhase::CurveSE3Map& b){
if(a.size() != b.size())
return false;
for(ContactPhase::CurveSE3Map::const_iterator it = a.begin() ; it != a.end() ; ++it){
if(b.count(it->first) < 1)
return false;
if((it->second != b.at(it->first)) && !(it->second->isApprox(b.at(it->first).get())))
return false;
}
return true;
}
} //scenario
}// multicontact-api
......
......@@ -332,8 +332,8 @@ void explicitContactPhaseAssertEqual(ContactPhase& cp1, ContactPhase& cp2){
BOOST_CHECK((*cp1.contactNormalForces(*ee))(cp1.contactNormalForces(*ee)->max()) == (*cp2.contactNormalForces(*ee))(cp2.contactNormalForces(*ee)->max()));
}
}
const ContactPhase::CurveSE3Map trajMap = cp2.effectorTrajectories();
for (ContactPhase::CurveSE3Map::const_iterator mit = trajMap.begin() ; mit != trajMap.end(); ++mit)
const ContactPhase::CurveSE3Map_t trajMap = cp2.effectorTrajectories();
for (ContactPhase::CurveSE3Map_t::const_iterator mit = trajMap.begin() ; mit != trajMap.end(); ++mit)
{
BOOST_CHECK(cp2.effectorTrajectories().count(mit->first));
BOOST_CHECK(cp1.effectorTrajectories(mit->first)->isApprox(cp2.effectorTrajectories(mit->first).get()));
......@@ -719,7 +719,7 @@ BOOST_AUTO_TEST_CASE(contact_phase)
cp2.addContactForceTrajectory("right_hand",buildRandomPolynomial12D());
cp2.addContactNormalForceTrajectory("right_hand",buildRandomPolynomial1D());
int num_ctc = 0;
for (ContactPhase::CurveMap::const_iterator mit = cp2.contactForces().begin() ; mit != cp2.contactForces().end(); ++mit)
for (ContactPhase::CurveMap_t::const_iterator mit = cp2.contactForces().begin() ; mit != cp2.contactForces().end(); ++mit)
{
BOOST_CHECK(mit->first == "right_hand" || mit->first == "left_leg");
num_ctc ++;
......@@ -738,8 +738,8 @@ BOOST_AUTO_TEST_CASE(contact_phase)
BOOST_CHECK((*cp2.effectorTrajectories()["right_leg"])(1.2).isApprox((*effR)(1.2)));
BOOST_CHECK(cp2.effectorTrajectories().size() == 2);
int num_eff_traj = 0;
const ContactPhase::CurveSE3Map trajMap = cp2.effectorTrajectories();
for (ContactPhase::CurveSE3Map::const_iterator mit = trajMap.begin() ; mit != trajMap.end(); ++mit)
const ContactPhase::CurveSE3Map_t trajMap = cp2.effectorTrajectories();
for (ContactPhase::CurveSE3Map_t::const_iterator mit = trajMap.begin() ; mit != trajMap.end(); ++mit)
{
BOOST_CHECK(mit->first == "knee" || mit->first == "right_leg");
num_eff_traj ++;
......
Markdown is supported
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