[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp

@@ -34,12 +34,15 @@ void exposeContact6DLoop() {
           "Initialize the contact model.\n\n"
           ":param state: state of the multibody system\n"
           ":param joint1_id: Parent joint id of the first contact frame\n"
-          ":param joint1_placement: Placement of the first contact frame with respect to the parent joint\n"
+          ":param joint1_placement: Placement of the first contact frame with "
+          "respect to the parent joint\n"
           ":param joint2_id: Parent joint id of the second contact frames\n"
-          ":param joint2_placement: Placement of the second contact frame with respect to the parent joint\n"
+          ":param joint2_placement: Placement of the second contact frame with "
+          "respect to the parent joint\n"
           ":param ref: reference frame of contact (must be pinocchio::LOCAL)\n"
           ":param nu: dimension of control vector\n"
-          ":param gains: gains of the contact model (default np.matrix([0.,0.]))"))
+          ":param gains: gains of the contact model (default "
+          "np.matrix([0.,0.]))"))
       .def("calc", &ContactModel6DLoop::calc, bp::args("self", "data", "x"),
            "Compute the 6D loop-contact Jacobian and drift.\n\n"
            "The rigid contact model throught acceleration-base holonomic "
@@ -49,7 +52,8 @@ void exposeContact6DLoop() {
            ":param x: state point (dim. state.nx)")
       .def("calcDiff", &ContactModel6DLoop::calcDiff,
            bp::args("self", "data", "x"),
-           "Compute the derivatives of the 6D loop-contact holonomic constraint.\n\n"
+           "Compute the derivatives of the 6D loop-contact holonomic "
+           "constraint.\n\n"
            "The rigid contact model throught acceleration-base holonomic "
            "constraint\n"
            "of the contact frame placement.\n"
@@ -62,11 +66,11 @@ void exposeContact6DLoop() {
            ":param data: cost data\n"
            ":param force: force vector (dimension 6)")
       .def("updateForceDiff", &ContactModel6DLoop::updateForceDiff,
-              bp::args("self", "data", "df_dx", "df_du"),
-              "Update the force derivatives.\n\n"
-              ":param data: cost data\n"
-              ":param df_dx: Jacobian of the force with respect to the state\n"
-              ":param df_du: Jacobian of the force with respect to the control")
+           bp::args("self", "data", "df_dx", "df_du"),
+           "Update the force derivatives.\n\n"
+           ":param data: cost data\n"
+           ":param df_dx: Jacobian of the force with respect to the state\n"
+           ":param df_du: Jacobian of the force with respect to the control")
       .def("createData", &ContactModel6DLoop::createData,
            bp::with_custodian_and_ward_postcall<0, 2>(),
            bp::args("self", "data"),
@@ -85,7 +89,8 @@ void exposeContact6DLoop() {
           "joint1_placement",
           bp::make_function(&ContactModel6DLoop::get_joint1_placement,
                             bp::return_value_policy<bp::return_by_value>()),
-          "Placement of the first contact frame with respect to the parent joint")
+          "Placement of the first contact frame with respect to the parent "
+          "joint")
       .add_property(
           "joint2_id",
           bp::make_function(&ContactModel6DLoop::get_joint2_id,
@@ -95,7 +100,8 @@ void exposeContact6DLoop() {
           "joint2_placement",
           bp::make_function(&ContactModel6DLoop::get_joint2_placement,
                             bp::return_value_policy<bp::return_by_value>()),
-          "Placement of the second contact frame with respect to the parent joint")
+          "Placement of the second contact frame with respect to the parent "
+          "joint")
       .add_property(
           "gains",
           bp::make_function(&ContactModel6DLoop::get_gains,

include/crocoddyl/multibody/contact-base.hpp

@@ -94,8 +94,9 @@ class ContactModelAbstractTpl {
    * @param[in] data   Contact data
    * @param[in] force  Contact force
    */
-  virtual void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data,
-                       const MatrixXs& df_dx, const MatrixXs& df_du) const;
+  virtual void updateForceDiff(
+      const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
+      const MatrixXs& df_du) const;
 
   /**
    * @brief Set the stack of spatial forces to zero

include/crocoddyl/multibody/contacts/contact-6d-loop.hpp

@@ -112,18 +112,21 @@ class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> {
   virtual void updateForce(const boost::shared_ptr<ContactDataAbstract> &data,
                            const VectorXs &force);
 
-
   /**
    * @brief Updates the force differential for the given contact data.
    *
-   * This function updates the force differential matrices with respect to the state and control variables.
+   * This function updates the force differential matrices with respect to the
+   * state and control variables.
    *
    * @param[in] data  Shared pointer to the contact data abstract.
-   * @param[in] df_dx Matrix representing the differential of the force with respect to the state variables.
-   * @param[in] df_du Matrix representing the differential of the force with respect to the control variables.
+   * @param[in] df_dx Matrix representing the differential of the force with
+   * respect to the state variables.
+   * @param[in] df_du Matrix representing the differential of the force with
+   * respect to the control variables.
    */
-  virtual void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data,
-                               const MatrixXs& df_dx, const MatrixXs& df_du);
+  virtual void updateForceDiff(
+      const boost::shared_ptr<ContactDataAbstract> &data, const MatrixXs &df_dx,
+      const MatrixXs &df_du);
 
   /**
    * @brief Create the 6d contact data
@@ -137,7 +140,8 @@ class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> {
   const int get_joint1_id() const;
 
   /**
-   * @brief Return the first contact frame placement with respect to the parent joint
+   * @brief Return the first contact frame placement with respect to the parent
+   * joint
    */
   const SE3 &get_joint1_placement() const;
 
@@ -147,7 +151,8 @@ class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> {
   const int get_joint2_id() const;
 
   /**
-   * @brief Return the second contact frame placement with respect to the parent joint
+   * @brief Return the second contact frame placement with respect to the parent
+   * joint
    */
   const SE3 &get_joint2_placement() const;
 
@@ -162,7 +167,8 @@ class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> {
   void set_joint1_id(const int joint1_id);
 
   /**
-   * @brief Set the first contact frame placement with respect to the parent joint
+   * @brief Set the first contact frame placement with respect to the parent
+   * joint
    */
   void set_joint1_placement(const SE3 &joint1_placement);
 
@@ -172,7 +178,8 @@ class ContactModel6DLoopTpl : public ContactModelAbstractTpl<_Scalar> {
   void set_joint2_id(const int joint2_id);
 
   /**
-   * @brief Set the second contact frame placement with respect to the parent joint
+   * @brief Set the second contact frame placement with respect to the parent
+   * joint
    */
   void set_joint2_placement(const SE3 &joint2_placement);
 
@@ -331,7 +338,8 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
   // Placement related data
   SE3 oMf1;   // Placement of the first contact frame in the world frame
   SE3 oMf2;   // Placement of the second contact frame in the world frame
-  SE3 f1Mf2;  // Relative placement of the contact frames in the first contact frame
+  SE3 f1Mf2;  // Relative placement of the contact frames in the first contact
+              // frame
   SE3ActionMatrix j1Xf1;
   SE3ActionMatrix j2Xf2;
   SE3ActionMatrix f1Xf2;

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

@@ -30,12 +30,12 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
       gains_(gains) {
   if (ref != pinocchio::ReferenceFrame::LOCAL) {
     std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported "
-              "for 6D loop contacts"
+                 "for 6D loop contacts"
               << std::endl;
   }
   if (joint1_id == 0 || joint2_id == 0) {
     std::cerr << "Warning: At least one of the parents joints id is zero"
-              "you should use crocoddyl::ContactModel6D instead"
+                 "you should use crocoddyl::ContactModel6D instead"
               << std::endl;
   }
 }
@@ -59,7 +59,7 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
   }
   if (joint1_id == 0 || joint2_id == 0) {
     std::cerr << "Warning: At least one of the parents joints id is zero"
-              "you should use crocoddyl::ContactModel6D instead"
+                 "you should use crocoddyl::ContactModel6D instead"
               << std::endl;
   }
 }
@@ -101,8 +101,7 @@ void ContactModel6DLoopTpl<Scalar>::calc(
     d->f1vf2 = d->f1Mf2.act(d->f2vf2);
     d->f1af2 = d->f1Mf2.act(d->f2af2);
   }
-  d->a0.noalias() =
-      (d->f1af1 - d->f1af2 + d->f1vf1.cross(d->f1vf2)).toVector();
+  d->a0.noalias() = (d->f1af1 - d->f1af2 + d->f1vf1.cross(d->f1vf2)).toVector();
 
   if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
     d->a0.noalias() -= gains_[0] * pinocchio::log6(d->f1Mf2).toVector();
@@ -135,37 +134,45 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
       pinocchio::LOCAL, d->v2_partial_dq, d->a2_partial_dq, d->a2_partial_dv,
       d->a2_partial_da);
 
-  d->da0_dq_t1.noalias() = joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq;
+  d->da0_dq_t1.noalias() =
+      joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq;
 
   d->da0_dq_t2.noalias() = d->f1af2.toActionMatrix() * d->Jc;
-  d->f2_a2_partial_dq.noalias() = joint2_placement_.toActionMatrixInverse() *
-                              d->a2_partial_dq;
+  d->f2_a2_partial_dq.noalias() =
+      joint2_placement_.toActionMatrixInverse() * d->a2_partial_dq;
   d->da0_dq_t2.noalias() += d->f1Xf2 * d->f2_a2_partial_dq;
-
-  d->f1_v1_partial_dq.noalias() = joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq;
-  d->da0_dq_t3.noalias() = - d->f1vf2.toActionMatrix() * d->f1_v1_partial_dq;
+
+  d->f1_v1_partial_dq.noalias() =
+      joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq;
+  d->da0_dq_t3.noalias() = -d->f1vf2.toActionMatrix() * d->f1_v1_partial_dq;
   d->da0_dq_t3_tmp.noalias() = d->f1vf2.toActionMatrix() * d->Jc;
   d->da0_dq_t3.noalias() += d->f1vf1.toActionMatrix() * d->da0_dq_t3_tmp;
-  d->da0_dq_t3_tmp.noalias() = joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
-  d->da0_dq_t3.noalias() += d->f1vf1.toActionMatrix() * d->f1Xf2 * d->da0_dq_t3_tmp;
+  d->da0_dq_t3_tmp.noalias() =
+      joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
+  d->da0_dq_t3.noalias() +=
+      d->f1vf1.toActionMatrix() * d->f1Xf2 * d->da0_dq_t3_tmp;
   d->da0_dx.leftCols(nv).noalias() = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3;
 
-  d->f2_a2_partial_dv.noalias() = joint2_placement_.toActionMatrixInverse() * d->a2_partial_dv;
+  d->f2_a2_partial_dv.noalias() =
+      joint2_placement_.toActionMatrixInverse() * d->a2_partial_dv;
   d->f1Jf2.noalias() = d->f1Xf2 * d->f2Jf2;
-  d->da0_dx.rightCols(nv).noalias() = joint1_placement_.toActionMatrixInverse() * d->a1_partial_dv;
+  d->da0_dx.rightCols(nv).noalias() =
+      joint1_placement_.toActionMatrixInverse() * d->a1_partial_dv;
   d->da0_dx.rightCols(nv).noalias() -= d->f1Xf2 * d->f2_a2_partial_dv;
   d->da0_dx.rightCols(nv).noalias() -= d->f1vf2.toActionMatrix() * d->f1Jf1;
   d->da0_dx.rightCols(nv).noalias() += d->f1vf1.toActionMatrix() * d->f1Jf2;
 
   if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
     Matrix6s f1Mf2_log6;
     pinocchio::Jlog6(d->f1Mf2, f1Mf2_log6);
-    d->dpos_dq.noalias() = d->oMf2.toActionMatrixInverse() * d->oMf1.toActionMatrix() * d->f1Jf1;
+    d->dpos_dq.noalias() =
+        d->oMf2.toActionMatrixInverse() * d->oMf1.toActionMatrix() * d->f1Jf1;
     d->dpos_dq.noalias() -= d->f2Jf2;
     d->da0_dx.leftCols(nv).noalias() += gains_[0] * f1Mf2_log6 * d->dpos_dq;
   }
   if (std::abs<Scalar>(gains_[1]) > std::numeric_limits<Scalar>::epsilon()) {
-    d->f2_v2_partial_dq.noalias() = joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
+    d->f2_v2_partial_dq.noalias() =
+        joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
     d->f1_v2_partial_dq.noalias() = d->f1Xf2 * d->f2_v2_partial_dq;
     d->dvel_dq.noalias() = d->f1_v1_partial_dq;
     d->dvel_dq.noalias() -= d->f1vf2.toActionMatrix() * d->Jc;
@@ -179,8 +186,8 @@ template <typename Scalar>
 void ContactModel6DLoopTpl<Scalar>::updateForce(
     const boost::shared_ptr<ContactDataAbstract> &data, const VectorXs &force) {
   if (force.size() != 6) {
-    throw_pretty(
-        "Contact force vector has wrong dimension (expected 6 got " << force.size() << ")");
+    throw_pretty("Contact force vector has wrong dimension (expected 6 got "
+                 << force.size() << ")");
   }
   Data *d = static_cast<Data *>(data.get());
   d->f = pinocchio::ForceTpl<Scalar>(-force);
@@ -205,8 +212,8 @@ void ContactModel6DLoopTpl<Scalar>::updateForce(
 
 template <typename Scalar>
 void ContactModel6DLoopTpl<Scalar>::updateForceDiff(
-    const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
-    const MatrixXs& df_du) {
+    const boost::shared_ptr<ContactDataAbstract> &data, const MatrixXs &df_dx,
+    const MatrixXs &df_du) {
   if (static_cast<std::size_t>(df_dx.rows()) != nc_ ||
       static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx())
     throw_pretty("df_dx has wrong dimension");

unittest/factory/contact_loop.cpp

@@ -36,13 +36,14 @@ std::ostream& operator<<(std::ostream& os,
 ContactLoopModelFactory::ContactLoopModelFactory() {}
 ContactLoopModelFactory::~ContactLoopModelFactory() {}
 
-boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactLoopModelFactory::create(
-      ContactLoopModelTypes::Type contact_type,
-      PinocchioModelTypes::Type model_type,
-      const int joint1_id, const pinocchio::SE3 &joint1_placement,
-      const int joint2_id, const pinocchio::SE3 &joint2_placement,
-      Eigen::Vector2d gains,
-      std::size_t nu ) const {
+boost::shared_ptr<crocoddyl::ContactModelAbstract>
+ContactLoopModelFactory::create(ContactLoopModelTypes::Type contact_type,
+                                PinocchioModelTypes::Type model_type,
+                                const int joint1_id,
+                                const pinocchio::SE3& joint1_placement,
+                                const int joint2_id,
+                                const pinocchio::SE3& joint2_placement,
+                                Eigen::Vector2d gains, std::size_t nu) const {
   PinocchioModelFactory model_factory(model_type);
   boost::shared_ptr<crocoddyl::StateMultibody> state =
       boost::make_shared<crocoddyl::StateMultibody>(model_factory.create());
@@ -53,8 +54,7 @@ boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactLoopModelFactory::crea
   switch (contact_type) {
     case ContactLoopModelTypes::ContactModel6DLoop_LOCAL:
       contact = boost::make_shared<crocoddyl::ContactModel6DLoop>(
-          state, joint1_id, joint1_placement, 
-          joint2_id, joint2_placement,
+          state, joint1_id, joint1_placement, joint2_id, joint2_placement,
           pinocchio::ReferenceFrame::LOCAL, nu, gains);
       break;
     default:
@@ -64,20 +64,24 @@ boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactLoopModelFactory::crea
   return contact;
 }
 
-boost::shared_ptr<crocoddyl::ContactModelAbstract> create_random_loop_contact() {
+boost::shared_ptr<crocoddyl::ContactModelAbstract>
+create_random_loop_contact() {
   static bool once = true;
   if (once) {
     srand((unsigned)time(NULL));
     once = false;
   }
   boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
-  ContactLoopModelFactory factory;;
+  ContactLoopModelFactory factory;
+  ;
   if (rand() % 1 == 0) {
-    contact = factory.create(ContactLoopModelTypes::ContactModel6DLoop_LOCAL,
-                            PinocchioModelTypes::RandomHumanoid,
-                            0, pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Random()),
-                            1, pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Random()),
-                            Eigen::Vector2d::Random());
+    contact = factory.create(
+        ContactLoopModelTypes::ContactModel6DLoop_LOCAL,
+        PinocchioModelTypes::RandomHumanoid, 0,
+        pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Random()),
+        1,
+        pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Random()),
+        Eigen::Vector2d::Random());
   }
   return contact;
 }

unittest/factory/contact_loop.hpp

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University, 
+// Copyright (C) 2019-2024, University of Edinburgh, Heriot-Watt University,
 //                          LAAS-CNRS
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
@@ -22,10 +22,7 @@ namespace crocoddyl {
 namespace unittest {
 
 struct ContactLoopModelTypes {
-  enum Type {
-    ContactModel6DLoop_LOCAL,
-    NbContactModelTypes
-  };
+  enum Type { ContactModel6DLoop_LOCAL, NbContactModelTypes };
   static std::vector<Type> init_all() {
     std::vector<Type> v;
     v.reserve(NbContactModelTypes);
@@ -37,7 +34,8 @@ struct ContactLoopModelTypes {
   static const std::vector<Type> all;
 };
 
-std::ostream& operator<<(std::ostream& os, const ContactLoopModelTypes::Type& type);
+std::ostream& operator<<(std::ostream& os,
+                         const ContactLoopModelTypes::Type& type);
 
 class ContactLoopModelFactory {
  public:
@@ -48,10 +46,9 @@ class ContactLoopModelFactory {
 
   boost::shared_ptr<crocoddyl::ContactModelAbstract> create(
       ContactLoopModelTypes::Type contact_type,
-      PinocchioModelTypes::Type model_type,
-      const int joint1_id, const pinocchio::SE3 &joint1_placement,
-      const int joint2_id, const pinocchio::SE3 &joint2_placement,
-      Eigen::Vector2d gains,
+      PinocchioModelTypes::Type model_type, const int joint1_id,
+      const pinocchio::SE3& joint1_placement, const int joint2_id,
+      const pinocchio::SE3& joint2_placement, Eigen::Vector2d gains,
       std::size_t nu = std::numeric_limits<std::size_t>::max()) const;
 };