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

for more information, see https://pre-commit.ci
loco-3d · Oct 25, 2023 · d3cfbaa · d3cfbaa
1 parent 0087464
commit d3cfbaa
Show file tree

Hide file tree

Showing 26 changed files with 240 additions and 187 deletions.
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp
@@ -28,10 +28,11 @@ 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<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex, 
-               Eigen::Vector3d, pinocchio::ReferenceFrame, std::size_t, 
+      bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
+               Eigen::Vector3d, pinocchio::ReferenceFrame, std::size_t,
                bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
-          bp::args("self", "state", "id", "xref", "type", "nu", "gains", "rotation"),
+          bp::args("self", "state", "id", "xref", "type", "nu", "gains",
+                   "rotation"),
           "Initialize the contact model.\n\n"
           ":param state: state of the multibody system\n"
           ":param id: reference frame id of the contact\n"
@@ -42,7 +43,7 @@ void exposeContact1D() {
           "np.matrix([0.,0.]))\n"
           ":param rotation: rotation of the reference frame's z axis"))
       .def(bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
-                    Eigen::Vector3d, pinocchio::ReferenceFrame, 
+                    Eigen::Vector3d, pinocchio::ReferenceFrame,
                     bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
           bp::args("self", "state", "id", "xref", "type", "gains", "rotation"),
           "Initialize the contact model.\n\n"

diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py
@@ -1083,7 +1083,13 @@ def __init__(self, model, collector):
 
 class Contact1DModelDerived(crocoddyl.ContactModelAbstract):
     def __init__(
-        self, state, id, xref, type=pinocchio.ReferenceFrame.LOCAL, gains=[0.0, 0.0], rotation=np.eye(3)
+        self,
+        state,
+        id,
+        xref,
+        type=pinocchio.ReferenceFrame.LOCAL,
+        gains=[0.0, 0.0],
+        rotation=np.eye(3),
     ):
         crocoddyl.ContactModelAbstract.__init__(self, state, type, 1)
         self.id = id
@@ -1158,9 +1164,9 @@ def calcDiff(self, data, x):
         if self.gains[1] != 0.0:
             data.da0_local_dx[:, :nv] += self.gains[1] * data.fXjdv_dq[:3, :]
             data.da0_local_dx[:, nv:] += self.gains[1] * data.fJf[:3, :]
-        
+
         if self.type == pinocchio.LOCAL:
-            data.da0_dx[:] = (self.rotation @ data.da0_local_dx)[2,:]
+            data.da0_dx[:] = (self.rotation @ data.da0_local_dx)[2, :]
         if self.type == pinocchio.WORLD or self.type == pinocchio.LOCAL_WORLD_ALIGNED:
             data.a0_local[:] = pinocchio.getFrameClassicalAcceleration(
                 self.state.pinocchio, data.pinocchio, self.id, pinocchio.LOCAL
@@ -1170,8 +1176,10 @@ def calcDiff(self, data, x):
             if self.gains[1] != 0.0:
                 data.a0_local[:] += self.gains[1] * data.vv
             data.a0[0] = np.dot(np.dot(self.rotation, oRf), data.a0_local)[2]
-
-            data.a0_skew[:, :] = pinocchio.skew(np.dot(np.dot(self.rotation, oRf), data.a0_local))
+
+            data.a0_skew[:, :] = pinocchio.skew(
+                np.dot(np.dot(self.rotation, oRf), data.a0_local)
+            )
             data.a0_world_skew = np.dot(data.a0_skew, np.dot(self.rotation, oRf))
             data.da0_dx[:] = np.dot(np.dot(self.rotation, oRf), data.da0_local_dx)[2]
             data.da0_dx[:nv] -= np.dot(data.a0_world_skew, data.fJf[3:, :])[2]
@@ -1187,12 +1195,14 @@ def updateForce(self, data, force):
         data.f.linear[:2] = np.zeros(2)
         data.f.angular = np.zeros(3)
         if self.type == pinocchio.LOCAL:
-            data.fext.linear = np.dot(data.jMf.rotation, self.rotation.T)[:,2] * force[0]
-            data.fext.angular = np.cross(data.jMf.translation, data.fext.linear); 
+            data.fext.linear = (
+                np.dot(data.jMf.rotation, self.rotation.T)[:, 2] * force[0]
+            )
+            data.fext.angular = np.cross(data.jMf.translation, data.fext.linear)
             data.dtau_dq[:, :] = np.zeros((nv, nv))
         if self.type == pinocchio.WORLD or self.type == pinocchio.LOCAL_WORLD_ALIGNED:
             oRf = data.pinocchio.oMf[self.id].rotation
-            data.f_local.linear = np.dot(oRf.T, self.rotation.T)[:,2] * force[0]
+            data.f_local.linear = np.dot(oRf.T, self.rotation.T)[:, 2] * force[0]
             data.f_local.angular = np.zeros(3)
             data.fext = data.jMf.act(data.f_local)
             data.f_skew[:, :] = pinocchio.skew(data.f_local.linear)
@@ -1304,6 +1314,7 @@ def updateForce(self, data, force):
             data.fJf_df[:, :] = np.dot(data.f_skew, data.fJf[3:, :])
             data.dtau_dq[:, :] = -np.dot(data.fJf[:3, :].T, data.fJf_df)
 
+
 class Contact1DDataDerived(crocoddyl.ContactDataAbstract):
     def __init__(self, model, data):
         crocoddyl.ContactDataAbstract.__init__(self, model, data)
@@ -1330,6 +1341,7 @@ def __init__(self, model, data):
         self.fXjda_dv = np.zeros((6, model.state.nv))
         self.fJf_df = np.zeros((3, model.state.nv))
 
+
 class Contact3DDataDerived(crocoddyl.ContactDataAbstract):
     def __init__(self, model, data):
         crocoddyl.ContactDataAbstract.__init__(self, model, data)

diff --git a/examples/log/arm_manipulation_fwddyn.log b/examples/log/arm_manipulation_fwddyn.log
@@ -1,4 +1,4 @@
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
    0  1.926e+03  1.331e+03  2.708e+03  1.000e-09  1.000e-09  1.0000  3.142e+00  0.000e+00  0.000e+00  1.331e+03  8.047e+02
    1  1.661e+03  1.908e+03  3.817e+03  1.000e-09  1.000e-09  0.1250  0.000e+00  0.000e+00  0.000e+00  4.473e+02  2.650e+02
    2  6.675e+02  1.642e+03  3.283e+03  1.000e-09  1.000e-09  0.5000  0.000e+00  0.000e+00  0.000e+00  1.231e+03  9.930e+02
@@ -9,7 +9,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
    7  2.502e+02  2.328e+02  4.656e+02  1.000e-07  1.000e-07  0.0020  0.000e+00  0.000e+00  0.000e+00  9.084e-01  2.471e-01
    8  2.142e+02  2.323e+02  4.647e+02  1.000e-07  1.000e-07  0.2500  0.000e+00  0.000e+00  0.000e+00  1.016e+02  3.607e+01
    9  1.768e+02  1.951e+02  3.901e+02  1.000e-07  1.000e-07  0.5000  0.000e+00  0.000e+00  0.000e+00  1.463e+02  3.738e+01
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   10  5.864e+01  1.588e+02  3.177e+02  1.000e-07  1.000e-07  0.5000  0.000e+00  0.000e+00  0.000e+00  1.191e+02  1.182e+02
   11  3.604e+01  3.890e+01  7.781e+01  1.000e-07  1.000e-07  0.5000  0.000e+00  0.000e+00  0.000e+00  2.918e+01  2.260e+01
   12  3.554e+01  1.897e+01  3.795e+01  1.000e-07  1.000e-07  0.0625  0.000e+00  0.000e+00  0.000e+00  2.298e+00  5.036e-01
@@ -20,7 +20,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   17  3.035e+01  1.940e+01  3.881e+01  1.000e-07  1.000e-07  0.0625  0.000e+00  0.000e+00  0.000e+00  2.350e+00  1.441e+00
   18  2.899e+01  1.856e+01  3.711e+01  1.000e-07  1.000e-07  0.0625  0.000e+00  0.000e+00  0.000e+00  2.247e+00  1.357e+00
   19  2.774e+01  1.784e+01  3.568e+01  1.000e-07  1.000e-07  0.1250  0.000e+00  0.000e+00  0.000e+00  4.181e+00  1.248e+00
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   20  2.537e+01  1.784e+01  3.568e+01  1.000e-07  1.000e-07  0.1250  0.000e+00  0.000e+00  0.000e+00  4.182e+00  2.374e+00
   21  2.254e+01  1.581e+01  3.162e+01  1.000e-07  1.000e-07  0.2500  0.000e+00  0.000e+00  0.000e+00  6.917e+00  2.831e+00
   22  1.990e+01  1.338e+01  2.677e+01  1.000e-08  1.000e-08  1.0000  0.000e+00  0.000e+00  0.000e+00  1.338e+01  2.640e+00
@@ -31,7 +31,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   27  8.212e+00  1.991e-02  3.982e-02  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.991e-02  3.716e-02
   28  8.183e+00  1.601e-02  3.201e-02  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.601e-02  2.922e-02
   29  8.162e+00  1.137e-02  2.275e-02  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.137e-02  2.025e-02
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   30  8.150e+00  7.142e-03  1.428e-02  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  7.142e-03  1.258e-02
   31  8.142e+00  4.322e-03  8.644e-03  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  4.322e-03  7.639e-03
   32  8.137e+00  2.722e-03  5.445e-03  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.722e-03  4.799e-03
@@ -42,7 +42,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   37  8.129e+00  3.366e-04  6.732e-04  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  3.366e-04  6.105e-04
   38  8.129e+00  2.286e-04  4.572e-04  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.286e-04  4.153e-04
   39  8.129e+00  1.556e-04  3.112e-04  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.556e-04  2.829e-04
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   40  8.129e+00  1.060e-04  2.120e-04  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.060e-04  1.929e-04
   41  8.128e+00  7.225e-05  1.445e-04  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  7.225e-05  1.380e-04
   42  8.126e+00  2.635e-03  5.269e-03  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.635e-03  2.761e-03
@@ -53,7 +53,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   47  8.125e+00  1.177e-05  2.353e-05  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.177e-05  2.133e-05
   48  8.125e+00  7.858e-06  1.572e-05  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  7.858e-06  1.429e-05
   49  8.125e+00  5.297e-06  1.059e-05  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  5.297e-06  9.645e-06
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   50  8.125e+00  3.582e-06  7.163e-06  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  3.582e-06  6.525e-06
   51  8.125e+00  2.425e-06  4.850e-06  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.425e-06  4.419e-06
   52  8.125e+00  1.643e-06  3.286e-06  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.643e-06  2.994e-06
@@ -64,7 +64,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   57  8.125e+00  2.349e-07  4.698e-07  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.349e-07  4.282e-07
   58  8.125e+00  1.592e-07  3.184e-07  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.592e-07  2.902e-07
   59  8.125e+00  1.079e-07  2.158e-07  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.079e-07  1.967e-07
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   60  8.125e+00  7.314e-08  1.463e-07  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  7.314e-08  1.333e-07
   61  8.125e+00  4.957e-08  9.914e-08  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  4.957e-08  9.038e-08
   62  8.125e+00  3.360e-08  6.720e-08  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  3.360e-08  6.126e-08
@@ -75,7 +75,7 @@ iter    cost       stop       grad       xreg       ureg      step   ||ffeas||
   67  8.125e+00  4.807e-09  9.614e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  4.807e-09  8.765e-09
   68  8.125e+00  3.258e-09  6.517e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  3.258e-09  5.941e-09
   69  8.125e+00  2.209e-09  4.417e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  2.209e-09  4.027e-09
-iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV    
+iter    cost       stop       grad       xreg       ureg      step   ||ffeas||  ||gfeas||  ||hfeas||   dV-exp       dV
   70  8.125e+00  1.497e-09  2.994e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.497e-09  2.729e-09
   71  8.125e+00  1.015e-09  2.029e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  1.015e-09  1.850e-09
   72  8.125e+00  6.878e-10  1.376e-09  1.000e-09  1.000e-09  1.0000  0.000e+00  0.000e+00  0.000e+00  6.878e-10  1.254e-09