Merge pull request #2637 from jcarpent/topic/examples

Add CRBA CasADi example

Author: jcarpent

examples/casadi/CMakeLists.txt

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2024 INRIA
+# Copyright (c) 2025 INRIA
 #
 
 function(ADD_PINOCCHIO_CPP_CASADI_EXAMPLE EXAMPLE)
@@ -9,6 +9,7 @@ endfunction()
 if(BUILD_WITH_CASADI_SUPPORT)
 
   add_pinocchio_cpp_casadi_example(casadi-aba)
+  add_pinocchio_cpp_casadi_example(casadi-crba)
   add_pinocchio_cpp_casadi_example(casadi-rnea)
 
   if(BUILD_PYTHON_INTERFACE)

examples/casadi/casadi-crba.cpp

@@ -0,0 +1,73 @@
+#include "pinocchio/autodiff/casadi.hpp"
+
+#include "pinocchio/multibody/sample-models.hpp"
+
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+
+int main(int /*argc*/, char ** /*argv*/)
+{
+  using namespace pinocchio;
+
+  typedef double Scalar;
+  typedef ::casadi::SX ADScalar;
+
+  typedef ModelTpl<Scalar> Model;
+  typedef Model::Data Data;
+
+  typedef ModelTpl<ADScalar> ADModel;
+  typedef ADModel::Data ADData;
+
+  // Create a random humanoid model
+  Model model;
+  buildModels::humanoidRandom(model);
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill(1.);
+  Data data(model);
+
+  // Pick up random configuration, velocity and acceleration vectors.
+  Eigen::VectorXd q(model.nq);
+  q = randomConfiguration(model);
+
+  // Create CasADi model and data from model
+  typedef ADModel::ConfigVectorType ConfigVectorAD;
+  ADModel ad_model = model.cast<ADScalar>();
+  ADData ad_data(ad_model);
+
+  // Create symbolic CasADi vectors
+  ::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);
+  ConfigVectorAD q_ad(model.nq);
+  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
+
+  // Build CasADi function
+  crba(ad_model, ad_data, q_ad, pinocchio::Convention::WORLD);
+  ad_data.M.triangularView<Eigen::StrictlyLower>() =
+    ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
+  ::casadi::SX M_ad(model.nv, model.nv);
+  for (Eigen::DenseIndex j = 0; j < model.nv; ++j)
+  {
+    for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
+    {
+      M_ad(i, j) = ad_data.M(i, j);
+    }
+  }
+
+  ::casadi::Function eval_crba("eval_crba", ::casadi::SXVector{cs_q}, ::casadi::SXVector{M_ad});
+
+  // Evaluate CasADi expression with real value
+  std::vector<double> q_vec((size_t)model.nq);
+  Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;
+
+  ::casadi::DM M_res = eval_crba(::casadi::DMVector{q_vec})[0];
+  Data::MatrixXs M_mat =
+    Eigen::Map<Data::MatrixXs>(static_cast<std::vector<double>>(M_res).data(), model.nv, model.nv);
+
+  // Eval CRBA using classic Pinocchio model
+  pinocchio::crba(model, data, q);
+  data.M.triangularView<Eigen::StrictlyLower>() =
+    data.M.transpose().triangularView<Eigen::StrictlyLower>();
+
+  // Print both results
+  std::cout << "pinocchio double:\n" << "\tM =\n" << data.M << std::endl;
+  std::cout << "pinocchio CasADi:\n" << "\tM =\n" << M_mat.transpose() << std::endl;
+}

include/pinocchio/spatial/inertia.hpp

@@ -1,5 +1,6 @@
 //
-// Copyright (c) 2015-2021 CNRS INRIA
+// Copyright (c) 2015-2018 CNRS
+// Copyright (c) 2018-2025 INRIA
 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 
@@ -1210,7 +1211,7 @@ namespace pinocchio
       const Scalar exp_2d1 = exp(2 * d1);
       const Scalar exp_2d2 = exp(2 * d2);
       const Scalar exp_2d3 = exp(2 * d3);
-      const Scalar exp_d1 = exp(d1);
+      // const Scalar exp_d1 = exp(d1);
       const Scalar exp_d2 = exp(d2);
       const Scalar exp_d3 = exp(d3);
 

unittest/casadi/spatial.cpp

@@ -99,7 +99,6 @@ BOOST_AUTO_TEST_CASE(test_log3_firstorder_derivatives)
 
   typedef pinocchio::SE3Tpl<ADScalar> SE3AD;
   typedef SE3AD::Vector3 Vector3AD;
-  typedef SE3AD::Matrix3 Matrix3AD;
 
   SE3::Matrix3 RTarget;
   pinocchio::toRotationMatrix(Vector3(SE3::Vector3::UnitX()), pinocchio::PI<Scalar>() / 4, RTarget);