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 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/CMakeLists.txt b/examples/CMakeLists.txt index b3a4593b36..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(autodiff) +add_subdirectory(casadi) +add_subdirectory(cppad) add_subdirectory(codegen) diff --git a/examples/casadi/CMakeLists.txt b/examples/casadi/CMakeLists.txt new file mode 100644 index 0000000000..d3a334636c --- /dev/null +++ b/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() diff --git a/examples/cartpole-casadi.py b/examples/casadi/cartpole.py similarity index 73% rename from examples/cartpole-casadi.py rename to examples/casadi/cartpole.py index 115bf67820..812399b00b 100644 --- a/examples/cartpole-casadi.py +++ b/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) 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; +} 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; +} diff --git a/examples/casadi-quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py similarity index 90% rename from examples/casadi-quadrotor-ocp.py rename to examples/casadi/quadrotor-ocp.py index 5ede2c982b..a5d0636c60 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,31 @@ 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) + _, 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 ImportError as err: + print( + "Error while initializing the viewer. It seems you should install Python meshcat" + ) + print(err) + sys.exit(0) if __name__ == "__main__": 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 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 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()