From 9a21e1261eadbec0655743f99b79b0b1e82d63cb Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 10:32:39 +0200 Subject: [PATCH 01/15] examples: mv autodiff to CppAD folder --- examples/CMakeLists.txt | 2 +- examples/{autodiff => cppad}/CMakeLists.txt | 2 +- examples/{autodiff => cppad}/autodiff-rnea.cpp | 0 examples/{autodiff => cppad}/autodiff-rnea.py | 0 4 files changed, 2 insertions(+), 2 deletions(-) rename examples/{autodiff => cppad}/CMakeLists.txt (88%) rename examples/{autodiff => cppad}/autodiff-rnea.cpp (100%) rename examples/{autodiff => cppad}/autodiff-rnea.py (100%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b3a4593b36..a591daa098 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -118,5 +118,5 @@ if(BUILD_PYTHON_INTERFACE) endforeach() endif() -add_subdirectory(autodiff) +add_subdirectory(cppad) add_subdirectory(codegen) diff --git a/examples/autodiff/CMakeLists.txt b/examples/cppad/CMakeLists.txt similarity index 88% rename from examples/autodiff/CMakeLists.txt rename to examples/cppad/CMakeLists.txt index 7fcc0ab86a..988eabb246 100644 --- a/examples/autodiff/CMakeLists.txt +++ b/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() diff --git a/examples/autodiff/autodiff-rnea.cpp b/examples/cppad/autodiff-rnea.cpp similarity index 100% rename from examples/autodiff/autodiff-rnea.cpp rename to examples/cppad/autodiff-rnea.cpp diff --git a/examples/autodiff/autodiff-rnea.py b/examples/cppad/autodiff-rnea.py similarity index 100% rename from examples/autodiff/autodiff-rnea.py rename to examples/cppad/autodiff-rnea.py From b134acb32958c118d45194055644de61c3daf5b7 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 15:26:54 +0200 Subject: [PATCH 02/15] examples: move CasADi files to a dedicated folder --- bindings/python/CMakeLists.txt | 8 ++++---- examples/casadi/CMakeLists.txt | 16 ++++++++++++++++ .../{cartpole-casadi.py => casadi/cartpole.py} | 0 .../quadrotor-ocp.py} | 0 .../simulation-pendulum-variational.ipynb} | 0 5 files changed, 20 insertions(+), 4 deletions(-) create mode 100644 examples/casadi/CMakeLists.txt rename examples/{cartpole-casadi.py => casadi/cartpole.py} (100%) rename examples/{casadi-quadrotor-ocp.py => casadi/quadrotor-ocp.py} (100%) rename examples/{simulation-pendulum-variational-casadi.ipynb => casadi/simulation-pendulum-variational.ipynb} (100%) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 6998a7d2ab..96f5b77af6 100644 --- a/bindings/python/CMakeLists.txt +++ b/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}) diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt new file mode 100644 index 0000000000..8fc5a9e2af --- /dev/null +++ b/examples/casadi/CMakeLists.txt @@ -0,0 +1,16 @@ +# +# Copyright (c) 2024 INRIA +# + +if(BUILD_WITH_CASADI_SUPPORT) + 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() diff --git a/examples/cartpole-casadi.py b/examples/casadi/cartpole.py similarity index 100% rename from examples/cartpole-casadi.py rename to examples/casadi/cartpole.py diff --git a/examples/casadi-quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py similarity index 100% rename from examples/casadi-quadrotor-ocp.py rename to examples/casadi/quadrotor-ocp.py diff --git a/examples/simulation-pendulum-variational-casadi.ipynb b/examples/casadi/simulation-pendulum-variational.ipynb similarity index 100% rename from examples/simulation-pendulum-variational-casadi.ipynb rename to examples/casadi/simulation-pendulum-variational.ipynb From f3729236473077fcef391267a34f4d913d883063 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 15:32:26 +0200 Subject: [PATCH 03/15] cmake: fix missing include --- examples/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index a591daa098..dc1629b45c 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -107,10 +107,6 @@ 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") @@ -118,5 +114,6 @@ if(BUILD_PYTHON_INTERFACE) endforeach() endif() +add_subdirectory(casadi) add_subdirectory(cppad) add_subdirectory(codegen) From a54d0d6b1e2cc0ea1b5c815019b74d627351ccd5 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 16:16:32 +0200 Subject: [PATCH 04/15] examples/casadi: fix cartpole --- examples/casadi/cartpole.py | 56 ++++++++++--------------------------- 1 file changed, 15 insertions(+), 41 deletions(-) diff --git a/examples/casadi/cartpole.py b/examples/casadi/cartpole.py index 115bf67820..7ec0cb2675 100644 --- a/examples/casadi/cartpole.py +++ b/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) From 0f9b8ce35486a4607ed47f0d89e988736b42e18b Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 17:37:01 +0200 Subject: [PATCH 05/15] examples/casadi: fix ocp example --- examples/casadi/quadrotor-ocp.py | 32 +++++++++++++++++++------------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/examples/casadi/quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py index 5ede2c982b..d2fabf7466 100644 --- a/examples/casadi/quadrotor-ocp.py +++ b/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__": From 44bcb05d9ba3103b3e971e06677fb24cbb5d0e67 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 18:44:27 +0200 Subject: [PATCH 06/15] examples/casadi: add RNEA example --- examples/casadi/CMakeLists.txt | 7 +++ examples/casadi/casadi-rnea.cpp | 83 +++++++++++++++++++++++++++++++++ 2 files changed, 90 insertions(+) create mode 100644 examples/casadi/casadi-rnea.cpp diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt index 8fc5a9e2af..321e2f8e16 100644 --- a/examples/casadi/CMakeLists.txt +++ b/examples/casadi/CMakeLists.txt @@ -2,7 +2,14 @@ # 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-rnea) + if(BUILD_PYTHON_INTERFACE) set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp cartpole) diff --git a/examples/casadi/casadi-rnea.cpp b/examples/casadi/casadi-rnea.cpp new file mode 100644 index 0000000000..3f9e3dc66e --- /dev/null +++ b/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 Model; + typedef Model::Data Data; + + typedef ModelTpl 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(); + 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(static_cast>(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(static_cast>(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(static_cast>(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 q_vec((size_t)model.nq); + Eigen::Map(q_vec.data(), model.nq, 1) = q; + + std::vector v_vec((size_t)model.nv); + Eigen::Map(v_vec.data(), model.nv, 1) = v; + + std::vector a_vec((size_t)model.nv); + Eigen::Map(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( + static_cast>(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; +} From afc5d0adbfa1737abdad0ae2edb72ee56c3594de Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 18:51:59 +0200 Subject: [PATCH 07/15] examples/casadi: add ABA example --- examples/casadi/CMakeLists.txt | 1 + examples/casadi/casadi-aba.cpp | 84 ++++++++++++++++++++++++++++++++++ 2 files changed, 85 insertions(+) create mode 100644 examples/casadi/casadi-aba.cpp diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt index 321e2f8e16..fd8fd9ab4c 100644 --- a/examples/casadi/CMakeLists.txt +++ b/examples/casadi/CMakeLists.txt @@ -8,6 +8,7 @@ endfunction() if(BUILD_WITH_CASADI_SUPPORT) + add_pinocchio_cpp_casadi_example(casadi-aba) add_pinocchio_cpp_casadi_example(casadi-rnea) if(BUILD_PYTHON_INTERFACE) diff --git a/examples/casadi/casadi-aba.cpp b/examples/casadi/casadi-aba.cpp new file mode 100644 index 0000000000..984a018801 --- /dev/null +++ b/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 Model; + typedef Model::Data Data; + + typedef ModelTpl 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(); + 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(static_cast>(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(static_cast>(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(static_cast>(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 q_vec((size_t)model.nq); + Eigen::Map(q_vec.data(), model.nq, 1) = q; + + std::vector v_vec((size_t)model.nv); + Eigen::Map(v_vec.data(), model.nv, 1) = v; + + std::vector tau_vec((size_t)model.nv); + Eigen::Map(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( + static_cast>(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; +} From c9634436acc5d2c96176f394993d99b40027cb5c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 19:13:30 +0200 Subject: [PATCH 08/15] changelog: sync --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4aeb52228f..35c19924d7 100644 --- a/CHANGELOG.md +++ b/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 From 00f6c19399781e6a6ed6793f6d3c1fec8a533af8 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 21:22:33 +0200 Subject: [PATCH 09/15] examples/casadi: remove useless functions --- examples/casadi/cartpole.py | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/examples/casadi/cartpole.py b/examples/casadi/cartpole.py index 7ec0cb2675..10ad184f1c 100644 --- a/examples/casadi/cartpole.py +++ b/examples/casadi/cartpole.py @@ -90,7 +90,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 q = self.q_node v = self.v_node u = self.u_node @@ -113,24 +112,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 - ) - - 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 nv = self.model.nv From 196a90450a1a03434f4b9bc4189d5ff836717437 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 21:23:10 +0200 Subject: [PATCH 10/15] cmake: fix CasADi example name for Python --- examples/casadi/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt index fd8fd9ab4c..c5a1548b46 100644 --- a/examples/casadi/CMakeLists.txt +++ b/examples/casadi/CMakeLists.txt @@ -16,7 +16,7 @@ if(BUILD_WITH_CASADI_SUPPORT) set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp cartpole) foreach(EXAMPLE ${${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES}) - set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}") + 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() From f281f3c144d0a2b94d6611e19b09d3a3828ac374 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 21:31:56 +0200 Subject: [PATCH 11/15] example: remove Python warning --- examples/casadi/quadrotor-ocp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/casadi/quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py index d2fabf7466..c85ac4de16 100644 --- a/examples/casadi/quadrotor-ocp.py +++ b/examples/casadi/quadrotor-ocp.py @@ -274,7 +274,7 @@ def main(): try: import matplotlib.pyplot as plt - fig0, axs0 = plt.subplots(nrows=2) + _, axs0 = plt.subplots(nrows=2) xs = np.vstack(oc_problem.xs) axs0[0].plot(xs[:, :3]) From 765dae315fd9f0bcf9fe2444f50faeabe35aac2d Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 21:40:01 +0200 Subject: [PATCH 12/15] examples: fix cartpole examples when meshcat is missing --- examples/casadi/cartpole.py | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/examples/casadi/cartpole.py b/examples/casadi/cartpole.py index 10ad184f1c..812399b00b 100644 --- a/examples/casadi/cartpole.py +++ b/examples/casadi/cartpole.py @@ -2,8 +2,9 @@ import hppfcl as fcl import numpy as np import pinocchio as pin -import pinocchio.visualize +from pinocchio.visualize import MeshcatVisualizer import pinocchio.casadi as cpin +import sys def make_cartpole(ub=True): @@ -168,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, :].T -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) From 6bfed34a8e1726a1611f215fa680d87d4f9078b2 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 26 Aug 2024 22:04:25 +0200 Subject: [PATCH 13/15] cmake: fix CasADi examples --- examples/casadi/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt index c5a1548b46..d3a334636c 100644 --- a/examples/casadi/CMakeLists.txt +++ b/examples/casadi/CMakeLists.txt @@ -13,7 +13,10 @@ if(BUILD_WITH_CASADI_SUPPORT) if(BUILD_PYTHON_INTERFACE) - set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp cartpole) + 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}") From e1f0088e09f395e649d4005b42669be25bfefb38 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 27 Aug 2024 09:10:34 +0200 Subject: [PATCH 14/15] core: fix namespace ambiguity on Windows systems --- include/pinocchio/autodiff/casadi.hpp | 30 +++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/include/pinocchio/autodiff/casadi.hpp b/include/pinocchio/autodiff/casadi.hpp index 3ee1638764..0340d0121e 100644 --- a/include/pinocchio/autodiff/casadi.hpp +++ b/include/pinocchio/autodiff/casadi.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019-2021 INRIA CNRS +// Copyright (c) 2019-2024 INRIA CNRS // #ifndef __pinocchio_autodiff_casadi_hpp__ @@ -38,7 +38,7 @@ namespace boost // This is a workaround to make the code compiling with Eigen. namespace casadi { - inline bool operator||(const bool x, const casadi::Matrix & /*y*/) + inline bool operator||(const bool x, const ::casadi::Matrix & /*y*/) { return x; } @@ -95,12 +95,12 @@ namespace Eigen /// @brief Eigen::NumTraits<> specialization for casadi::SX /// template - struct NumTraits> + struct NumTraits<::casadi::Matrix> { - using Real = casadi::Matrix; - using NonInteger = casadi::Matrix; - using Literal = casadi::Matrix; - using Nested = casadi::Matrix; + using Real = ::casadi::Matrix; + using NonInteger = ::casadi::Matrix; + using Literal = ::casadi::Matrix; + using Nested = ::casadi::Matrix; enum { @@ -118,24 +118,24 @@ namespace Eigen MulCost = 2 }; - static casadi::Matrix epsilon() + static ::casadi::Matrix epsilon() { - return casadi::Matrix(std::numeric_limits::epsilon()); + return ::casadi::Matrix(std::numeric_limits::epsilon()); } - static casadi::Matrix dummy_precision() + static ::casadi::Matrix dummy_precision() { - return casadi::Matrix(NumTraits::dummy_precision()); + return ::casadi::Matrix(NumTraits::dummy_precision()); } - static casadi::Matrix highest() + static ::casadi::Matrix highest() { - return casadi::Matrix(std::numeric_limits::max()); + return ::casadi::Matrix(std::numeric_limits::max()); } - static casadi::Matrix lowest() + static ::casadi::Matrix lowest() { - return casadi::Matrix(std::numeric_limits::min()); + return ::casadi::Matrix(std::numeric_limits::min()); } static int digits10() From e3a055463998e1817e1f4e9a5c030f425497139c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 27 Aug 2024 09:28:18 +0200 Subject: [PATCH 15/15] examples/casadi: fix Python warnings --- examples/casadi/quadrotor-ocp.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/casadi/quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py index c85ac4de16..a5d0636c60 100644 --- a/examples/casadi/quadrotor-ocp.py +++ b/examples/casadi/quadrotor-ocp.py @@ -290,8 +290,12 @@ def main(): ax.plot(us[:, idx]) plt.show(block=False) - except Exception: - pass + except ImportError as err: + print( + "Error while initializing the viewer. It seems you should install Python meshcat" + ) + print(err) + sys.exit(0) if __name__ == "__main__":