diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp index c9e5294b8a..5331150cd0 100644 --- a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp +++ b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp @@ -15,10 +15,10 @@ namespace python { void exposeContact1D() { bp::register_ptr_to_python >(); - bp::enum_("Contact1DMaskType") - .value("X_MASK", X_MASK) - .value("Y_MASK", Y_MASK) - .value("Z_MASK", Z_MASK) + bp::enum_("Vector3MaskType") + .value("x", x) + .value("y", y) + .value("z", z) .export_values(); bp::class_ >( @@ -29,7 +29,7 @@ void exposeContact1D() { "The calc and calcDiff functions compute the contact Jacobian and drift (holonomic constraint) or\n" "the derivatives of the holonomic constraint, respectively.", bp::init, pinocchio::FrameIndex, double, std::size_t, - bp::optional >( + bp::optional >( bp::args("self", "state", "id", "xref", "nu", "gains", "mask", "type"), "Initialize the contact model.\n\n" ":param state: state of the multibody system\n" @@ -37,7 +37,7 @@ void exposeContact1D() { ":param xref: contact position used for the Baumgarte stabilization\n" ":param nu: dimension of control vector\n" ":param gains: gains of the contact model (default np.matrix([0.,0.]))\n" - ":param mask: axis of the contact constraint (0=x, 1=y or 2=z)\n" + ":param mask: axis of the contact constraint (default z)\n" ":param type: reference type of contact")) .def(bp::init, pinocchio::FrameIndex, double, bp::optional >( diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hpp b/include/crocoddyl/multibody/contacts/contact-1d.hpp index 17b5d2dbb8..d64b3db564 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hpp +++ b/include/crocoddyl/multibody/contacts/contact-1d.hpp @@ -22,7 +22,7 @@ namespace crocoddyl { -enum Contact1DMaskType { X_MASK = 0, Y_MASK = 1, Z_MASK = 2 }; +enum Vector3MaskType { x = 0, y = 1, z = 2 }; template class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { @@ -39,7 +39,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { typedef typename MathBase::Vector3s Vector3s; typedef typename MathBase::VectorXs VectorXs; typedef typename MathBase::Matrix3s Matrix3s; - + typedef typename MathBase::Matrix6s Matrix6s; /** * @brief Initialize the 1d contact model * @@ -48,12 +48,12 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { * @param[in] xref Contact position used for the Baumgarte stabilization * @param[in] nu Dimension of the control vector * @param[in] gains Baumgarte stabilization gains - * @param[in] mask Constraint 1D axis + * @param[in] mask Constraint 1D axis (default z) * @param[in] ref Reference type of contact */ ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Scalar xref, const std::size_t nu, const Vector2s& gains = Vector2s::Zero(), - const Contact1DMaskType& mask = Z_MASK, const pinocchio::ReferenceFrame ref = pinocchio::LOCAL); + const Vector3MaskType& mask = Vector3MaskType::z, const pinocchio::ReferenceFrame ref = pinocchio::LOCAL); /** * @brief Initialize the 1d contact model @@ -130,12 +130,12 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { /** * @brief Modify contact 1D mask */ - void set_mask(const Contact1DMaskType mask); + void set_mask(const Vector3MaskType mask); /** * @brief Get contact 1D mask */ - const Contact1DMaskType get_mask() const; + const Vector3MaskType get_mask() const; /** * @brief Print relevant information of the 1d contact model @@ -153,7 +153,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { private: Scalar xref_; //!< Contact position used for the Baumgarte stabilization Vector2s gains_; //!< Baumgarte stabilization gains - Contact1DMaskType mask_; //!< Axis of the 1D contact in (x,y,z) + Vector3MaskType mask_; //!< Axis of the 1D contact in (x,y,z) pinocchio::ReferenceFrame type_; //!< Reference type of contact }; @@ -182,7 +182,7 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> { fXjda_dv(6, model->get_state()->get_nv()) { frame = model->get_id(); jMf = model->get_state()->get_pinocchio()->frames[frame].placement; - fXj = jMf.inverse().toActionMatrix(); + fXj = pinocchio::SE3::Identity().toActionMatrix(); //jMf.inverse().toActionMatrix(); fJf.setZero(); v_partial_dq.setZero(); a_partial_dq.setZero(); @@ -196,6 +196,11 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> { vv_skew.setZero(); vw_skew.setZero(); oRf.setZero(); + type = model->get_type(); + mask = model->get_mask(); + jMc_ = pinocchio::SE3::Identity(); + wMlwa_ = pinocchio::SE3::Identity(); + lwaMj_ = pinocchio::SE3::Identity(); } using Base::a0; @@ -224,6 +229,11 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> { Matrix3s vv_skew; Matrix3s vw_skew; Matrix2s oRf; + pinocchio::ReferenceFrame type; + Vector3MaskType mask; + pinocchio::SE3 jMc_; + pinocchio::SE3 wMlwa_; + pinocchio::SE3 lwaMj_; }; } // namespace crocoddyl diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hxx b/include/crocoddyl/multibody/contacts/contact-1d.hxx index 49533b2826..9b53b36b47 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hxx +++ b/include/crocoddyl/multibody/contacts/contact-1d.hxx @@ -11,23 +11,17 @@ namespace crocoddyl { template ContactModel1DTpl::ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Scalar xref, const std::size_t nu, const Vector2s& gains, - const Contact1DMaskType& mask, const pinocchio::ReferenceFrame type) + const Vector3MaskType& mask, const pinocchio::ReferenceFrame type) : Base(state, 1, nu), xref_(xref), gains_(gains), mask_(mask), type_(type) { id_ = id; - if (mask_ < 0 || mask_ > 2) { - throw_pretty("Invalid argument: " - << "Contact1D mask must be in {0,1,2} for {x,y,z}"); - } } template ContactModel1DTpl::ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Scalar xref, const Vector2s& gains, const pinocchio::ReferenceFrame type) - : Base(state, 1), xref_(xref), gains_(gains), type_(type) { + : Base(state, 1), xref_(xref), gains_(gains), type_(type), mask_(Vector3MaskType::z) { id_ = id; - // Default mask is z - mask_ = Z_MASK; } template @@ -69,6 +63,17 @@ void ContactModel1DTpl::calcDiff(const boost::shared_ptrget_nv(); pinocchio::skew(d->vv, d->vv_skew); pinocchio::skew(d->vw, d->vw_skew); + // Matrix6s fXj; + if (type_ == pinocchio::LOCAL){ + d->fXj = d->jMf.inverse().toActionMatrix(); + } + else if (type_ == pinocchio::WORLD) { + d->fXj = d->fXj; + } + else if (type_ == pinocchio::LOCAL_WORLD_ALIGNED) { + d->lwaMj_.translation(d->jMf.inverse().translation()); + d->fXj = d->lwaMj_.toActionMatrix(); + } d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq; d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq; d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv; @@ -100,23 +105,23 @@ void ContactModel1DTpl::updateForce(const boost::shared_ptr(data.get()); - pinocchio::SE3 jMc; switch (type_) { case pinocchio::LOCAL: { - jMc = d->jMf; + d->jMc_ = d->jMf; break; } case pinocchio::WORLD: { - jMc = d->jMf.act(d->pinocchio->oMf[id_].inverse()); + d->jMc_ = d->jMf.act(d->pinocchio->oMf[id_].inverse()); break; } case pinocchio::LOCAL_WORLD_ALIGNED: { - jMc = pinocchio::SE3(Matrix3s::Identity(), d->jMf.translation()); + d->wMlwa_.translation(d->pinocchio->oMf[id_].translation()); + d->jMc_ = d->jMf.act(d->pinocchio->oMf[id_].actInv(d->wMlwa_)); break; } } - data->f.linear() = jMc.rotation().col(mask_) * force[0]; - data->f.angular() = jMc.translation().cross(data->f.linear()); + data->f.linear() = d->jMc_.rotation().col(mask_) * force[0]; + data->f.angular() = d->jMc_.translation().cross(data->f.linear()); } template @@ -156,12 +161,12 @@ const pinocchio::ReferenceFrame ContactModel1DTpl::get_type() const { } template -void ContactModel1DTpl::set_mask(const Contact1DMaskType mask) { +void ContactModel1DTpl::set_mask(const Vector3MaskType mask) { mask_ = mask; } template -const Contact1DMaskType ContactModel1DTpl::get_mask() const { +const Vector3MaskType ContactModel1DTpl::get_mask() const { return mask_; } diff --git a/include/crocoddyl/multibody/residuals/contact-force.hxx b/include/crocoddyl/multibody/residuals/contact-force.hxx index 831e6ccfa3..eee4b95fea 100644 --- a/include/crocoddyl/multibody/residuals/contact-force.hxx +++ b/include/crocoddyl/multibody/residuals/contact-force.hxx @@ -40,9 +40,20 @@ void ResidualModelContactForceTpl::calc(const boost::shared_ptrcontact_type) { - case Contact1D: - data->r = ((d->contact->jMf.actInv(d->contact->f) - fref_).linear()).row(2); + case Contact1D:{ + ContactData1DTpl* d1d = static_cast*>(d->contact.get()); + if(d1d->type == pinocchio::LOCAL){ + data->r = ((d->contact->jMf.actInv(d->contact->f) - fref_).linear()).row(d1d->mask); + } + else if(d1d->type == pinocchio::WORLD) { + data->r = ((d->contact->pinocchio->oMf[id_].act(d->contact->jMf.actInv(d->contact->f)) - fref_).linear()).row(d1d->mask); + } + else if(d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) { + d1d->wMlwa_.translation_impl(d->contact->pinocchio->oMf[id_].translation()); + data->r = (( d1d->wMlwa_.actInv( d->contact->pinocchio->oMf[id_].act(d->contact->jMf.inverse()) ).act(d->contact->f) - fref_).linear()).row(d1d->mask); + } break; + } case Contact3D: data->r = (d->contact->jMf.actInv(d->contact->f) - fref_).linear(); break; diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 76982cad52..453de3bca8 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -28,6 +28,7 @@ SET(${PROJECT_NAME}_CPP_TESTS test_costs_collision test_cost_sum test_contacts + test_contact1d test_controls test_impulses test_multiple_contacts diff --git a/unittest/factory/contact1d.cpp b/unittest/factory/contact1d.cpp new file mode 100644 index 0000000000..8596ee4b69 --- /dev/null +++ b/unittest/factory/contact1d.cpp @@ -0,0 +1,141 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2021, University of Edinburgh +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#include "contact1d.hpp" +#include "crocoddyl/multibody/contacts/contact-1d.hpp" +#include "crocoddyl/core/utils/exception.hpp" + +namespace crocoddyl { +namespace unittest { + +const std::vector ContactModelMaskTypes::all(ContactModelMaskTypes::init_all()); + +const std::vector PinocchioReferenceTypes::all(PinocchioReferenceTypes::init_all()); + +std::ostream& operator<<(std::ostream& os, const ContactModelMaskTypes::Type& type) { + switch (type) { + case ContactModelMaskTypes::X: + os << "X"; + break; + case ContactModelMaskTypes::Y: + os << "Y"; + break; + case ContactModelMaskTypes::Z: + os << "Z"; + break; + case ContactModelMaskTypes::NbMaskTypes: + os << "NbMaskTypes"; + break; + default: + os << "Unknown type"; + break; + } + return os; +} + +std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type& type) { + switch (type) { + case PinocchioReferenceTypes::LOCAL: + os << "LOCAL"; + break; + case PinocchioReferenceTypes::WORLD: + os << "WORLD"; + break; + case PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED: + os << "LOCAL_WORLD_ALIGNED"; + break; + case PinocchioReferenceTypes::NbPinRefTypes: + os << "NbPinRefTypes"; + break; + default: + os << "Unknown type"; + break; + } + return os; +} + +ContactModel1DFactory::ContactModel1DFactory() {} +ContactModel1DFactory::~ContactModel1DFactory() {} + +boost::shared_ptr ContactModel1DFactory::create(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type, + const std::string frame_name, + std::size_t nu) const { + PinocchioModelFactory model_factory(model_type); + boost::shared_ptr state = + boost::make_shared(model_factory.create()); + boost::shared_ptr contact; + std::size_t frame_id = 0; + if (frame_name == "") { + frame_id = model_factory.get_frame_id(); + } else { + frame_id = state->get_pinocchio()->getFrameId(frame_name); + } + if (nu == std::numeric_limits::max()) { + nu = state->get_nv(); + } + + Eigen::Vector2d gains = Eigen::Vector2d::Zero(); + switch (mask_type) { + case ContactModelMaskTypes::X: { + if(reference_type == PinocchioReferenceTypes::LOCAL){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL); + } else if(reference_type == PinocchioReferenceTypes::WORLD){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::WORLD); + } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL_WORLD_ALIGNED); + } + break; + } + case ContactModelMaskTypes::Y: { + if(reference_type == PinocchioReferenceTypes::LOCAL){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL); + } else if(reference_type == PinocchioReferenceTypes::WORLD){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::WORLD); + } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL_WORLD_ALIGNED); + } + break; + } + case ContactModelMaskTypes::Z: + if(reference_type == PinocchioReferenceTypes::LOCAL){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL); + } else if(reference_type == PinocchioReferenceTypes::WORLD){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::WORLD); + } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){ + contact = boost::make_shared(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL_WORLD_ALIGNED); + } + break; + default: + throw_pretty(__FILE__ ": Wrong ContactModelMaskTypes::Type given"); + break; + } + return contact; +} + +boost::shared_ptr create_random_contact1d() { + static bool once = true; + if (once) { + srand((unsigned)time(NULL)); + once = false; + } + boost::shared_ptr contact; + ContactModel1DFactory factory; + if (rand() % 3 == 0) { + contact = factory.create(ContactModelMaskTypes::X, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + } else if (rand() % 3 == 1) { + contact = factory.create(ContactModelMaskTypes::Y, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + } else { + contact = factory.create(ContactModelMaskTypes::Z, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + } + return contact; +} + +} // namespace unittest +} // namespace crocoddyl diff --git a/unittest/factory/contact1d.hpp b/unittest/factory/contact1d.hpp new file mode 100644 index 0000000000..c56915b1b9 --- /dev/null +++ b/unittest/factory/contact1d.hpp @@ -0,0 +1,75 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2021, University of Edinburgh +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#ifndef CROCODDYL_CONTACT_1D_FACTORY_HPP_ +#define CROCODDYL_CONTACT_1D_FACTORY_HPP_ + +#include +#include + +#include "state.hpp" +#include "crocoddyl/multibody/contact-base.hpp" +#include "crocoddyl/multibody/numdiff/contact.hpp" +#include "crocoddyl/multibody/contacts/multiple-contacts.hpp" + +namespace crocoddyl { +namespace unittest { + + +struct ContactModelMaskTypes { + enum Type {X, Y, Z, NbMaskTypes}; + static std::vector init_all() { + std::vector v; + v.clear(); + for (int i = 0; i < NbMaskTypes; ++i) { + v.push_back((Type)i); + } + return v; + } + static const std::vector all; +}; + +struct PinocchioReferenceTypes { + enum Type { LOCAL, WORLD, LOCAL_WORLD_ALIGNED, NbPinRefTypes}; + static std::vector init_all() { + std::vector v; + v.clear(); + for (int i = 0; i < NbPinRefTypes; ++i) { + v.push_back((Type)i); + } + return v; + } + static const std::vector all; +}; + + +std::ostream& operator<<(std::ostream& os, const ContactModelMaskTypes::Type& type); + +std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type& type); + + +class ContactModel1DFactory { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit ContactModel1DFactory(); + ~ContactModel1DFactory(); + + boost::shared_ptr create( + ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type, + const std::string frame_name = std::string(""), + const std::size_t nu = std::numeric_limits::max()) const; +}; + +boost::shared_ptr create_random_contact1d(); + +} // namespace unittest +} // namespace crocoddyl + +#endif // CROCODDYL_CONTACT_1D_FACTORY_HPP_ diff --git a/unittest/test_contact1d.cpp b/unittest/test_contact1d.cpp new file mode 100644 index 0000000000..8e88a70b1d --- /dev/null +++ b/unittest/test_contact1d.cpp @@ -0,0 +1,228 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2019-2021, University of Edinburgh +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#define BOOST_TEST_NO_MAIN +#define BOOST_TEST_ALTERNATIVE_INIT_API + +#include +#include + +#include +#include + +#include "crocoddyl/multibody/contacts/contact-1d.hpp" + +#include "factory/contact1d.hpp" +#include "unittest_common.hpp" + +using namespace crocoddyl::unittest; +using namespace boost::unit_test; + +//----------------------------------------------------------------------------// + +void test_construct_data(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // Run the print function + std::ostringstream tmp; + tmp << *model; + + // create the corresponding data object + const boost::shared_ptr& pinocchio_model = model->get_state()->get_pinocchio(); + pinocchio::Data pinocchio_data(*pinocchio_model.get()); + boost::shared_ptr data = model->createData(&pinocchio_data); +} + +void test_calc_fetch_jacobians(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // create the corresponding data object + const boost::shared_ptr& pinocchio_model = model->get_state()->get_pinocchio(); + pinocchio::Data pinocchio_data(*pinocchio_model.get()); + boost::shared_ptr data = model->createData(&pinocchio_data); + + // Compute the jacobian and check that the impulse model fetch it. + Eigen::VectorXd x = model->get_state()->rand(); + crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x); + + // Getting the jacobian from the model + model->calc(data, x); + + // Check that only the Jacobian has been filled + BOOST_CHECK(!data->Jc.isZero()); + BOOST_CHECK(!data->a0.isZero()); + BOOST_CHECK(data->da0_dx.isZero()); + BOOST_CHECK(data->f.toVector().isZero()); + BOOST_CHECK(data->df_dx.isZero()); + BOOST_CHECK(data->df_du.isZero()); +} + +void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // create the corresponding data object + const boost::shared_ptr& pinocchio_model = model->get_state()->get_pinocchio(); + pinocchio::Data pinocchio_data(*pinocchio_model.get()); + boost::shared_ptr data = model->createData(&pinocchio_data); + + // Compute the jacobian and check that the impulse model fetch it. + Eigen::VectorXd x = model->get_state()->rand(); + crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x); + + // Getting the jacobian from the model + model->calc(data, x); + model->calcDiff(data, x); + + // Check that nothing has been computed and that all value are initialized to 0 + BOOST_CHECK(!data->Jc.isZero()); + BOOST_CHECK(!data->a0.isZero()); + BOOST_CHECK(!data->da0_dx.isZero()); + BOOST_CHECK(data->f.toVector().isZero()); + BOOST_CHECK(data->df_dx.isZero()); + BOOST_CHECK(data->df_du.isZero()); +} + +void test_update_force(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // create the corresponding data object + const boost::shared_ptr& pinocchio_model = model->get_state()->get_pinocchio(); + pinocchio::Data pinocchio_data(*pinocchio_model.get()); + boost::shared_ptr data = model->createData(&pinocchio_data); + + // Create a random force and update it + Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows()); + model->updateForce(data, f); +// boost::shared_ptr m = boost::static_pointer_cast(model); + + // Check that nothing has been computed and that all value are initialized to 0 + BOOST_CHECK(data->Jc.isZero()); + BOOST_CHECK(data->a0.isZero()); + BOOST_CHECK(data->da0_dx.isZero()); + BOOST_CHECK(!data->f.toVector().isZero()); + BOOST_CHECK(data->df_dx.isZero()); + BOOST_CHECK(data->df_du.isZero()); +} + +void test_update_force_diff(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // create the corresponding data object + const boost::shared_ptr& pinocchio_model = model->get_state()->get_pinocchio(); + pinocchio::Data pinocchio_data(*pinocchio_model.get()); + boost::shared_ptr data = model->createData(&pinocchio_data); + + // Create a random force and update it + Eigen::MatrixXd df_dx = Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols()); + Eigen::MatrixXd df_du = Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols()); + model->updateForceDiff(data, df_dx, df_du); + + // Check that nothing has been computed and that all value are initialized to 0 + BOOST_CHECK(data->Jc.isZero()); + BOOST_CHECK(data->a0.isZero()); + BOOST_CHECK(data->da0_dx.isZero()); + BOOST_CHECK(data->f.toVector().isZero()); + BOOST_CHECK(!data->df_dx.isZero()); + BOOST_CHECK(!data->df_du.isZero()); +} + +void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { +#if BOOST_VERSION / 100 % 1000 >= 60 + using namespace boost::placeholders; +#endif + // create the model + ContactModel1DFactory factory; + boost::shared_ptr model = factory.create(mask_type, model_type, reference_type); + + // create the corresponding data object + pinocchio::Model& pinocchio_model = *model->get_state()->get_pinocchio().get(); + pinocchio::Data pinocchio_data(pinocchio_model); + boost::shared_ptr data = model->createData(&pinocchio_data); + + // Create the equivalent num diff model and data. + crocoddyl::ContactModelNumDiff model_num_diff(model); + const boost::shared_ptr& data_num_diff = model_num_diff.createData(&pinocchio_data); + + // Generating random values for the state + const Eigen::VectorXd& x = model->get_state()->rand(); + + // Compute all the pinocchio function needed for the models. + crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); + + // set the function that needs to be called at every step of the numdiff + std::vector reevals; + reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio, &pinocchio_model, &pinocchio_data, _1, _2)); + model_num_diff.set_reevals(reevals); + // model_num_diff.set_disturbance(1e-3); + // Computing the contact derivatives + model->calc(data, x); + model->calcDiff(data, x); + + // Computing the contact derivatives via numerical differentiation + model_num_diff.calc(data_num_diff, x); + model_num_diff.calcDiff(data_num_diff, x); + + // Checking the partial derivatives against NumDiff + double tol = sqrt(model_num_diff.get_disturbance()); + BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol)); +} + +//----------------------------------------------------------------------------// + +void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_type, + PinocchioModelTypes::Type model_type, + PinocchioReferenceTypes::Type reference_type) { + boost::test_tools::output_test_stream test_name; + test_name << "test_" << mask_type << "_" << model_type << "_" << reference_type; + std::cout << "Running " << test_name.str() << std::endl; + test_suite* ts = BOOST_TEST_SUITE(test_name.str()); + ts->add(BOOST_TEST_CASE(boost::bind(&test_construct_data, mask_type, model_type, reference_type))); + ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_fetch_jacobians, mask_type, model_type, reference_type))); + ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives, mask_type, model_type, reference_type))); + ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force, mask_type, model_type, reference_type))); + ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force_diff, mask_type, model_type, reference_type))); + ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, mask_type, model_type, reference_type))); + framework::master_test_suite().add(ts); +} + +bool init_function() { + for (size_t mask_type = 0; mask_type < ContactModelMaskTypes::all.size(); ++mask_type) { + for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size(); ++model_type) { + for (size_t reference_type = 0; reference_type < PinocchioReferenceTypes::all.size(); ++reference_type) { + register_contact_model_unit_tests(ContactModelMaskTypes::all[mask_type], + PinocchioModelTypes::all[model_type], + PinocchioReferenceTypes::all[reference_type]); + } + } + } + return true; +} + +int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }