formatting

loco-3d · Mar 31, 2022 · 44b65f3 · 44b65f3
1 parent 1fecfb4
commit 44b65f3
Show file tree

Hide file tree

Showing 7 changed files with 77 additions and 79 deletions.
diff --git 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<boost::shared_ptr<ContactModel1D> >();
 
-  bp::enum_<Vector3MaskType>("Vector3MaskType")
-      .value("x", x)
-      .value("y", y)
-      .value("z", z)
-      .export_values();
+  bp::enum_<Vector3MaskType>("Vector3MaskType").value("x", x).value("y", y).value("z", z).export_values();
 
   bp::class_<ContactModel1D, bp::bases<ContactModelAbstract> >(
       "ContactModel1D",

diff --git 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<StateMultibody> 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
@@ -64,13 +64,11 @@ void ContactModel1DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbst
   pinocchio::skew(d->vv, 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
@@ -40,17 +40,20 @@ void ResidualModelContactForceTpl<Scalar>::calc(const boost::shared_ptr<Residual
 
   // We transform the force to the contact frame
   switch (d->contact_type) {
-    case Contact1D:{
+    case Contact1D: {
       ContactData1DTpl<Scalar>* d1d = static_cast<ContactData1DTpl<Scalar>*>(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
@@ -62,11 +62,9 @@ std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type&
 ContactModel1DFactory::ContactModel1DFactory() {}
 ContactModel1DFactory::~ContactModel1DFactory() {}
 
-boost::shared_ptr<crocoddyl::ContactModelAbstract> 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<crocoddyl::ContactModelAbstract> 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<crocoddyl::StateMultibody> state =
       boost::make_shared<crocoddyl::StateMultibody>(model_factory.create());
@@ -84,33 +82,42 @@ boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModel1DFactory::create
   Eigen::Vector2d gains = Eigen::Vector2d::Zero();
   switch (mask_type) {
     case ContactModelMaskTypes::X: {
-      if(reference_type == PinocchioReferenceTypes::LOCAL){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL);
-      } else if(reference_type == PinocchioReferenceTypes::WORLD){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::WORLD);
-      } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL_WORLD_ALIGNED);
+      if (reference_type == PinocchioReferenceTypes::LOCAL) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x,
+                                                                pinocchio::LOCAL);
+      } else if (reference_type == PinocchioReferenceTypes::WORLD) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x,
+                                                                pinocchio::WORLD);
+      } else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(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<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL);
-      } else if(reference_type == PinocchioReferenceTypes::WORLD){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::WORLD);
-      } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL_WORLD_ALIGNED);
+      if (reference_type == PinocchioReferenceTypes::LOCAL) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y,
+                                                                pinocchio::LOCAL);
+      } else if (reference_type == PinocchioReferenceTypes::WORLD) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y,
+                                                                pinocchio::WORLD);
+      } else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(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<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL);
-      } else if(reference_type == PinocchioReferenceTypes::WORLD){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::WORLD);
-      } else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
-        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL_WORLD_ALIGNED);
-      }      
+      if (reference_type == PinocchioReferenceTypes::LOCAL) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z,
+                                                                pinocchio::LOCAL);
+      } else if (reference_type == PinocchioReferenceTypes::WORLD) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z,
+                                                                pinocchio::WORLD);
+      } else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
+        contact = boost::make_shared<crocoddyl::ContactModel1D>(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<crocoddyl::ContactModelAbstract> create_random_contact1d() {
   boost::shared_ptr<crocoddyl::ContactModelAbstract> 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
@@ -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<Type> init_all() {
     std::vector<Type> 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<Type> init_all() {
     std::vector<Type> v;
     v.clear();
@@ -47,12 +46,10 @@ struct PinocchioReferenceTypes {
   static const std::vector<Type> 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<crocoddyl::ContactModelAbstract> 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<std::size_t>::max()) const;
 };
 

diff --git 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<crocoddyl::ContactDataAbstract> 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<crocoddyl::ContactModelAbstract> 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<crocoddyl::ContactModelAbstract> 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<crocoddyl::ContactModelAbstract> 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<crocoddyl::ContactModel3D> m = boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);
+  //   boost::shared_ptr<crocoddyl::ContactModel3D> m = boost::static_pointer_cast<crocoddyl::ContactModel3D>(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,17 +202,17 @@ 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);
 }
 
 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]);
       }
     }
   }