Added assert for non supported algorithms and apply precommit

stack-of-tasks · Jan 16, 2025 · 3fa5eaa · 3fa5eaa
1 parent 064c2a5
commit 3fa5eaa
Show file tree

Hide file tree

Showing 21 changed files with 127 additions and 73 deletions.
diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx
@@ -43,6 +43,11 @@ namespace pinocchio
         const Eigen::MatrixBase<ConfigVectorType> & q,
         const Eigen::MatrixBase<TangentVectorType> & v)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
 
         const JointIndex i = jmodel.id();
@@ -162,8 +167,8 @@ namespace pinocchio
         {
           Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
 
-          fi.toVector().noalias() += Ia * data.oa_gf[i].toVector()
-                                     + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
+          fi.toVector().noalias() +=
+            Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
           data.oYaba[parent] += Ia;
           data.of[parent] += fi;
         }

diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx
@@ -106,6 +106,11 @@ namespace pinocchio
         const Eigen::MatrixBase<ConfigVectorType> & q,
         const Eigen::MatrixBase<TangentVectorType> & v)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
         typedef typename Data::Motion Motion;
 
@@ -169,7 +174,8 @@ namespace pinocchio
 
         Force & fi = data.of[i];
 
-        jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector();
+        jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -=
+          Jcols.transpose() * fi.toVector();
 
         jdata.U().noalias() = Ia * Jcols;
         jdata.StU().noalias() = Jcols.transpose() * jdata.U();
@@ -184,8 +190,9 @@ namespace pinocchio
         {
           Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
 
-          fi.toVector().noalias() += Ia * data.oa_gf[i].toVector()
-                                     + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
+          fi.toVector().noalias() +=
+            Ia * data.oa_gf[i].toVector()
+            + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
           data.oYaba[parent] += Ia;
           data.of[parent] += fi;
         }
@@ -379,6 +386,11 @@ namespace pinocchio
         const Eigen::MatrixBase<ConfigVectorType> & q,
         const Eigen::MatrixBase<TangentVectorType> & v)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
 
         const JointIndex i = jmodel.id();
@@ -426,13 +438,15 @@ namespace pinocchio
 
         jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i];
         jmodel.calc_aba(
-          jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, parent > 0);
+          jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia,
+          parent > 0);
 
         if (parent > 0)
         {
           Force & pa = data.f[i];
-          pa.toVector().noalias() += Ia * data.a_gf[i].toVector()
-                                     + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
+          pa.toVector().noalias() +=
+            Ia * data.a_gf[i].toVector()
+            + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
           data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
           data.f[parent] += data.liMi[i].act(pa);
         }
@@ -621,6 +635,11 @@ namespace pinocchio
         Data & data,
         const Eigen::MatrixBase<ConfigVectorType> & q)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
 
         const JointIndex & i = jmodel.id();

diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx
@@ -35,6 +35,11 @@ namespace pinocchio
       Data & data,
       const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       typedef typename Model::JointIndex JointIndex;
       typedef typename Data::Motion Motion;
 

diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx
@@ -41,6 +41,11 @@ namespace pinocchio
         const Eigen::MatrixBase<ConfigVectorType> & q,
         const Eigen::MatrixBase<TangentVectorType> & v)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
         typedef
           typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type

diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx
@@ -41,6 +41,11 @@ namespace pinocchio
       const Model & model,
       Data & data)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       typedef typename Model::JointIndex JointIndex;
       typedef typename Data::Motion Motion;
 
@@ -81,8 +86,8 @@ namespace pinocchio
         Motion & oa_gf = data.oa_gf[i];
         ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
         const typename Data::TangentVectorType & a = data.ddq;
-        data.a[i] =
-          jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
+        data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
+                    + (data.v[i] ^ jdata.v());
         if (parent > 0)
           data.a[i] += data.liMi[i].actInv(data.a[parent]);
         oa = data.oMi[i].act(data.a[i]);

diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx
@@ -92,6 +92,11 @@ namespace pinocchio
       const Eigen::MatrixBase<ConfigVectorType> & q,
       const Eigen::MatrixBase<TangentVectorType> & v)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       typedef typename Model::JointIndex JointIndex;
       typedef typename Data::Motion Motion;
       typedef typename Data::Force Force;

diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx
@@ -27,6 +27,11 @@ namespace pinocchio
       const Model & model,
       Data & data)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       const JointIndex & i = jmodel.id();
       data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
       data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array()

diff --git a/include/pinocchio/algorithm/geometry.hxx b/include/pinocchio/algorithm/geometry.hxx
@@ -45,9 +45,7 @@ namespace pinocchio
     {
       const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint;
       if (joint_id > 0)
-        geom_data.oMg[i] =
-          (data.oMi[joint_id]
-           * geom_model.geometryObjects[i].placement);
+        geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement);
       else
         geom_data.oMg[i] = geom_model.geometryObjects[i].placement;
     }

diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx
@@ -54,6 +54,11 @@ namespace pinocchio
       const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq,
       const Eigen::MatrixBase<Matrix3xOut2> & v_partial_dv)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       typedef typename Model::JointIndex JointIndex;
       typedef typename Data::SE3 SE3;
       typedef typename Data::Motion Motion;
@@ -160,6 +165,7 @@ namespace pinocchio
       const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
       const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
     {
+
       typedef typename Model::JointIndex JointIndex;
       typedef typename Data::SE3 SE3;
       typedef typename Data::Motion Motion;

diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx
@@ -371,8 +371,7 @@ namespace pinocchio
       for (JointIndex i = jointId; i > 0; i = model.parents[i])
       {
         Pass::run(
-          model.joints[i], data.joints[i],
-          typename Pass::ArgsType(model, data, q.derived(), J_));
+          model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_));
       }
     }
 

diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx
@@ -51,6 +51,11 @@ namespace pinocchio
         const Eigen::MatrixBase<TangentVectorType1> & v,
         const Eigen::MatrixBase<TangentVectorType2> & a)
       {
+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");
+
         typedef typename Model::JointIndex JointIndex;
         typedef typename Data::SE3 SE3;
         typedef typename Data::Motion Motion;
@@ -76,7 +81,8 @@ namespace pinocchio
         if (parent > 0)
           vi += data.liMi[i].actInv(data.v[parent]);
 
-        ai = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v());
+        ai =
+          jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v());
         if (parent > 0)
           ai += data.liMi[i].actInv(data.a[parent]);
 

diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx
@@ -233,8 +233,8 @@ namespace pinocchio
         else
           data.oMi[i] = data.liMi[i];
 
-        data.a[i] =
-          jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
+        data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
+                    + (data.v[i] ^ jdata.v());
         data.a[i] += data.liMi[i].actInv(data.a[parent]);
       }
     };

diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx
@@ -191,8 +191,9 @@ namespace pinocchio
 
       if (parent > 0)
       {
-        pa.toVector().noalias() += Ia * data.a_bias[i].toVector()
-                                   + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
+        pa.toVector().noalias() +=
+          Ia * data.a_bias[i].toVector()
+          + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
         data.Yaba[parent] += impl::internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
         data.f[parent] += data.liMi[i].act(pa);
       }
@@ -311,7 +312,8 @@ namespace pinocchio
       const JointIndex parent = model.parents[i];
 
       // Abusing otherwise unused data.tau below.
-      jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i];
+      jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() =
+        jdata.S().transpose() * data.f[i];
       jmodel.jointVelocitySelector(data.u) -= jmodel.jointVelocitySelector(data.tau);
       data.f[i].toVector().noalias() -=
         jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.tau);

diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx
@@ -363,6 +363,11 @@ namespace pinocchio
       const Eigen::MatrixBase<TangentVectorType1> & v,
       const Eigen::MatrixBase<TangentVectorType2> & a)
     {
+      assert(
+        (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+         == false)
+        && "Algorithm not supported for mimic joints");
+
       typedef typename Model::JointIndex JointIndex;
 
       const JointIndex i = jmodel.id();

diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx
@@ -306,8 +306,8 @@ namespace pinocchio
         else
           data.oMi[i] = data.liMi[i];
 
-        data.a[i] =
-          jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
+        data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
+                    + (data.v[i] ^ jdata.v());
         if (parent > 0)
         {
           data.a[i] += data.liMi[i].actInv(data.a[parent]);

diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx
@@ -81,13 +81,13 @@ namespace pinocchio
       typedef
         typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
           ColsBlock;
-      ColsBlock J_cols =
-        jmodel.jointExtendedModelCols(data.J); // data.J has all the phi (in world frame) stacked in columns
-      ColsBlock psid_cols =
-        jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame
+      ColsBlock J_cols = jmodel.jointExtendedModelCols(
+        data.J); // data.J has all the phi (in world frame) stacked in columns
+      ColsBlock psid_cols = jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame
       ColsBlock psidd_cols =
         jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame
-      ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame
+      ColsBlock dJ_cols =
+        jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame
 
       J_cols.noalias() =
         data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint
@@ -98,8 +98,8 @@ namespace pinocchio
         ov, psid_cols,
         psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs
       ov += vJ;
-      oa +=
-        (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c());
+      oa += (ov ^ vJ)
+            + data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c());
       motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i))
                                                     // + vJ Composite rigid body inertia
       Inertia & oY = data.oYcrb[i];

diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx
@@ -195,8 +195,9 @@ namespace pinocchio
 
   template<typename InputType, typename ReturnType>
   struct jointConfigExtendedModelSelectorVisitor
-  : fusion::
-      JointUnaryVisitorBase<jointConfigExtendedModelSelectorVisitor<InputType, ReturnType>, ReturnType>
+  : fusion::JointUnaryVisitorBase<
+      jointConfigExtendedModelSelectorVisitor<InputType, ReturnType>,
+      ReturnType>
   {
     typedef boost::fusion::vector<InputType> ArgsType;
 

diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp
@@ -542,8 +542,7 @@ namespace pinocchio
       return A.middleCols(Base::i_j, nj());
     }
     template<typename D>
-    typename SizeDepType<NV>::template ColsReturn<D>::Type
-    jointCols(Eigen::MatrixBase<D> & A) const
+    typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
     {
       return A.middleCols(Base::i_v, nv());
     }

diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp
@@ -435,8 +435,7 @@ namespace pinocchio
     // Non-const access
     // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?)
     template<typename D>
-    typename SizeDepType<NV>::template ColsReturn<D>::Type
-    jointCols(Eigen::MatrixBase<D> & A) const
+    typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
     {
       return derived().jointCols_impl(A.derived());
     }
@@ -494,8 +493,7 @@ namespace pinocchio
 
     // Non-const access
     template<typename D>
-    typename SizeDepType<NV>::template RowsReturn<D>::Type
-    jointRows(Eigen::MatrixBase<D> & A) const
+    typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const
     {
       return derived().jointRows_impl(A.derived());
     }