...
 
Commits (17)
......@@ -32,10 +32,11 @@ INCLUDE(cmake/ide.cmake)
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
SET(CMAKE_CXX_STANDARD 11)
# Project dependencies
ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.0.0")
ADD_PROJECT_DEPENDENCY(curves 0.4.1 REQUIRED PKG_CONFIG_REQUIRES "curves >= 0.4.1")
ADD_PROJECT_DEPENDENCY(curves 0.5.1 REQUIRED PKG_CONFIG_REQUIRES "curves >= 0.5.1")
IF(NOT CURVES_WITH_PINOCCHIO_SUPPORT)
MESSAGE(FATAL_ERROR "you need to use a curves version compiled with pinocchio support")
ENDIF(NOT CURVES_WITH_PINOCCHIO_SUPPORT)
......
Subproject commit a5c65a0ee51191be3f2e2667b95830706c211bb2
Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5
No preview for this file type
No preview for this file type
No preview for this file type
No preview for this file type
......@@ -97,10 +97,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
*/
Matrix6X generatorMatrix() const {
Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
for(size_t i=0; i<m_num_contact_points; i++)
{
gen.block(0, i*3, 3, 3) = Matrix3::Identity();
gen.block(3, i*3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
for (size_t i = 0; i < m_num_contact_points; i++) {
gen.block(0, i * 3, 3, 3) = Matrix3::Identity();
gen.block(3, i * 3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
}
return gen;
}
......@@ -140,4 +139,6 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
} // namespace scenario
} // namespace multicontact_api
DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactModelTpl<Scalar>)
#endif // ifndef __multicontact_api_scenario_contact_model_planar_hpp__
......@@ -76,9 +76,15 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
void load(Archive& ar, const unsigned int version) {
ar >> boost::serialization::make_nvp("placement", m_placement);
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
if (version >= 1) {
ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
} else {
double mu;
ar >> boost::serialization::make_nvp("mu", mu);
m_contact_model = ContactModel(mu);
}
}
BOOST_SERIALIZATION_SPLIT_MEMBER() // why is it required ? using only serialize() lead to compilation error,
......@@ -88,4 +94,6 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
} // namespace scenario
} // namespace multicontact_api
DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactPatchTpl<Scalar>)
#endif // __multicontact_api_scenario_contact_patch_hpp__
......@@ -658,9 +658,15 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
void serialize(Archive& ar, const unsigned int version) {
// ar& boost::serialization::make_nvp("placement", m_placement);
curves::serialization::register_types<Archive>(ar);
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;
}
curves::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);
......@@ -699,4 +705,6 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
} // namespace scenario
} // namespace multicontact_api
DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactPhaseTpl<Scalar>)
#endif // CONTACTPHASE_HPP
......@@ -1088,13 +1088,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
}
}
}
if(first_phase == m_contact_phases.size())
throw std::invalid_argument("The contact sequence doesn't have any phase with an effector trajectory"
" for the given effector name");
if(first_phase > 0){
if (first_phase == m_contact_phases.size())
throw std::invalid_argument(
"The contact sequence doesn't have any phase with an effector trajectory"
" for the given effector name");
if (first_phase > 0) {
// add a first constant phase at the initial placement
curve_SE3_ptr ptr_init(new SE3Curve_t(first_placement, first_placement, m_contact_phases.at(0).timeInitial(),
m_contact_phases.at(first_phase).timeInitial()));
m_contact_phases.at(first_phase).timeInitial()));
res.add_curve_ptr(ptr_init);
}
// loop over this phases to concatenate the trajectories
......@@ -1109,10 +1110,11 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
res.add_curve_ptr(ptr);
}
}
if(last_phase < m_contact_phases.size() - 1){
if (last_phase < m_contact_phases.size() - 1) {
// add a last constant phase until the end of the contact sequence
curve_SE3_ptr ptr_final(new SE3Curve_t(last_placement, last_placement, m_contact_phases.at(last_phase).timeFinal(),
m_contact_phases.back().timeFinal()));
curve_SE3_ptr ptr_final(new SE3Curve_t(last_placement, last_placement,
m_contact_phases.at(last_phase).timeFinal(),
m_contact_phases.back().timeFinal()));
res.add_curve_ptr(ptr_final);
}
return res;
......@@ -1248,4 +1250,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
} // namespace scenario
} // namespace multicontact_api
DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactSequenceTpl<Scalar>)
#endif // __multicontact_api_scenario_contact_sequence_hpp__
......@@ -13,6 +13,22 @@
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/version.hpp>
const unsigned int API_VERSION = 2; // must be increased everytime the save() method of a class is modified
// Macro used to define the serialization version of a templated class
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type) \
namespace boost { \
namespace serialization { \
template <Template> \
struct version<Type> { \
static constexpr unsigned int value = API_VERSION; \
}; \
template <Template> \
constexpr unsigned int version<Type>::value; \
} \
}
namespace multicontact_api {
namespace serialization {
......
......@@ -7,6 +7,7 @@ SET(${PROJECT_NAME}_TESTS
geometry
scenario
examples
serialization_versionning
)
FOREACH(TEST ${${PROJECT_NAME}_TESTS})
......@@ -15,6 +16,7 @@ FOREACH(TEST ${${PROJECT_NAME}_TESTS})
ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS})
TARGET_COMPILE_DEFINITIONS(examples PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
TARGET_COMPILE_DEFINITIONS(serialization_versionning PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
......
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
// Copyright (c) 2019-2020, CNRS
// Authors: Pierre Fernbach <pierre.fernbach@laas.fr>,
#include <iostream>
......@@ -59,6 +59,8 @@ BOOST_AUTO_TEST_CASE(step_in_place) {
cs.loadFromBinary(path + "step_in_place.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_COM) {
......@@ -69,6 +71,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_COM) {
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_REF) {
......@@ -81,6 +85,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false));
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_WB) {
......@@ -96,6 +102,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_WB) {
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic) {
......@@ -103,6 +111,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic) {
cs.loadFromBinary(path + "step_in_place_quasistatic.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
......@@ -113,6 +123,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
......@@ -124,6 +136,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
......@@ -139,6 +153,8 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm) {
......@@ -146,6 +162,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm) {
cs.loadFromBinary(path + "walk_20cm.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
......@@ -156,6 +174,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
......@@ -167,6 +187,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
......@@ -182,6 +204,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic) {
......@@ -189,6 +213,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic) {
cs.loadFromBinary(path + "walk_20cm_quasistatic.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
......@@ -199,6 +225,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(!cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
......@@ -210,6 +238,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
......@@ -225,6 +255,8 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_SUITE_END()
......@@ -129,6 +129,8 @@ class ExamplesSerialization(unittest.TestCase):
cs.loadFromBinary(str(PATH / "step_in_place.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
def test_step_in_place_COM(self):
cs = ContactSequence()
......@@ -138,6 +140,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, effector=False, wholeBody=False)
def test_step_in_place_REF(self):
......@@ -150,6 +154,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_step_in_place_WB(self):
......@@ -165,6 +171,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, effector=True, wholeBody=True)
def test_step_in_place_quasistatic(self):
......@@ -172,6 +180,8 @@ class ExamplesSerialization(unittest.TestCase):
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
def test_step_in_place_quasistatic_COM(self):
cs = ContactSequence()
......@@ -181,6 +191,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_step_in_place_quasistatic_REF(self):
......@@ -192,6 +204,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
def test_step_in_place_quasistatic_WB(self):
......@@ -207,12 +221,16 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
def test_walk_20cm(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm.cs"))
self.assertEqual(cs.size(), 23)
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
self.assertTrue(cs.haveConsistentContacts())
def test_walk_20cm_COM(self):
......@@ -223,6 +241,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, effector=False, wholeBody=False)
def test_walk_20cm_REF(self):
......@@ -234,6 +254,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_walk_20cm_WB(self):
......@@ -249,12 +271,16 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, effector=True, wholeBody=True)
def test_walk_20cm_quasistatic(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
self.assertEqual(cs.size(), 23)
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
self.assertTrue(cs.haveConsistentContacts())
def test_walk_20cm_quasistatic_COM(self):
......@@ -265,6 +291,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_walk_20cm_quasistatic_REF(self):
......@@ -276,6 +304,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
def test_walk_20cm_quasistatic_WB(self):
......@@ -291,6 +321,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
......
......@@ -387,7 +387,7 @@ BOOST_AUTO_TEST_CASE(contact_model_points_positions) {
BOOST_CHECK(mp.contact_points_positions().isApprox(positions));
Matrix6X generators = mp.generatorMatrix();
BOOST_CHECK_EQUAL(generators.rows(), 6);
BOOST_CHECK_EQUAL(generators.cols(), 6*3);
BOOST_CHECK_EQUAL(generators.cols(), 6 * 3);
mp.num_contact_points(2);
BOOST_CHECK_EQUAL(mp.num_contact_points(), 2);
......@@ -827,9 +827,8 @@ BOOST_AUTO_TEST_CASE(contact_phase) {
cp2.addContactForceTrajectory("right_hand", buildRandomPolynomial12D());
cp2.addContactNormalForceTrajectory("right_hand", buildRandomPolynomial1D());
int num_ctc = 0;
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");
for (auto const& mit : cp2.contactForces()) {
BOOST_CHECK(mit.first == "right_hand" || mit.first == "left_leg");
num_ctc++;
}
BOOST_CHECK(num_ctc == 2);
......@@ -1741,8 +1740,8 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_contact_model_defined) {
cs1.append(cp0);
cs1.append(cp1);
// cp.addContact("right_hand", ContactPatch(SE3::Identity().setRandom()));
// cp.addContact("left_foot", ContactPatch(SE3::Identity().setRandom()));
// cp.addContact("right_hand", ContactPatch(SE3::Identity().setRandom()));
// cp.addContact("left_foot", ContactPatch(SE3::Identity().setRandom()));
BOOST_CHECK(!cs1.haveContactModelDefined());
ContactModel mp1(0.3, ContactType::CONTACT_PLANAR);
Matrix3X positions = Matrix3X::Random(3, 4);
......@@ -1763,14 +1762,13 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_contact_model_defined) {
cs1.contactPhase(2).contactPatch("left_foot").m_contact_model = mp2;
BOOST_CHECK(!cs1.haveContactModelDefined());
mp3.m_contact_type = ContactType::CONTACT_PLANAR; // no effect
mp3.m_contact_type = ContactType::CONTACT_PLANAR; // no effect
BOOST_CHECK(!cs1.haveContactModelDefined());
cs1.contactPhase(2).contactPatch("right_hand").m_contact_model.m_contact_type = ContactType::CONTACT_PLANAR;
BOOST_CHECK(cs1.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_com_traj) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = buildRandomContactPhase(0, 2);
......
// Copyright (c) 2019-2020, CNRS
// Authors: Pierre Fernbach <pierre.fernbach@laas.fr>,
#include <iostream>
#define BOOST_TEST_MODULE StatsTests
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/scenario/fwd.hpp"
#include "curves/fwd.h"
#include <curves/so3_linear.h>
#include <curves/se3_curve.h>
#include <curves/polynomial.h>
#include <curves/bezier_curve.h>
#include <curves/piecewise_curve.h>
#include <curves/exact_cubic.h>
#include <curves/cubic_hermite_spline.h>
/**
* This unit test try to deserialize the ContactSequences in the examples/previous_versions folder
* And check that they are compatible with the current version
*/
using namespace multicontact_api::scenario;
const std::string path = TEST_DATA_PATH;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(api_0) {
ContactSequence cs;
cs.loadFromBinary(path + "previous_versions/api_0.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
BOOST_CHECK(cs.haveJointsTrajectories());
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(!cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_CASE(api_1) {
ContactSequence cs;
cs.loadFromBinary(path + "previous_versions/api_1.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
BOOST_CHECK(cs.haveJointsTrajectories());
BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
BOOST_CHECK(cs.haveContactForcesTrajectories());
BOOST_CHECK(cs.haveZMPtrajectories());
BOOST_CHECK(cs.haveFriction());
BOOST_CHECK(cs.haveContactModelDefined());
}
BOOST_AUTO_TEST_SUITE_END()