Restructure CppAD and CasADi examples

CHANGELOG.md

@@ -21,6 +21,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Add compatibility with jrl-cmakemodules workspace ([#2333](https://github.com/stack-of-tasks/pinocchio/pull/2333))
 - Add ``collision_color`` parameter to `MeshcatVisualizer.loadViewerModel` ([#2350](https://github.com/stack-of-tasks/pinocchio/pull/2350))
 - Add ``BuildFromMJCF`` function to RobotWrapper ([#2363](https://github.com/stack-of-tasks/pinocchio/pull/2363))
+- Add more CasADi examples ([#2388](https://github.com/stack-of-tasks/pinocchio/pull/2388))
 
 ### Removed
 - Remove deprecated headers related to joint constraints ([#2382](https://github.com/stack-of-tasks/pinocchio/pull/2382))
@@ -29,6 +30,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Use eigenpy to expose `GeometryObject::meshMaterial` variant ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
 - GepettoViewer is no more the default viewer for RobotWrapper ([#2331](https://github.com/stack-of-tasks/pinocchio/pull/2331))
 - Modernize python code base with ruff ([#2367](https://github.com/stack-of-tasks/pinocchio/pull/2367))
+- Restructure CppAD and CasADi examples ([#2388](https://github.com/stack-of-tasks/pinocchio/pull/2388))
 
 ## [3.1.0] - 2024-07-04
 

bindings/python/CMakeLists.txt

@@ -42,7 +42,7 @@ function(PINOCCHIO_PYTHON_BINDINGS_SPECIFIC_TYPE scalar_name)
     # OpenMP is linked with pinocchio_parallel target
     target_compile_definitions(${PYTHON_LIB_NAME} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_OPENMP)
   endif()
-  add_dependencies(${PROJECT_NAME}_python ${PYTHON_LIB_NAME})
+  add_dependencies(${PROJECT_NAME}-python ${PYTHON_LIB_NAME})
 
   # Remove lib prefix for the target and use the right define for DLLAPI definition
   set_target_properties(${PYTHON_LIB_NAME} PROPERTIES PREFIX "" DEFINE_SYMBOL "${PYWRAP}_EXPORTS")
@@ -137,11 +137,11 @@ function(INSTALL_PYTHON_FILES)
 endfunction()
 
 if(BUILD_PYTHON_INTERFACE)
-  add_custom_target(${PROJECT_NAME}_python)
-  set_target_properties(${PROJECT_NAME}_python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+  add_custom_target(${PROJECT_NAME}-python)
+  set_target_properties(${PROJECT_NAME}-python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
 
   python_build_get_target(python_build_target)
-  add_dependencies(${PROJECT_NAME}_python ${python_build_target})
+  add_dependencies(${PROJECT_NAME}-python ${python_build_target})
 
   set(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.6.5")
   if(IS_ABSOLUTE ${PYTHON_SITELIB})

examples/CMakeLists.txt

@@ -107,16 +107,13 @@ if(BUILD_PYTHON_INTERFACE)
     list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES run-algo-in-parallel)
   endif()
 
-  if(BUILD_WITH_CASADI_SUPPORT)
-    list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES casadi-quadrotor-ocp)
-  endif()
-
   foreach(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES})
     set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}")
     add_python_unit_test("${EXAMPLE_NAME}" "examples/${EXAMPLE}.py" "bindings/python")
     add_windows_dll_path_to_test(${EXAMPLE_NAME})
   endforeach()
 endif()
 
-add_subdirectory(autodiff)
+add_subdirectory(casadi)
+add_subdirectory(cppad)
 add_subdirectory(codegen)

examples/casadi/CMakeLists.txt

@@ -0,0 +1,27 @@
+#
+# Copyright (c) 2024 INRIA
+#
+
+function(ADD_PINOCCHIO_CPP_CASADI_EXAMPLE EXAMPLE)
+  add_pinocchio_cpp_example(${EXAMPLE} CASADI)
+endfunction()
+
+if(BUILD_WITH_CASADI_SUPPORT)
+
+  add_pinocchio_cpp_casadi_example(casadi-aba)
+  add_pinocchio_cpp_casadi_example(casadi-rnea)
+
+  if(BUILD_PYTHON_INTERFACE)
+
+    set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp)
+    if(BUILD_WITH_COLLISION_SUPPORT)
+      list(APPEND ${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES cartpole)
+    endif()
+
+    foreach(EXAMPLE ${${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES})
+      set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-casadi-${EXAMPLE}")
+      add_python_unit_test(${EXAMPLE_NAME} "examples/casadi/${EXAMPLE}.py" "bindings/python")
+      add_windows_dll_path_to_test(${EXAMPLE_NAME})
+    endforeach()
+  endif()
+endif()

examples/cartpole-casadi.py → examples/casadi/cartpole.py

@@ -2,7 +2,9 @@
 import hppfcl as fcl
 import numpy as np
 import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer
 import pinocchio.casadi as cpin
+import sys
 
 
 def make_cartpole(ub=True):
@@ -89,8 +91,6 @@ def create_dynamics(self):
 
     def create_discrete_dynamics(self):
         """Create the map `(q,v) -> (qnext, vnext)` using semi-implicit Euler integration."""
-        nv = self.model.nv
-        nq = self.model.nq
         q = self.q_node
         v = self.v_node
         u = self.u_node
@@ -113,50 +113,6 @@ def create_discrete_dynamics(self):
             ["qnext", "vnext"],
         )
 
-        self.dyn_jac_expr = self.dyn_qv_fn_.jacobian()(
-            q=q, dq_=casadi.SX.zeros(nv), v=v, u=u
-        )
-        self.dyn_jac_expr = self.dyn_jac_expr["jac"][:, nq:]
-        print("dyn jac expr:", self.dyn_jac_expr.shape)
-        self.dyn_jac_fn = casadi.Function("Ddyn", [q, v, u], [self.dyn_jac_expr])
-
-        self.create_discrete_dynamics_state()
-
-    def create_discrete_dynamics_state(self):
-        nq = self.model.nq
-        nv = self.model.nv
-        dt = self.timestep
-        u = self.u_node
-        model = self.cmodel
-
-        # discrete dynamics in terms of x:
-        state = casadi.SX.sym("x", nq + nv)
-        self.x_node = state
-        q, v = casadi.vertsplit(state, nq)
-        dq_ = self.dq_
-        q_dq = self.q_dq
-
-        a = self.acc_func(q_dq, v, u)
-        vnext = v + a * dt
-
-        # implicit form
-        next_state = casadi.SX.sym("xnext", nq + nv)
-        qn, vn = casadi.vertsplit(next_state, nq)
-        dqn_ = casadi.SX.sym("dqn_", nv)
-        qn_dq = cpin.integrate(model, qn, dqn_)
-
-        res_q = cpin.difference(model, q_dq, qn_dq) - dt * vnext
-        res_v = (vn - v) - dt * a
-
-        residual = casadi.vertcat(res_q, res_v)
-        self.dyn_residual = casadi.Function(
-            "residual",
-            [state, u, next_state, dq_, dqn_],
-            [residual],
-            ["x", "u", "xnext", "dq_", "dqn_"],
-            ["r"],
-        )
-
     def forward(self, x, u):
         nq = self.model.nq
         nv = self.model.nv
@@ -213,15 +169,21 @@ def integrate_no_control(x0, nsteps):
 states_ = integrate_no_control(x0, nsteps=400)
 states_ = np.stack(states_).T
 
+try:
+    viz = MeshcatVisualizer(
+        model=model,
+        collision_model=cartpole.collision_model,
+        visual_model=cartpole.visual_model,
+    )
 
-viz = pin.visualize.MeshcatVisualizer(
-    model=model,
-    collision_model=cartpole.collision_model,
-    visual_model=cartpole.visual_model,
-)
-
-viz.initViewer()
-viz.loadViewerModel("pinocchio")
+    viz.initViewer()
+    viz.loadViewerModel("pinocchio")
 
-qs_ = states_[: model.nq, :]
-viz.play(q_trajectory=qs_, dt=dt)
+    qs_ = states_[: model.nq, :].T
+    viz.play(q_trajectory=qs_, dt=dt)
+except ImportError as err:
+    print(
+        "Error while initializing the viewer. It seems you should install Python meshcat"
+    )
+    print(err)
+    sys.exit(0)

examples/casadi/casadi-aba.cpp

@@ -0,0 +1,84 @@
+#include "pinocchio/autodiff/casadi.hpp"
+
+#include "pinocchio/multibody/sample-models.hpp"
+
+#include "pinocchio/algorithm/aba.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);
+  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
+  Eigen::VectorXd tau(Eigen::VectorXd::Random(model.nv));
+
+  // Create CasADi model and data from model
+  typedef ADModel::ConfigVectorType ConfigVectorAD;
+  typedef ADModel::TangentVectorType TangentVectorAD;
+  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);
+
+  ::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
+  TangentVectorAD v_ad(model.nv);
+  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
+
+  ::casadi::SX cs_tau = ::casadi::SX::sym("tau", model.nv);
+  TangentVectorAD tau_ad(model.nv);
+  tau_ad =
+    Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
+
+  // Build CasADi function
+  aba(ad_model, ad_data, q_ad, v_ad, tau_ad);
+  ::casadi::SX a_ad(model.nv, 1);
+
+  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
+    a_ad(k) = ad_data.ddq[k];
+
+  ::casadi::Function eval_aba(
+    "eval_aba", ::casadi::SXVector{cs_q, cs_v, cs_tau}, ::casadi::SXVector{a_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;
+
+  std::vector<double> v_vec((size_t)model.nv);
+  Eigen::Map<Eigen::VectorXd>(v_vec.data(), model.nv, 1) = v;
+
+  std::vector<double> tau_vec((size_t)model.nv);
+  Eigen::Map<Eigen::VectorXd>(tau_vec.data(), model.nv, 1) = tau;
+
+  ::casadi::DM a_casadi_res = eval_aba(::casadi::DMVector{q_vec, v_vec, tau_vec})[0];
+  Data::TangentVectorType a_casadi_vec = Eigen::Map<Data::TangentVectorType>(
+    static_cast<std::vector<double>>(a_casadi_res).data(), model.nv, 1);
+
+  // Eval ABA using classic Pinocchio model
+  pinocchio::aba(model, data, q, v, tau);
+
+  // Print both results
+  std::cout << "pinocchio double:\n" << "\ta = " << data.ddq.transpose() << std::endl;
+  std::cout << "pinocchio CasADi:\n" << "\ta = " << a_casadi_vec.transpose() << std::endl;
+}

examples/casadi/casadi-rnea.cpp

@@ -0,0 +1,83 @@
+#include "pinocchio/autodiff/casadi.hpp"
+
+#include "pinocchio/multibody/sample-models.hpp"
+
+#include "pinocchio/algorithm/rnea.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);
+  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
+  Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));
+
+  // Create CasADi model and data from model
+  typedef ADModel::ConfigVectorType ConfigVectorAD;
+  typedef ADModel::TangentVectorType TangentVectorAD;
+  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);
+
+  ::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
+  TangentVectorAD v_ad(model.nv);
+  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
+
+  ::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);
+  TangentVectorAD a_ad(model.nv);
+  a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
+
+  // Build CasADi function
+  rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
+  ::casadi::SX tau_ad(model.nv, 1);
+
+  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
+    tau_ad(k) = ad_data.tau[k];
+
+  ::casadi::Function eval_rnea(
+    "eval_rnea", ::casadi::SXVector{cs_q, cs_v, cs_a}, ::casadi::SXVector{tau_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;
+
+  std::vector<double> v_vec((size_t)model.nv);
+  Eigen::Map<Eigen::VectorXd>(v_vec.data(), model.nv, 1) = v;
+
+  std::vector<double> a_vec((size_t)model.nv);
+  Eigen::Map<Eigen::VectorXd>(a_vec.data(), model.nv, 1) = a;
+
+  ::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{q_vec, v_vec, a_vec})[0];
+  Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(
+    static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);
+
+  // Eval RNEA using classic Pinocchio model
+  pinocchio::rnea(model, data, q, v, a);
+
+  // Print both results
+  std::cout << "pinocchio double:\n" << "\ttau = " << data.tau.transpose() << std::endl;
+  std::cout << "pinocchio CasADi:\n" << "\ttau = " << tau_casadi_vec.transpose() << std::endl;
+}