Unverified Commit d6e897ac authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #12 from loco-3d/devel

Devel
parents d217188f 8c81b3e9
Pipeline #9968 passed with stage
in 11 minutes and 22 seconds
......@@ -10,19 +10,16 @@
#include "multicontact-api/serialization/spatial.hpp"
#include <curves/fwd.h>
#include <curves/curve_abc.h>
#include <curves/piecewise_curve.h>
#include <curves/serialization/curves.hpp>
#include <map>
#include <vector>
#include <set>
#include <string>
#include <sstream>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <curves/serialization/registeration.hpp>
namespace multicontact_api {
namespace scenario {
......@@ -45,9 +42,9 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
typedef curves::curve_abc_t curve_t;
// typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
typedef curves::curve_SE3_t curve_SE3_t;
typedef boost::shared_ptr<curve_t> curve_ptr;
typedef curves::curve_ptr_t curve_ptr;
// typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr;
typedef curves::curve_SE3_ptr_t curve_SE3_ptr;
typedef curves::piecewise3_t piecewise3_t;
typedef curves::piecewise_t piecewise_t;
typedef piecewise_t::t_time_t t_time_t;
......
......@@ -415,6 +415,18 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "CoM acceleration trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_c->dim() != 3) {
std::cout << "CoM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_dc->dim() != 3) {
std::cout << "CoM velocity trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_ddc->dim() != 3) {
std::cout << "CoM acceleration trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_c->min() != phase.timeInitial()) {
std::cout << "CoM trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......@@ -508,6 +520,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "AM velocity trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_L->dim() != 3) {
std::cout << "AM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_dL->dim() != 3) {
std::cout << "AM derivative trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_L->min() != phase.timeInitial()) {
std::cout << "AM trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......@@ -582,7 +602,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* placement.
* @return
*/
bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
const bool use_rotation = true) const {
if (!haveTimings()) return false;
for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
for (std::string eeName : m_contact_phases.at(i).getContactsCreated(m_contact_phases.at(i + 1))) {
......@@ -601,7 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return false;
}
ContactPatch::SE3 pMax = ContactPatch::SE3((*traj)(traj->max()).matrix());
if (!pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec)) {
if ((use_rotation &&
!pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec)) ||
(!pMax.translation().isApprox(
m_contact_phases.at(i + 1).contactPatches().at(eeName).placement().translation(), prec))) {
std::cout << "Effector trajectory for " << eeName
<< " do not end at it's contact placement in the next phase, for phase " << i << std::endl;
std::cout << "Last point : " << std::endl
......@@ -612,7 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
}
if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
ContactPatch::SE3 pMin = ContactPatch::SE3((*traj)(traj->min()).matrix());
if (!pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec)) {
if ((use_rotation &&
!pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec)) ||
(!pMin.translation().isApprox(
m_contact_phases.at(i - 1).contactPatches().at(eeName).placement().translation(), prec))) {
std::cout << "Effector trajectory for " << eeName
<< " do not start at it's contact placement in the previous phase, for phase " << i << std::endl;
std::cout << "First point : " << std::endl
......@@ -818,6 +845,24 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return true;
}
/**
* @brief haveContactModelDefined check that all the contact patch have a contact_model defined
* @return
*/
bool haveContactModelDefined() const {
size_t i = 0;
for (const ContactPhase& phase : m_contact_phases) {
for (const std::string& eeName : phase.effectorsInContact()) {
if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type == ContactType::CONTACT_UNDEFINED) {
std::cout << "ContactModel not defined for phase " << i << " and effector " << eeName << std::endl;
return false;
}
}
++i;
}
return true;
}
/**
* @brief haveZMPtrajectories check that all the contact phases have a zmp trajectory
* @return
......@@ -829,6 +874,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "ZMP trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_zmp->dim() != 3) {
std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_zmp->min() != phase.timeInitial()) {
std::cout << "ZMP trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......@@ -1025,7 +1074,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
*/
piecewise_SE3_t concatenateEffectorTrajectories(const std::string& eeName) const {
piecewise_SE3_t res = piecewise_SE3_t();
transform_t last_placement;
transform_t last_placement, first_placement;
// first find the first and last phase with a trajectory for this effector
size_t first_phase = m_contact_phases.size();
size_t last_phase = 0;
......@@ -1034,9 +1083,20 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
last_phase = i;
if (first_phase > i) {
first_phase = i;
curve_SE3_ptr curve = m_contact_phases.at(i).effectorTrajectories().at(eeName);
first_placement = curve->operator()(curve->min());
}
}
}
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()));
res.add_curve_ptr(ptr_init);
}
// loop over this phases to concatenate the trajectories
for (size_t i = first_phase; i <= last_phase; ++i) {
if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
......@@ -1049,6 +1109,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
res.add_curve_ptr(ptr);
}
}
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()));
res.add_curve_ptr(ptr_final);
}
return res;
}
......
......@@ -26,9 +26,10 @@ struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase> ContactSequence;
template <typename Scalar>
struct ContactModelPlanarTpl;
typedef ContactModelPlanarTpl<double> ContactModelPlanar;
struct ContactModelTpl;
typedef ContactModelTpl<double> ContactModel;
enum ContactType { CONTACT_UNDEFINED, CONTACT_PLANAR, CONTACT_POINT };
enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
} // namespace scenario
......
# Copyright (c) 2015-2018, CNRS
# Authors: Justin Carpentier <jcarpent@laas.fr>
# Copyright (c) 2015-2020, CNRS
# Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel
ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK)
......@@ -11,11 +11,9 @@ SET(${PROJECT_NAME}_TESTS
FOREACH(TEST ${${PROJECT_NAME}_TESTS})
ADD_UNIT_TEST(${TEST} ${TEST})
TARGET_LINK_LIBRARIES(${TEST} ${Boost_LIBRARIES})
PKG_CONFIG_USE_DEPENDENCY(${TEST} eigen3)
PKG_CONFIG_USE_DEPENDENCY(${TEST} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${TEST} curves)
TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} ${Boost_LIBRARIES})
ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS})
TARGET_COMPILE_DEFINITIONS(examples PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
IF(BUILD_PYTHON_INTERFACE)
......
......@@ -80,6 +80,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false));
}
BOOST_AUTO_TEST_CASE(step_in_place_WB) {
......
......@@ -9,9 +9,9 @@ from curves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial
from numpy import array, array_equal, isclose, random
import pinocchio as pin
from multicontact_api import ContactModelPlanar, ContactPatch, ContactPhase, ContactSequence
from multicontact_api import ContactModel, ContactPatch, ContactPhase, ContactSequence, ContactType
from pinocchio import SE3, Quaternion
import pickle
pin.switchToNumpyArray()
......@@ -149,57 +149,112 @@ def buildRandomContactPhase(min=-1, max=-1):
class ContactModelTest(unittest.TestCase):
def test_contact_model_planar(self):
def test_contact_model(self):
mu = 0.3
zmp_radius = 0.01
# default constructor
mp = ContactModel()
self.assertEqual(mp.mu, -1.)
self.assertEqual(mp.contact_type, ContactType.CONTACT_UNDEFINED)
self.assertEqual(mp.num_contact_points, 1)
self.assertEqual(len(mp.contact_points_positions.shape), 1)
self.assertEqual(mp.contact_points_positions.shape[0], 3)
self.assertTrue(not mp.contact_points_positions.any())
# constructor with friction
mp_mu = ContactModel(mu)
self.assertEqual(mp_mu.mu, mu)
self.assertEqual(mp_mu.contact_type, ContactType.CONTACT_UNDEFINED)
self.assertEqual(mp.num_contact_points, 1)
self.assertEqual(len(mp.contact_points_positions.shape), 1)
self.assertEqual(mp.contact_points_positions.shape[0], 3)
self.assertTrue(not mp.contact_points_positions.any())
# constructor with both values
mp1 = ContactModelPlanar(mu, zmp_radius)
mp1 = ContactModel(mu, ContactType.CONTACT_PLANAR)
# test getter bindings
self.assertEqual(mp1.mu, mu)
self.assertEqual(mp1.ZMP_radius, zmp_radius)
self.assertEqual(mp1.contact_type, ContactType.CONTACT_PLANAR)
self.assertEqual(mp.num_contact_points, 1)
self.assertEqual(len(mp.contact_points_positions.shape), 1)
self.assertEqual(mp.contact_points_positions.shape[0], 3)
self.assertTrue(not mp.contact_points_positions.any())
# copy constructor :
mp2 = ContactModelPlanar(mp1)
mp2 = ContactModel(mp1)
self.assertEqual(mp2.mu, mu)
self.assertEqual(mp2.ZMP_radius, zmp_radius)
self.assertEqual(mp2.contact_type, ContactType.CONTACT_PLANAR)
self.assertEqual(mp.num_contact_points, 1)
self.assertEqual(len(mp.contact_points_positions.shape), 1)
self.assertEqual(mp.contact_points_positions.shape[0], 3)
self.assertTrue(not mp.contact_points_positions.any())
# test operator ==
self.assertTrue(mp1 == mp2)
mp1.mu = 0.5
self.assertTrue(mp1 != mp2)
def test_contact_model_contact_points(self):
mp1 = ContactModel(0.5, ContactType.CONTACT_PLANAR)
mp1.num_contact_points = 4
self.assertEqual(mp1.num_contact_points, 4)
self.assertEqual(mp1.contact_points_positions.shape[0], 3)
self.assertEqual(mp1.contact_points_positions.shape[1], 4)
self.assertTrue(not mp1.contact_points_positions.any())
pos = np.random.rand(3, 5)
mp1.contact_points_positions = pos
self.assertEqual(mp1.num_contact_points, 5)
self.assertEqual(mp1.contact_points_positions.shape[0], 3)
self.assertEqual(mp1.contact_points_positions.shape[1], 5)
self.assertTrue(isclose(mp1.contact_points_positions, pos).all())
generators = mp1.generatorMatrix()
self.assertEqual(generators.shape[0], 6)
self.assertEqual(generators.shape[1], 5*3)
mp1.num_contact_points = 2
self.assertEqual(mp1.num_contact_points, 2)
self.assertEqual(mp1.contact_points_positions.shape[0], 3)
self.assertEqual(mp1.contact_points_positions.shape[1], 2)
self.assertTrue(not mp1.contact_points_positions.any())
def test_contact_model_serialization_default(self):
mp1 = ContactModelPlanar()
mp1 = ContactModel()
mp1.saveAsText("mp_test.txt")
mp_txt = ContactModelPlanar()
mp_txt = ContactModel()
mp_txt.loadFromText("mp_test.txt")
self.assertEqual(mp1, mp_txt)
mp1.saveAsBinary("mp_test")
mp_bin = ContactModelPlanar()
mp_bin = ContactModel()
mp_bin.loadFromBinary("mp_test")
self.assertEqual(mp1, mp_bin)
mp1.saveAsXML("mp_test.xml", 'ContactModel')
mp_xml = ContactModelPlanar()
mp_xml = ContactModel()
mp_xml.loadFromXML("mp_test.xml", 'ContactPatch')
self.assertEqual(mp1, mp_xml)
mp_pickled = pickle.dumps(mp1)
mp_from_pickle = pickle.loads(mp_pickled)
self.assertEqual(mp1, mp_from_pickle)
def test_contact_model_serialization_full(self):
mu = 0.3
zmp_radius = 0.01
# constructor with both values
mp1 = ContactModelPlanar(mu, zmp_radius)
mp1 = ContactModel(mu, ContactType.CONTACT_PLANAR)
mp1.saveAsText("mp_test.txt")
mp_txt = ContactModelPlanar()
mp_txt = ContactModel()
mp_txt.loadFromText("mp_test.txt")
self.assertEqual(mp1, mp_txt)
mp1.saveAsBinary("mp_test")
mp_bin = ContactModelPlanar()
mp_bin = ContactModel()
mp_bin.loadFromBinary("mp_test")
self.assertEqual(mp1, mp_bin)
mp1.saveAsXML("mp_test.xml", 'ContactModel')
mp_xml = ContactModelPlanar()
mp_xml = ContactModel()
mp_xml.loadFromXML("mp_test.xml", 'ContactPatch')
self.assertEqual(mp1, mp_xml)
mp_pickled = pickle.dumps(mp1)
mp_from_pickle = pickle.loads(mp_pickled)
self.assertEqual(mp1, mp_from_pickle)
class ContactPatchTest(unittest.TestCase):
......@@ -278,6 +333,9 @@ class ContactPatchTest(unittest.TestCase):
cp_xml = ContactPatch()
cp_xml.loadFromXML("cp_test.xml", 'ContactPatch')
self.assertEqual(cp1, cp_xml)
cp_pickled = pickle.dumps(cp1)
cp_from_pickle = pickle.loads(cp_pickled)
self.assertEqual(cp1, cp_from_pickle)
def test_serialization_full(self):
p = SE3()
......@@ -295,6 +353,32 @@ class ContactPatchTest(unittest.TestCase):
cp_xml = ContactPatch()
cp_xml.loadFromXML("cp_test.xml", 'ContactPatch')
self.assertEqual(cp1, cp_xml)
cp_pickled = pickle.dumps(cp1)
cp_from_pickle = pickle.loads(cp_pickled)
self.assertEqual(cp1, cp_from_pickle)
def test_contact_patch_model_accessor(self):
p = SE3()
p.setRandom()
cp1 = ContactPatch(p, 0.9)
cm = cp1.contact_model
self.assertEqual(cm.mu, 0.9)
cm.mu = 0.5
self.assertEqual(cp1.friction, 0.5)
cp1.contact_model.contact_type = ContactType.CONTACT_PLANAR
self.assertEqual(cp1.contact_model.contact_type, ContactType.CONTACT_PLANAR)
cp1.friction = 2
self.assertEqual(cp1.contact_model.mu, 2)
self.assertEqual(cm.mu, 2)
pos = np.random.rand(3, 4)
cp1.contact_model.contact_points_positions = pos
self.assertEqual(cp1.contact_model.num_contact_points, 4)
self.assertEqual(cp1.contact_model.contact_points_positions.shape[0], 3)
self.assertEqual(cp1.contact_model.contact_points_positions.shape[1], 4)
self.assertTrue(isclose(cp1.contact_model.contact_points_positions, pos).all())
class ContactPhaseTest(unittest.TestCase):
......@@ -1144,6 +1228,9 @@ class ContactPhaseTest(unittest.TestCase):
cp_xml = ContactPhase()
cp_xml.loadFromXML("cp_test.xml", 'ContactPhase')
self.assertEqual(cp1, cp_xml)
cp_pickled = pickle.dumps(cp1)
cp_from_pickle = pickle.loads(cp_pickled)
self.assertEqual(cp1, cp_from_pickle)
def test_contact_phase_serialization_full(self):
cp1 = buildRandomContactPhase(0., 2.)
......@@ -1160,6 +1247,9 @@ class ContactPhaseTest(unittest.TestCase):
cp_xml.loadFromXML("cp_test_full.xml", 'ContactPhase')
self.assertEqual(cp1, cp_xml)
# TODO : check serialization from another file
cp_pickled = pickle.dumps(cp1)
cp_from_pickle = pickle.loads(cp_pickled)
self.assertEqual(cp1, cp_from_pickle)
def test_contact_phase_contacts_variation(self):
# # contacts repositioned :
......@@ -1591,6 +1681,41 @@ class ContactSequenceTest(unittest.TestCase):
consistent = cs4.haveTimings()
self.assertFalse(consistent)
def test_contact_sequence_have_contact_model(self):
cs1 = ContactSequence(0)
cp0 = buildRandomContactPhase(0, 2)
cp1 = buildRandomContactPhase(2, 4.)
cs1.append(cp0)
cs1.append(cp1)
self.assertFalse(cs1.haveContactModelDefined())
mp1 = ContactModel(0.5, ContactType.CONTACT_PLANAR)
pos = np.random.rand(3, 5)
mp1.contact_points_positions = pos
mp2 = ContactModel(1., ContactType.CONTACT_POINT)
pos = np.random.rand(3, 5)
mp1.contact_points_positions = pos
cs1.contactPhases[0].contactPatch("right-leg").contact_model = mp1
cs1.contactPhases[0].contactPatch("left-leg").contact_model = mp2
cs1.contactPhases[1].contactPatch("right-leg").contact_model = mp1
cs1.contactPhases[1].contactPatch("left-leg").contact_model = mp2
self.assertTrue(cs1.haveContactModelDefined())
cp2 = buildRandomContactPhase(6., 8.)
cs1.append(cp2)
self.assertFalse(cs1.haveContactModelDefined())
mp3 = ContactModel(0.2)
cs1.contactPhases[2].contactPatch("right-leg").contact_model = mp3
cs1.contactPhases[2].contactPatch("left-leg").contact_model = mp2
self.assertFalse(cs1.haveContactModelDefined())
mp3.contact_type = ContactType.CONTACT_PLANAR # do not change the contact model already in the seqence
self.assertFalse(cs1.haveContactModelDefined())
cs1.contactPhases[2].contactPatch("right-leg").contact_model.contact_type = ContactType.CONTACT_PLANAR
self.assertTrue(cs1.haveContactModelDefined())
def test_contact_sequence_concatenate_config_traj(self):
cs1 = ContactSequence(0)
cp0 = buildRandomContactPhase(0, 2)
......@@ -1741,6 +1866,15 @@ class ContactSequenceTest(unittest.TestCase):
with self.assertRaises(ValueError):
cs1.phaseAtTime(10.)
def test_pickle_contact_sequence(self):
cs = ContactSequence()
for i in range(10):
cp = buildRandomContactPhase(0., 2.)
cs.append(cp)
cs_pickled = pickle.dumps(cs)
cs_from_pickle = pickle.loads(cs_pickled)
self.assertEqual(cs_from_pickle, cs)
if __name__ == '__main__':
unittest.main()
......@@ -149,6 +149,7 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_step_in_place_WB(self):
......
......@@ -6,7 +6,7 @@ import multicontact_api
class TrivialTest(unittest.TestCase):
""" A test written by someone who has no idea what this software is about"""
def test_trivial(self):
comopla = multicontact_api.ContactModelPlanar()
comopla = multicontact_api.ContactModel()
epsilon = 0.00001
value_wanted = -1.0
self.assertTrue((comopla.mu - value_wanted) < epsilon)
......
// 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>
......@@ -7,19 +7,16 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "multicontact-api/scenario/contact-model-planar.hpp"
#include "multicontact-api/scenario/contact-model.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-sequence.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>
typedef Eigen::Matrix<double, 1, 1> point1_t;
using curves::point3_t;
......@@ -36,21 +33,9 @@ using curves::t_point3_t;
using curves::t_pointX_t;
using namespace multicontact_api::scenario;
typedef ContactSequence::ContactPhaseVector ContactPhaseVector;
typedef ContactModel::Matrix3X Matrix3X;
typedef ContactModel::Matrix6X Matrix6X;
template <typename Scalar>
struct ATpl {
typedef pinocchio::SE3Tpl<Scalar> SE3;
explicit ATpl() : data() {}
explicit ATpl(const ATpl& other) : data(other.data){};
bool operator==(const ATpl& other) { return data == other.data; }
protected:
SE3 data;
};
typedef ATpl<double> Ad;
typedef pinocchio::SE3Tpl<double> SE3;
curve_ptr_t buildPiecewisePolynomialC2() {
......@@ -356,19 +341,107 @@ void explicitContactPhaseAssertEqual(ContactPhase& cp1, ContactPhase& cp2) {
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(contact_model) {
const double mu = 0.3;
const double ZMP_radius = 0.01;
ContactModel mp;
BOOST_CHECK(mp.m_mu == -1.);
BOOST_CHECK(mp.m_contact_type == ContactType::CONTACT_UNDEFINED);
BOOST_CHECK(mp.num_contact_points() == 1);
BOOST_CHECK(mp.contact_points_positions().cols() == 1);
BOOST_CHECK(mp.contact_points_positions().isZero());
ContactModelPlanar mp1(mu, ZMP_radius);
ContactModelPlanar mp2(mp1);
const double mu = 0.3;
ContactModel mp_mu(mu);
BOOST_CHECK(mp_mu.m_mu == mu);
BOOST_CHECK(mp_mu.m_contact_type == ContactType::CONTACT_UNDEFINED);
BOOST_CHECK(mp_mu.num_contact_points() == 1);
BOOST_CHECK(mp_mu.contact_points_positions().cols() == 1);
BOOST_CHECK(mp_mu.contact_points_positions().isZero());
ContactModel mp1(mu, ContactType::CONTACT_PLANAR);
BOOST_CHECK(mp1.m_mu == mu);
BOOST_CHECK(mp1.m_ZMP_radius == ZMP_radius);
BOOST_CHECK(mp1.m_contact_type == ContactType::CONTACT_PLANAR);
BOOST_CHECK(mp1.num_contact_points() == 1);
BOOST_CHECK(mp1.contact_points_positions().cols() == 1);
BOOST_CHECK(mp1.contact_points_positions().isZero());
ContactModel mp2(mp1);
BOOST_CHECK(mp2.m_mu == mu);
BOOST_CHECK(mp2.m_contact_type == ContactType::CONTACT_PLANAR);
BOOST_CHECK(mp2.num_contact_points() == 1);
BOOST_CHECK(mp2.contact_points_positions().cols() == 1);
BOOST_CHECK(mp2.contact_points_positions().isZero());
}
BOOST_AUTO_TEST_CASE(contact_model_points_positions) {
const double mu = 0.3;
ContactModel mp(mu, ContactType::CONTACT_PLANAR);