Fix build

CMakeLists.txt

@@ -109,8 +109,8 @@ if(CMAKE_VERSION VERSION_GREATER "3.12")
   cmake_policy(SET CMP0074 NEW)
 endif()
 
-# Force C++ standard to be C++11 at least
-check_minimal_cxx_standard(11 ENFORCE)
+# Force C++ standard to be C++17 at least
+check_minimal_cxx_standard(17 ENFORCE)
 
 # --- OPTIONS ----------------------------------------
 option(BUILD_BENCHMARK "Build the benchmarks" OFF)
@@ -212,7 +212,7 @@ set(PINOCCHIO_MODEL_DIR "${PROJECT_SOURCE_DIR}/models")
 # --- DEPENDENCIES -----------------------------------
 # ----------------------------------------------------
 set(CMAKE_MODULE_PATH "${JRL_CMAKE_MODULES}/find-external/CppAD/" ${CMAKE_MODULE_PATH})
-add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
+add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.4.0")
 
 if(BUILD_WITH_URDF_SUPPORT)
   add_project_dependency(urdfdom_headers REQUIRED)

bindings/python/algorithm/admm-solver.cpp

@@ -168,6 +168,7 @@ namespace pinocchio
             ;
         }
 
+#ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
         class_
           .def(
             "solve",
@@ -200,7 +201,7 @@ namespace pinocchio
              bp::arg("admm_update_rule") = ADMMUpdateRule::SPECTRAL,
              bp::arg("stat_record") = false),
             "Solve the constrained conic problem, starting from the optional initial guess.");
-#ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
+  #ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
         {
           typedef Eigen::AccelerateLLT<context::SparseMatrix> AccelerateLLT;
           typedef DelassusOperatorSparseTpl<context::Scalar, context::Options, AccelerateLLT>
@@ -216,8 +217,10 @@ namespace pinocchio
              bp::arg("stat_record") = false),
             "Solve the constrained conic problem, starting from the optional initial guess.");
         }
-#endif
+  #endif // ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
+#endif   // ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
 
+#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
         bp::def(
           "computeConeProjection",
           computeConeProjection_wrapper<ConstraintModel, ConstraintModelAllocator>,
@@ -247,6 +250,7 @@ namespace pinocchio
           computeDeSaxeCorrection_wrapper<ConstraintModel, ConstraintModelAllocator>,
           bp::args("constraint_models", "velocities"),
           "Compute the complementarity shift associated to the De Saxé function.");
+#endif // ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
       }
       //
       //      template<typename S, int O>

bindings/python/algorithm/constraints/expose-cones.cpp

@@ -13,6 +13,29 @@
 // #include "pinocchio/bindings/python/serialization/serialization.hpp"
 #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
 
+#ifdef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
+namespace eigenpy
+{
+  // has_operator_equal return true for CoulombFrictionCone and DualCoulombFrictionCone with casadi
+  // but it should not.
+  // We provide specialization to enforce good result.
+  template<>
+  struct has_operator_equal<
+    pinocchio::python::context::CoulombFrictionCone,
+    pinocchio::python::context::CoulombFrictionCone>
+  {
+    typedef std::false_type type;
+  };
+  template<>
+  struct has_operator_equal<
+    pinocchio::python::context::DualCoulombFrictionCone,
+    pinocchio::python::context::DualCoulombFrictionCone>
+  {
+    typedef std::false_type type;
+  };
+} // namespace eigenpy
+#endif // ifdef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
+
 namespace pinocchio
 {
   namespace python

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

@@ -119,7 +119,7 @@ namespace pinocchio
 
       ContactCholeskyDecompositionPythonVisitor<context::ContactCholeskyDecomposition>::expose();
 
-      exposeConstraintDynamicsFor<RigidConstraintModel>();
+      exposeConstraintDynamicsFor<context::RigidConstraintModel>();
       // exposeConstraintDynamicsFor<WeldConstraintModel>();
     }
   } // namespace python

bindings/python/algorithm/pgs-solver.cpp

@@ -57,6 +57,7 @@ namespace pinocchio
       void run(ConstraintModelBase<ConstraintModel> * ptr = 0)
       {
         PINOCCHIO_UNUSED_VARIABLE(ptr);
+#ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
         class_
           .def(
             "solve", solve_wrapper<context::MatrixXs, ConstraintModel>,
@@ -72,6 +73,7 @@ namespace pinocchio
              bp::arg("solve_ncp") = true, bp::arg("stat_record") = false),
             "Solve the constrained conic problem composed of problem data (G,g,cones) and starting "
             "from the initial guess.");
+#endif // ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
       }
 
       //      template<typename S, int O>

development/scripts/pixi/activation.bat

@@ -23,3 +23,4 @@ if not defined PINOCCHIO_CODEGEN_SUPPORT (set PINOCCHIO_CODEGEN_SUPPORT=OFF)
 if not defined PINOCCHIO_SDF_SUPPORT (set PINOCCHIO_SDF_SUPPORT=OFF)
 if not defined PINOCCHIO_MPFR_SUPPORT (set PINOCCHIO_MPFR_SUPPORT=OFF)
 if not defined PINOCCHIO_BUILD_BENCHMARK (set PINOCCHIO_BUILD_BENCHMARK=OFF)
+if not defined PINOCCHIO_BUILD_WITH_TRACY(set PINOCCHIO_BUILD_WITH_TRACY=OFF)

development/scripts/pixi/activation.sh

@@ -51,3 +51,4 @@ export PINOCCHIO_CODEGEN_SUPPORT=${PINOCCHIO_CODEGEN_SUPPORT:=OFF}
 export PINOCCHIO_SDF_SUPPORT=${PINOCCHIO_SDF_SUPPORT:=OFF}
 export PINOCCHIO_MPFR_SUPPORT=${PINOCCHIO_MPFR_SUPPORT:=OFF}
 export PINOCCHIO_BUILD_BENCHMARK=${PINOCCHIO_BUILD_BENCHMARK:=OFF}
+export PINOCCHIO_BUILD_WITH_TRACY=${PINOCCHIO_BUILD_WITH_TRACY:=OFF}

include/pinocchio/algorithm/constraints/box-set.hpp

@@ -8,6 +8,7 @@
 #include "pinocchio/math/fwd.hpp"
 #include "pinocchio/algorithm/constraints/fwd.hpp"
 #include "pinocchio/algorithm/constraints/set-base.hpp"
+#include "pinocchio/math/matrix.hpp"
 
 namespace pinocchio
 {
@@ -60,7 +61,8 @@ namespace pinocchio
     , m_ub(ub)
     {
       PINOCCHIO_CHECK_INPUT_ARGUMENT(
-        (m_lb.array() <= m_ub.array()).all(), "Some components of lb are greater than ub");
+        arrayCompareAll(m_lb, m_ub, internal::ComparisonOperators::LE),
+        "Some components of lb are greater than ub");
     }
 
     /// \brief Copy constructor.
@@ -127,7 +129,7 @@ namespace pinocchio
       const Eigen::MatrixBase<VectorLikeIn> & x,
       const Eigen::MatrixBase<VectorLikeOut> & res_) const
     {
-      res_.const_cast_derived() = x.array().max(m_lb.array()).min(m_ub.array());
+      pinocchio::arrayBound(x, m_lb, m_ub, res_);
     }
 
     /// \brief Project a vector x such that scale * res is in the box.

include/pinocchio/algorithm/constraints/cone-base.hpp

@@ -5,6 +5,8 @@
 #ifndef __pinocchio_algorithm_constraints_cone_base_hpp__
 #define __pinocchio_algorithm_constraints_cone_base_hpp__
 
+#include "pinocchio/math/matrix.hpp"
+
 #include "pinocchio/algorithm/constraints/fwd.hpp"
 #include "pinocchio/algorithm/constraints/set-base.hpp"
 
@@ -37,7 +39,7 @@ namespace pinocchio
     {
       assert(x.size() == scale.size() && " x and scale should have the same size.");
       assert(
-        scale.isApprox(scale(0) * VectorLikeIn2::Ones(scale.size()))
+        pinocchio::isApprox(scale, scale(0) * VectorLikeIn2::Ones(scale.size()))
         && "Only scalar scaling are supported.");
       PINOCCHIO_UNUSED_VARIABLE(scale); // the cone is preserved when scaled by a scalar
       return project(x, x_proj);

include/pinocchio/algorithm/constraints/coulomb-friction-cone.hpp

@@ -8,7 +8,9 @@
 #include "pinocchio/algorithm/constraints/fwd.hpp"
 #include "pinocchio/algorithm/constraints/cone-base.hpp"
 #include "pinocchio/math/fwd.hpp"
+#include "pinocchio/math/matrix.hpp"
 #include "pinocchio/math/comparison-operators.hpp"
+#include "pinocchio/utils/check.hpp"
 
 namespace pinocchio
 {
@@ -67,7 +69,7 @@ namespace pinocchio
     explicit CoulombFrictionConeTpl(const Scalar mu)
     : mu(mu)
     {
-      assert(mu >= 0 && "mu must be positive");
+      assert(check_expression_if_real<Scalar>(mu >= 0) && "mu must be positive");
     }
 
     /// \brief Copy constructor.
@@ -122,7 +124,7 @@ namespace pinocchio
       const Eigen::MatrixBase<Vector3LikeIn> & x,
       const Eigen::MatrixBase<Vector3LikeOut> & res_) const
     {
-      assert(mu >= 0 && "mu must be positive");
+      assert(check_expression_if_real<Scalar>(mu >= 0) && "mu must be positive");
       //      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
       assert(x.size() == 3 && "The input vector is of wrong size.");
       typedef Eigen::Matrix<Scalar, 2, 1> Vector2Plain;
@@ -135,12 +137,12 @@ namespace pinocchio
       const Vector2Plain t = x.template head<2>();
       const Scalar t_norm = t.norm();
 
-      if (mu * t_norm <= -z)
+      if (check_expression_if_real<Scalar>(mu * t_norm <= -z))
       {
         res.setZero();
         return;
       }
-      else if (t_norm <= mu_z)
+      else if (check_expression_if_real<Scalar>(t_norm <= mu_z))
       {
         res = x;
         return;
@@ -149,7 +151,7 @@ namespace pinocchio
       {
         res.template head<2>() = (mu / t_norm) * t;
         res[2] = 1;
-        res.normalize();
+        pinocchio::normalize(res);
         const Scalar scale = x.dot(res);
         res *= scale;
         return;
@@ -166,11 +168,11 @@ namespace pinocchio
     typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like1) weightedProject(
       const Eigen::MatrixBase<Vector3Like1> & x, const Eigen::MatrixBase<Vector3Like2> & R) const
     {
-      assert(mu >= 0 && "mu must be positive");
+      assert(check_expression_if_real<Scalar>(mu >= 0) && "mu must be positive");
       //      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
       assert(x.size() == 3 && "The input vector is of wrong size.");
-      assert(R(2) > 0 && "R(2) must be strictly positive");
-      assert(R(0) == R(1) && "R(0) must be equal to R(1)");
+      assert(check_expression_if_real<Scalar>(R(2) > 0) && "R(2) must be strictly positive");
+      assert(check_expression_if_real<Scalar>(R(0) == R(1)) && "R(0) must be equal to R(1)");
 
       typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like1) Vector3Plain;
 
@@ -220,7 +222,7 @@ namespace pinocchio
 
       res[2] = math::max(Scalar(0), f[2]);
       const Scalar mu_fz = mu * res[2];
-      if (ft_norm > mu_fz)
+      if (check_expression_if_real<Scalar>(ft_norm > mu_fz))
       {
         res.template head<2>() = Scalar(mu_fz / ft_norm) * ft;
       }
@@ -300,7 +302,7 @@ namespace pinocchio
     explicit DualCoulombFrictionConeTpl(const Scalar mu)
     : mu(mu)
     {
-      assert(mu >= 0 && "mu must be positive");
+      assert(check_expression_if_real<Scalar>(mu >= 0) && "mu must be positive");
     }
 
     /// \brief Copy constructor.
@@ -342,7 +344,7 @@ namespace pinocchio
       const Eigen::MatrixBase<Vector3LikeIn> & x,
       const Eigen::MatrixBase<Vector3LikeOut> & res_) const
     {
-      assert(mu >= 0 && "mu must be positive");
+      assert(check_expression_if_real<Scalar>(mu >= 0) && "mu must be positive");
       //      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
       assert(x.size() == 3 && "The input vector is of wrong size.");
       const Scalar & z = x[2];
@@ -352,12 +354,12 @@ namespace pinocchio
       const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
       const Scalar t_norm = t.norm();
 
-      if (t_norm <= -mu * z)
+      if (check_expression_if_real<Scalar>(t_norm <= -mu * z))
       {
         res.setZero();
         return;
       }
-      else if (mu * t_norm <= z)
+      else if (check_expression_if_real<Scalar>(mu * t_norm <= z))
       {
         res = x;
         return;
@@ -366,7 +368,7 @@ namespace pinocchio
       {
         res.template head<2>() = t;
         res[2] = mu * t_norm;
-        res.normalize();
+        pinocchio::normalize(res);
         const Scalar scale = x.dot(res);
         res *= scale;
         return;

include/pinocchio/algorithm/constraints/joint-limit-constraint.hxx

@@ -85,16 +85,18 @@ namespace pinocchio
         const int q_index = idx_q + j_qi;
         const int q_reduce_index = nq_reduce + j_qi;
 
-        if (!(lb[q_index] == -std::numeric_limits<Scalar>::max()
-              || lb[q_index] == -std::numeric_limits<Scalar>::infinity()))
+        if (!(check_expression_if_real<Scalar>(lb[q_index] == -std::numeric_limits<Scalar>::max())
+              || check_expression_if_real<Scalar>(
+                lb[q_index] == -std::numeric_limits<Scalar>::infinity())))
         {
           activable_idx_rows_lower.push_back(idx_row);
           activable_idx_qs_lower.push_back(q_index);
           activable_idx_qs_reduce_lower.push_back(q_reduce_index);
           is_joint_really_active = true;
         }
-        if (!(ub[q_index] == +std::numeric_limits<Scalar>::max()
-              || ub[q_index] == +std::numeric_limits<Scalar>::infinity()))
+        if (!(check_expression_if_real<Scalar>(ub[q_index] == +std::numeric_limits<Scalar>::max())
+              || check_expression_if_real<Scalar>(
+                ub[q_index] == +std::numeric_limits<Scalar>::infinity())))
         {
           activable_idx_rows_upper.push_back(idx_row);
           activable_idx_qs_upper.push_back(q_index);
@@ -161,7 +163,7 @@ namespace pinocchio
     {
       const auto activable_idx_q = activable_idx_qs[i];
       bound_position_limit[bound_row_id] = lb[activable_idx_q];
-      assert(marg[activable_idx_q] >= 0);
+      assert(check_expression_if_real<Scalar>(marg[activable_idx_q] >= 0));
       bound_position_margin[bound_row_id] = marg[activable_idx_q];
       bound_row_id++;
     }
@@ -170,7 +172,7 @@ namespace pinocchio
     {
       const auto activable_idx_q = activable_idx_qs[i];
       bound_position_limit[bound_row_id] = ub[activable_idx_q];
-      assert(marg[activable_idx_q] >= 0);
+      assert(check_expression_if_real<Scalar>(marg[activable_idx_q] >= 0));
       bound_position_margin[bound_row_id] = marg[activable_idx_q];
       bound_row_id++;
     }
@@ -264,7 +266,8 @@ namespace pinocchio
       const Eigen::DenseIndex ie = static_cast<Eigen::DenseIndex>(i);
       const Eigen::DenseIndex idx_q = activable_idx_qs[i];
       activable_constraint_residual[ie] = bound_position_limit[ie] - data.q_in[idx_q];
-      if (activable_constraint_residual[ie] >= -bound_position_margin[ie])
+      if (check_expression_if_real<Scalar>(
+            activable_constraint_residual[ie] >= -bound_position_margin[ie]))
       {
         active_set_indexes.push_back(i);
         active_idx_rows.push_back(activable_idx_rows[i]);
@@ -281,7 +284,8 @@ namespace pinocchio
       const Eigen::DenseIndex ie = static_cast<Eigen::DenseIndex>(i);
       const Eigen::DenseIndex idx_q = activable_idx_qs[i];
       activable_constraint_residual[ie] = bound_position_limit[ie] - data.q_in[idx_q];
-      if (activable_constraint_residual[ie] <= bound_position_margin[ie])
+      if (check_expression_if_real<Scalar>(
+            activable_constraint_residual[ie] <= bound_position_margin[ie]))
       {
         active_set_indexes.push_back(i);
         active_idx_rows.push_back(activable_idx_rows[i]);

include/pinocchio/algorithm/constraints/orthant-cone.hpp

@@ -6,6 +6,7 @@
 #define __pinocchio_algorithm_constraints_orthant_cone_hpp__
 
 #include "pinocchio/math/fwd.hpp"
+#include "pinocchio/math/matrix.hpp"
 #include "pinocchio/algorithm/constraints/cone-base.hpp"
 
 namespace pinocchio
@@ -227,7 +228,7 @@ namespace pinocchio
       const Eigen::MatrixBase<VectorLikeIn> & x,
       const Eigen::MatrixBase<VectorLikeOut> & res_) const
     {
-      res_.const_cast_derived() = x.array().min(Scalar(0)).matrix();
+      pinocchio::arrayMin(x, Scalar(0), res_);
     }
 
     /// \brief Project the value given as input for the given row index.

include/pinocchio/algorithm/constraints/point-constraint-model-base.hpp

@@ -420,7 +420,7 @@ namespace pinocchio
                                      - (af1.linear() + vf1.angular().cross(vf1.linear()));
       acceleration_error.noalias() -= af1.angular().cross(position_error);
       acceleration_error.noalias() += vf1.angular().cross(vf1.angular().cross(position_error));
-      acceleration_error.noalias() -= 2 * vf1.angular().cross(velocity_error_component1);
+      acceleration_error.noalias() -= Scalar(2.) * vf1.angular().cross(velocity_error_component1);
 
       cdata.A1_world = this->getA1(cdata, WorldFrameTag());
       cdata.A2_world = this->getA2(cdata, WorldFrameTag());

include/pinocchio/algorithm/contact-inverse-dynamics.hpp

@@ -106,7 +106,7 @@ namespace pinocchio
         const Scalar contact_complementarity = cone.computeConicComplementarity(
           Vector3(sigma_segment + desaxce_correction), lambda_segment);
         const Scalar dual_feasibility =
-          std::abs(math::min(0., sigma_segment(2))); // proxy of dual feasibility
+          math::fabs(math::min(Scalar(0.), sigma_segment(2))); // proxy of dual feasibility
         settings.absolute_residual = math::max(
           settings.absolute_residual, math::max(contact_complementarity, dual_feasibility));