Restructure CppAD and CasADi examples

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,24 @@
+#
+# 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 cartpole)
+
+    foreach(EXAMPLE ${${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES})
+      set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${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,6 +2,7 @@
 import hppfcl as fcl
 import numpy as np
 import pinocchio as pin
+import pinocchio.visualize
 import pinocchio.casadi as cpin
 
 
@@ -90,7 +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
@@ -116,46 +116,20 @@ def create_discrete_dynamics(self):
         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"],
-        )
+        keys = [
+            "jac_qnext_dq_",
+            "jac_qnext_u",
+            "jac_qnext_v",
+            "jac_vnext_dq_",
+            "jac_vnext_u",
+            "jac_vnext_v",
+        ]
+        jac = []
+        for k in keys:
+            jac.append(self.dyn_jac_expr[k])
+        self.dyn_jac_expr = jac
+        self.dyn_jac_fn = casadi.Function("Ddyn", [q, v, u], self.dyn_jac_expr)
 
     def forward(self, x, u):
         nq = self.model.nq
@@ -223,5 +197,5 @@ def integrate_no_control(x0, nsteps):
 viz.initViewer()
 viz.loadViewerModel("pinocchio")
 
-qs_ = states_[: model.nq, :]
+qs_ = states_[: model.nq, :].T
 viz.play(q_trajectory=qs_, dt=dt)

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;
+}

examples/casadi-quadrotor-ocp.py → examples/casadi/quadrotor-ocp.py

@@ -9,7 +9,10 @@
 # This use the example-robot-data submodule, but if you have it already properly
 # installed in your PYTHONPATH, there is no need for this sys.path thing
 path = join(
-    dirname(dirname(abspath(__file__))), "models", "example-robot-data", "python"
+    dirname(dirname(dirname(abspath(__file__)))),
+    "models",
+    "example-robot-data",
+    "python",
 )
 sys.path.append(path)
 import example_robot_data  # noqa: E402
@@ -268,24 +271,27 @@ def main():
     oc_problem.solve(approx_hessian=True)
 
     # --------------PLOTS-----------
-    import matplotlib.pyplot as plt
+    try:
+        import matplotlib.pyplot as plt
 
-    fig0, axs0 = plt.subplots(nrows=2)
+        fig0, axs0 = plt.subplots(nrows=2)
 
-    xs = np.vstack(oc_problem.xs)
-    axs0[0].plot(xs[:, :3])
-    axs0[0].set_title("Quadcopter position")
+        xs = np.vstack(oc_problem.xs)
+        axs0[0].plot(xs[:, :3])
+        axs0[0].set_title("Quadcopter position")
 
-    axs0[1].plot(oc_problem.gaps["norm"])
-    axs0[1].set_title("Multiple shooting node gaps")
+        axs0[1].plot(oc_problem.gaps["norm"])
+        axs0[1].set_title("Multiple shooting node gaps")
 
-    fig1, axs1 = plt.subplots(nrows=4)
-    us = np.vstack(oc_problem.us)
+        _, axs1 = plt.subplots(nrows=4)
+        us = np.vstack(oc_problem.us)
 
-    for idx, ax in enumerate(axs1):
-        ax.plot(us[:, idx])
+        for idx, ax in enumerate(axs1):
+            ax.plot(us[:, idx])
 
-    plt.show(block=False)
+        plt.show(block=False)
+    except Exception:
+        pass
 
 
 if __name__ == "__main__":

examples/autodiff/CMakeLists.txt → examples/cppad/CMakeLists.txt

@@ -17,7 +17,7 @@ if(BUILD_WITH_AUTODIFF_SUPPORT)
 
     foreach(EXAMPLE ${${PROJECT_NAME}_AUTODIFF_PYTHON_EXAMPLES})
       set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}")
-      add_python_unit_test(${EXAMPLE_NAME} "examples/autodiff/${EXAMPLE}.py" "bindings/python")
+      add_python_unit_test(${EXAMPLE_NAME} "examples/cppad/${EXAMPLE}.py" "bindings/python")
       add_windows_dll_path_to_test(${EXAMPLE_NAME})
     endforeach()
   endif()