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