Skip to content

GitLab

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

Merge branch 'devel' into topic/fix_checks

parents 3ea4d75a 026241bc
Show whitespace changes
Inline Side-by-side
include/multicontact-api/scenario/fwd.hpp
View file @ 506ad2f2
......@@ -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
......
unittest/CMakeLists.txt
View file @ 506ad2f2
# 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)
......
unittest/python/scenario.py
View file @ 506ad2f2
......@@ -9,7 +9,7 @@ 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,37 +149,87 @@ 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)
......@@ -188,19 +238,18 @@ class ContactModelTest(unittest.TestCase):
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)
......@@ -308,6 +357,29 @@ class ContactPatchTest(unittest.TestCase):
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):
def test_default_constructor(self):
......@@ -1609,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)
......
unittest/python/trivial.py
View file @ 506ad2f2
......@@ -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)
......
unittest/scenario.cpp
View file @ 506ad2f2
......@@ -7,7 +7,7 @@
#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"
......@@ -33,6 +33,8 @@ 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;
typedef pinocchio::SE3Tpl<double> SE3;
......@@ -339,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);
mp.num_contact_points(4);
BOOST_CHECK_EQUAL(mp.num_contact_points(), 4);
BOOST_CHECK_EQUAL(mp.contact_points_positions().cols(), 4);
BOOST_CHECK(mp.contact_points_positions().isZero());
Matrix3X positions = Matrix3X::Random(3, 6);
mp.contact_points_positions(positions);
BOOST_CHECK_EQUAL(mp.num_contact_points(), 6);
BOOST_CHECK_EQUAL(mp.contact_points_positions().cols(), 6);
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);
mp.num_contact_points(2);
BOOST_CHECK_EQUAL(mp.num_contact_points(), 2);
BOOST_CHECK_EQUAL(mp.contact_points_positions().cols(), 2);
BOOST_CHECK(mp.contact_points_positions().isZero());
}
BOOST_AUTO_TEST_CASE(contact_model_operator_equal) {
ContactModel mp1(0.3, ContactType::CONTACT_PLANAR);
Matrix3X positions = Matrix3X::Random(3, 4);
mp1.contact_points_positions(positions);
ContactModel mp2(0.3, ContactType::CONTACT_PLANAR);
mp2.contact_points_positions(positions);
ContactModel mp3(mp1);
BOOST_CHECK(mp1 == mp2);
mp1.m_mu = 0.5;
BOOST_CHECK(mp1 != mp2);
BOOST_CHECK(mp1 == mp3);
ContactModel mp_n1(0.3, ContactType::CONTACT_PLANAR);
ContactModel mp_n2(1., ContactType::CONTACT_PLANAR);
mp1.contact_points_positions(positions);
ContactModel mp_n3(0.3, ContactType::CONTACT_UNDEFINED);
mp1.contact_points_positions(positions);
ContactModel mp_n4(0.3, ContactType::CONTACT_PLANAR);
mp_n4.num_contact_points(4);
BOOST_CHECK(mp1 != mp_n1);
BOOST_CHECK(mp1 != mp_n2);
BOOST_CHECK(mp1 != mp_n3);
BOOST_CHECK(mp1 != mp_n4);
}
// TODO : check serialization
BOOST_AUTO_TEST_CASE(contact_model_serialization) {
ContactModel mp1(0.3, ContactType::CONTACT_PLANAR);
Matrix3X positions = Matrix3X::Random(3, 4);
mp1.contact_points_positions(positions);
std::string fileName("fileTest_contactModel");
mp1.saveAsText(fileName + ".txt");
ContactModel mp_from_text;
mp_from_text.loadFromText(fileName + ".txt");
BOOST_CHECK(mp1 == mp_from_text);
mp1.saveAsXML(fileName + ".xml", "ContactModel");
ContactModel mp_from_xml;
mp_from_xml.loadFromXML(fileName + ".xml", "ContactModel");
BOOST_CHECK(mp1 == mp_from_xml);
mp1.saveAsBinary(fileName);
ContactModel mp_from_bin;
mp_from_bin.loadFromBinary(fileName);
BOOST_CHECK(mp1 == mp_from_bin);
}
BOOST_AUTO_TEST_CASE(contact_patch) {
......@@ -399,7 +489,16 @@ BOOST_AUTO_TEST_CASE(contact_patch) {
cp5.friction() = 2.;
BOOST_CHECK(cp4 != cp5);
ContactPatch cp6(cp4);
BOOST_CHECK(cp4 == cp6);
ContactModel mp1(cp4.friction(), ContactType::CONTACT_PLANAR);
Matrix3X positions = Matrix3X::Random(3, 4);
mp1.contact_points_positions(positions);
cp6.m_contact_model = mp1;
BOOST_CHECK(cp4 != cp6);
// serialization :
cp3.m_contact_model = mp1;
std::string fileName("fileTest");
cp3.saveAsText(fileName);
ContactPatch cp_from_text;
......@@ -1634,6 +1733,44 @@ BOOST_AUTO_TEST_CASE(contact_sequence_is_time_consistent) {
BOOST_CHECK(!consistent);
}
BOOST_AUTO_TEST_CASE(contact_sequence_have_contact_model_defined) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = buildRandomContactPhase(0, 2);
ContactPhase cp1 = buildRandomContactPhase(2, 4.);
ContactPhase cp2 = buildRandomContactPhase(0, 2);
cs1.append(cp0);
cs1.append(cp1);
// 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);
mp1.contact_points_positions(positions);
ContactModel mp2(0.5, ContactType::CONTACT_POINT);
Matrix3X positions2 = Matrix3X::Random(3, 1);
mp2.contact_points_positions(positions2);
cs1.contactPhase(0).contactPatch("right_hand").m_contact_model = mp1;
cs1.contactPhase(0).contactPatch("left_foot").m_contact_model = mp2;
cs1.contactPhase(1).contactPatch("right_hand").m_contact_model = mp1;
cs1.contactPhase(1).contactPatch("left_foot").m_contact_model = mp2;
BOOST_CHECK(cs1.haveContactModelDefined());
cs1.append(cp2);
BOOST_CHECK(!cs1.haveContactModelDefined());
ContactModel mp3(0.3);
cs1.contactPhase(2).contactPatch("right_hand").m_contact_model = mp3;
cs1.contactPhase(2).contactPatch("left_foot").m_contact_model = mp2;
BOOST_CHECK(!cs1.haveContactModelDefined());
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);
......@@ -1722,6 +1859,29 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_effector_traj) {
BOOST_CHECK(traj(8.).isApprox(traj_2->operator()(8.)));
BOOST_CHECK(traj(2.5).isApprox(traj_0->operator()(2.)));
BOOST_CHECK(traj(3.8).isApprox(traj_0->operator()(2.)));
BOOST_CHECK_THROW(cs1.concatenateEffectorTrajectories("test"), std::invalid_argument);
ContactPhase cp3 = ContactPhase(8, 12.);
cs1.append(cp3);
traj = cs1.concatenateEffectorTrajectories("right_leg");
BOOST_CHECK(traj.min() == 0.);
BOOST_CHECK(traj.max() == 12.);
ContactPhase cpm1 = ContactPhase(-2., 0.);
ContactSequence cs2 = ContactSequence(0);
cs2.append(cpm1);
cs2.append(cp0);
cs2.append(cp1);
cs2.append(cp2);
traj = cs2.concatenateEffectorTrajectories("right_leg");
BOOST_CHECK(traj.min() == -2.);
BOOST_CHECK(traj.max() == 8.);
cs2.append(cp3);
traj = cs2.concatenateEffectorTrajectories("right_leg");
BOOST_CHECK(traj.min() == -2.);
BOOST_CHECK(traj.max() == 12.);
}
BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_force_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