From 44b65f36a2aaa883b6346923b2bece8e5fb106f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Kleff?= Date: Thu, 31 Mar 2022 18:07:43 +0200 Subject: [PATCH] formatting --- .../multibody/contacts/contact-1d.cpp | 6 +- .../multibody/contacts/contact-1d.hpp | 9 +-- .../multibody/contacts/contact-1d.hxx | 8 +-- .../multibody/residuals/contact-force.hxx | 19 +++--- unittest/factory/contact1d.cpp | 64 +++++++++++-------- unittest/factory/contact1d.hpp | 10 +-- unittest/test_contact1d.cpp | 40 +++++------- 7 files changed, 77 insertions(+), 79 deletions(-) diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp index 5331150cd0..3752ccf31f 100644 --- a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp +++ b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp @@ -15,11 +15,7 @@ namespace python { void exposeContact1D() { bp::register_ptr_to_python >(); - bp::enum_("Vector3MaskType") - .value("x", x) - .value("y", y) - .value("z", z) - .export_values(); + bp::enum_("Vector3MaskType").value("x", x).value("y", y).value("z", z).export_values(); bp::class_ >( "ContactModel1D", diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hpp b/include/crocoddyl/multibody/contacts/contact-1d.hpp index d64b3db564..6bd961c11b 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hpp +++ b/include/crocoddyl/multibody/contacts/contact-1d.hpp @@ -53,7 +53,8 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { */ ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Scalar xref, const std::size_t nu, const Vector2s& gains = Vector2s::Zero(), - const Vector3MaskType& mask = Vector3MaskType::z, const pinocchio::ReferenceFrame ref = pinocchio::LOCAL); + const Vector3MaskType& mask = Vector3MaskType::z, + const pinocchio::ReferenceFrame ref = pinocchio::LOCAL); /** * @brief Initialize the 1d contact model @@ -153,7 +154,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { private: Scalar xref_; //!< Contact position used for the Baumgarte stabilization Vector2s gains_; //!< Baumgarte stabilization gains - Vector3MaskType 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 +183,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 = pinocchio::SE3::Identity().toActionMatrix(); //jMf.inverse().toActionMatrix(); + fXj = pinocchio::SE3::Identity().toActionMatrix(); // jMf.inverse().toActionMatrix(); fJf.setZero(); v_partial_dq.setZero(); a_partial_dq.setZero(); @@ -200,7 +201,7 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> { mask = model->get_mask(); jMc_ = pinocchio::SE3::Identity(); wMlwa_ = pinocchio::SE3::Identity(); - lwaMj_ = pinocchio::SE3::Identity(); + lwaMj_ = pinocchio::SE3::Identity(); } using Base::a0; diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hxx b/include/crocoddyl/multibody/contacts/contact-1d.hxx index 9b53b36b47..8e6c5d9352 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hxx +++ b/include/crocoddyl/multibody/contacts/contact-1d.hxx @@ -64,13 +64,11 @@ void ContactModel1DTpl::calcDiff(const boost::shared_ptrvv, d->vv_skew); pinocchio::skew(d->vw, d->vw_skew); // Matrix6s fXj; - if (type_ == pinocchio::LOCAL){ + if (type_ == pinocchio::LOCAL) { d->fXj = d->jMf.inverse().toActionMatrix(); - } - else if (type_ == pinocchio::WORLD) { + } else if (type_ == pinocchio::WORLD) { d->fXj = d->fXj; - } - else if (type_ == pinocchio::LOCAL_WORLD_ALIGNED) { + } else if (type_ == pinocchio::LOCAL_WORLD_ALIGNED) { d->lwaMj_.translation(d->jMf.inverse().translation()); d->fXj = d->lwaMj_.toActionMatrix(); } diff --git a/include/crocoddyl/multibody/residuals/contact-force.hxx b/include/crocoddyl/multibody/residuals/contact-force.hxx index eee4b95fea..f2b7e9ca1a 100644 --- a/include/crocoddyl/multibody/residuals/contact-force.hxx +++ b/include/crocoddyl/multibody/residuals/contact-force.hxx @@ -40,17 +40,20 @@ void ResidualModelContactForceTpl::calc(const boost::shared_ptrcontact_type) { - case Contact1D:{ + case Contact1D: { ContactData1DTpl* d1d = static_cast*>(d->contact.get()); - if(d1d->type == pinocchio::LOCAL){ + 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) { + } 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); + 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; } diff --git a/unittest/factory/contact1d.cpp b/unittest/factory/contact1d.cpp index 8596ee4b69..3a5d551010 100644 --- a/unittest/factory/contact1d.cpp +++ b/unittest/factory/contact1d.cpp @@ -62,11 +62,9 @@ std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type& 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 { +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()); @@ -84,33 +82,42 @@ boost::shared_ptr ContactModel1DFactory::create 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); + 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); + 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); - } + 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"); @@ -128,11 +135,14 @@ boost::shared_ptr create_random_contact1d() { boost::shared_ptr contact; ContactModel1DFactory factory; if (rand() % 3 == 0) { - contact = factory.create(ContactModelMaskTypes::X, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + contact = + factory.create(ContactModelMaskTypes::X, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); } else if (rand() % 3 == 1) { - contact = factory.create(ContactModelMaskTypes::Y, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + contact = + factory.create(ContactModelMaskTypes::Y, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); } else { - contact = factory.create(ContactModelMaskTypes::Z, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); + contact = + factory.create(ContactModelMaskTypes::Z, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL); } return contact; } diff --git a/unittest/factory/contact1d.hpp b/unittest/factory/contact1d.hpp index c56915b1b9..206f707414 100644 --- a/unittest/factory/contact1d.hpp +++ b/unittest/factory/contact1d.hpp @@ -20,9 +20,8 @@ namespace crocoddyl { namespace unittest { - struct ContactModelMaskTypes { - enum Type {X, Y, Z, NbMaskTypes}; + enum Type { X, Y, Z, NbMaskTypes }; static std::vector init_all() { std::vector v; v.clear(); @@ -35,7 +34,7 @@ struct ContactModelMaskTypes { }; struct PinocchioReferenceTypes { - enum Type { LOCAL, WORLD, LOCAL_WORLD_ALIGNED, NbPinRefTypes}; + enum Type { LOCAL, WORLD, LOCAL_WORLD_ALIGNED, NbPinRefTypes }; static std::vector init_all() { std::vector v; v.clear(); @@ -47,12 +46,10 @@ struct PinocchioReferenceTypes { 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 @@ -62,8 +59,7 @@ class ContactModel1DFactory { boost::shared_ptr create( ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type, - PinocchioReferenceTypes::Type reference_type, - const std::string frame_name = std::string(""), + PinocchioReferenceTypes::Type reference_type, const std::string frame_name = std::string(""), const std::size_t nu = std::numeric_limits::max()) const; }; diff --git a/unittest/test_contact1d.cpp b/unittest/test_contact1d.cpp index 8e88a70b1d..73e312abfc 100644 --- a/unittest/test_contact1d.cpp +++ b/unittest/test_contact1d.cpp @@ -25,8 +25,7 @@ using namespace boost::unit_test; //----------------------------------------------------------------------------// -void test_construct_data(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, +void test_construct_data(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type, PinocchioReferenceTypes::Type reference_type) { // create the model ContactModel1DFactory factory; @@ -42,9 +41,8 @@ void test_construct_data(ContactModelMaskTypes::Type mask_type, 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) { +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); @@ -70,9 +68,8 @@ void test_calc_fetch_jacobians(ContactModelMaskTypes::Type mask_type, BOOST_CHECK(data->df_du.isZero()); } -void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, - PinocchioReferenceTypes::Type reference_type) { +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); @@ -99,9 +96,8 @@ void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type, BOOST_CHECK(data->df_du.isZero()); } -void test_update_force(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, - PinocchioReferenceTypes::Type reference_type) { +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); @@ -114,7 +110,7 @@ void test_update_force(ContactModelMaskTypes::Type mask_type, // 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); + // 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()); @@ -125,8 +121,7 @@ void test_update_force(ContactModelMaskTypes::Type mask_type, BOOST_CHECK(data->df_du.isZero()); } -void test_update_force_diff(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, +void test_update_force_diff(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type, PinocchioReferenceTypes::Type reference_type) { // create the model ContactModel1DFactory factory; @@ -151,8 +146,8 @@ void test_update_force_diff(ContactModelMaskTypes::Type mask_type, BOOST_CHECK(!data->df_du.isZero()); } -void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, +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; @@ -196,9 +191,8 @@ void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_t //----------------------------------------------------------------------------// -void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_type, - PinocchioModelTypes::Type model_type, - PinocchioReferenceTypes::Type reference_type) { +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; @@ -208,7 +202,8 @@ void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_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))); + ts->add( + BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, mask_type, model_type, reference_type))); framework::master_test_suite().add(ts); } @@ -216,9 +211,8 @@ 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]); + register_contact_model_unit_tests(ContactModelMaskTypes::all[mask_type], PinocchioModelTypes::all[model_type], + PinocchioReferenceTypes::all[reference_type]); } } }