Merge pull request stack-of-tasks#2659 from jorisv/topic/fix-algo-custom-scalar

Fix custom scalar support

Author: jcarpent

CHANGELOG.md

@@ -8,6 +8,13 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ### Fixed
 - Fix getters for models and data in `VisualizerPythonVisitor` ([#2647](https://github.com/stack-of-tasks/pinocchio/pull/2647))
+- Fix `pinocchio::cholesky::Mv` build with `CppAd` scalar ([#2659](https://github.com/stack-of-tasks/pinocchio/pull/2659))
+- Fix `pinocchio::contactABA` build with `casadi` scalar ([#2659](https://github.com/stack-of-tasks/pinocchio/pull/2659))
+
+### Added
+- Add explicit template instantiation for constraint algorithms for `casadi`, `CppAD` and `CppADCodeGen` scalar ([#2659](https://github.com/stack-of-tasks/pinocchio/pull/2659))
+- Add `casadi` bindings for `pinocchio.initConstraintDynamics` and `pinocchio.constraintDynamics` ([#2659](https://github.com/stack-of-tasks/pinocchio/pull/2659))
+
 
 ## [3.5.0] - 2025-04-02
 

bindings/python/algorithm/expose-constrained-dynamics.cpp

@@ -25,8 +25,6 @@ namespace pinocchio
     typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData)
       RigidConstraintDataVector;
 
-#ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
-
     static const context::VectorXs constraintDynamics_proxy(
       const context::Model & model,
       context::Data & data,
@@ -53,8 +51,6 @@ namespace pinocchio
       return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas);
     }
 
-#endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
-
     void exposeConstraintDynamics()
     {
       using namespace Eigen;
@@ -77,7 +73,6 @@ namespace pinocchio
 
       StdVectorPythonVisitor<RigidConstraintDataVector>::expose("StdVec_RigidConstraintData");
 
-#ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
       ContactCholeskyDecompositionPythonVisitor<context::ContactCholeskyDecomposition>::expose();
 
       bp::def(
@@ -112,8 +107,6 @@ namespace pinocchio
         "This function returns joint acceleration of the system. The contact forces are "
         "stored in the list data.contact_forces.",
         mimic_not_supported_function<>(0));
-
-#endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
     }
   } // namespace python
 } // namespace pinocchio

include/pinocchio/algorithm/cholesky.hxx

@@ -363,7 +363,7 @@ namespace pinocchio
 
           for (int k = model.nv - 1; k >= 0; --k)
           {
-            vout_[k] = M.row(k).segment(k, nvt[(size_t)k]) * vin.segment(k, nvt[(size_t)k]);
+            vout_[k] = M.row(k).segment(k, nvt[(size_t)k]).dot(vin.segment(k, nvt[(size_t)k]));
             vout_.segment(k + 1, nvt[(size_t)k] - 1) +=
               M.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * vin[k];
           }

include/pinocchio/algorithm/constrained-dynamics.hxx

@@ -13,6 +13,7 @@
 #include "pinocchio/algorithm/contact-cholesky.hxx"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/math/fwd.hpp"
 #include <limits>
 
 namespace pinocchio
@@ -888,16 +889,15 @@ namespace pinocchio
           cdata.contact_acceleration_deviation.linear() +=
             contact1_velocity.angular().cross(contact1_velocity.linear());
 
-        using std::max;
         if (cmodel.type == CONTACT_3D)
         {
-          primal_infeasibility = max<Scalar>(
+          primal_infeasibility = math::max<Scalar>(
             primal_infeasibility,
             cdata.contact_acceleration_deviation.linear().template lpNorm<Eigen::Infinity>());
         }
         else
         {
-          primal_infeasibility = max<Scalar>(
+          primal_infeasibility = math::max<Scalar>(
             primal_infeasibility,
             cdata.contact_acceleration_deviation.toVector().template lpNorm<Eigen::Infinity>());
         }

include/pinocchio/algorithm/contact-cholesky.txx

@@ -101,16 +101,6 @@ namespace pinocchio
   ContactCholeskyDecompositionTpl<context::Scalar, context::Options>::inverse<context::MatrixXs>(
     const Eigen::MatrixBase<context::MatrixXs> &) const;
 
-  extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI bool
-    ContactCholeskyDecompositionTpl<context::Scalar, context::Options>::
-    operator== <context::Scalar, context::Options>(
-      const ContactCholeskyDecompositionTpl<context::Scalar, context::Options> &) const;
-
-  extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI bool
-    ContactCholeskyDecompositionTpl<context::Scalar, context::Options>::
-    operator!= <context::Scalar, context::Options>(
-      const ContactCholeskyDecompositionTpl<context::Scalar, context::Options> &) const;
-
   PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
 } // namespace pinocchio

include/pinocchio/bindings/python/algorithm/contact-cholesky.hpp

@@ -188,11 +188,11 @@ namespace pinocchio
               "Returns the Constraint Cholesky decomposition associated to this "
               "DelassusCholeskyExpression.",
               bp::return_internal_reference<>())
-
             .def(DelassusOperatorBasePythonVisitor<DelassusCholeskyExpression>());
         }
 
         {
+#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
           typedef DelassusOperatorDenseTpl<context::Scalar, context::Options> DelassusOperatorDense;
           bp::class_<DelassusOperatorDense>(
             "DelassusOperatorDense", "Delassus Cholesky dense operator from a dense matrix.",
@@ -201,9 +201,11 @@ namespace pinocchio
               bp::args("self", "matrix"), "Build from a given dense matrix"))
 
             .def(DelassusOperatorBasePythonVisitor<DelassusOperatorDense>());
+#endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
         }
 
         {
+#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
           typedef DelassusOperatorSparseTpl<context::Scalar, context::Options>
             DelassusOperatorSparse;
           bp::class_<DelassusOperatorSparse, boost::noncopyable>(
@@ -213,6 +215,7 @@ namespace pinocchio
               bp::args("self", "matrix"), "Build from a given sparse matrix"))
 
             .def(DelassusOperatorBasePythonVisitor<DelassusOperatorSparse>());
+#endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
         }
 #ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
         {

include/pinocchio/context/casadi.hpp

@@ -5,12 +5,6 @@
 #ifndef __pinocchio_context_casadi_hpp__
 #define __pinocchio_context_casadi_hpp__
 
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS_DERIVATIVES
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN
-#define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY
 #define PINOCCHIO_SKIP_ALGORITHM_MODEL
 #define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY
 #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS

include/pinocchio/context/cppad.hpp

@@ -7,12 +7,6 @@
 
 #include <cppad/cppad.hpp>
 
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS_DERIVATIVES
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN
-#define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY
 #define PINOCCHIO_SKIP_ALGORITHM_MODEL
 #define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY
 #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS

include/pinocchio/context/cppadcg.hpp

@@ -8,12 +8,6 @@
 #include <cppad/cg/support/cppadcg_eigen.hpp>
 #include <cppad/cppad.hpp>
 
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS_DERIVATIVES
-#define PINOCCHIO_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY
-#define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN
-#define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY
 #define PINOCCHIO_SKIP_ALGORITHM_MODEL
 #define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY
 #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS

include/pinocchio/math/eigenvalues.hpp

@@ -35,7 +35,7 @@ namespace pinocchio
     void run(const MatrixLike & mat)
     {
       PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
-      PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
+      PINOCCHIO_CHECK_INPUT_ARGUMENT((check_expression_if_real<Scalar, true>(rel_tol > 0)));
       Scalar eigenvalue_est = principal_eigen_vector.norm();
 
       for (it = 0; it < max_it; ++it)
@@ -68,7 +68,7 @@ namespace pinocchio
     void lowest(const MatrixLike & mat, const bool compute_largest = true)
     {
       PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
-      PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
+      PINOCCHIO_CHECK_INPUT_ARGUMENT((check_expression_if_real<Scalar, true>(rel_tol > 0)));
 
       if (compute_largest)
         run(mat);