diff --git a/CMakeLists.txt b/CMakeLists.txt index 9748e0d588..0af40d2f56 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ ENDIF() # --- OPTIONS ---------------------------------------- OPTION(BUILD_BENCHMARK "Build the benchmarks" OFF) OPTION(BUILD_UTILS "Build the utils" OFF) -OPTION(BUILD_PYTHON_INTERFACE "Build the Python bindings" ON) +OPTION(BUILD_PYTHON_INTERFACE "Build the Python bindings" OFF) option(GENERATE_PYTHON_STUBS "Generate the Python stubs associated to the Python library" OFF) OPTION(BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit version" OFF) @@ -68,13 +68,13 @@ OPTION(BUILD_ADVANCED_TESTING "Build the advanced tests (multiprecision, etc.) o # --- OPTIONAL DEPENDENCIES ------------------------- OPTION(BUILD_WITH_URDF_SUPPORT "Build the library with the URDF format support" ON) OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision support (required HPP-FCL)" OFF) -OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" OFF) +OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" ON) OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the support of CASADI" ON) -OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the support of code generation (via CppADCodeGen)" OFF) +OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the support of code generation (via CppADCodeGen)" ON) OPTION(BUILD_WITH_OPENMP_SUPPORT "Build the library with the OpenMP support" OFF) cmake_dependent_option(LINK_PYTHON_INTERFACE_TO_OPENMP "Link OpenMP to the Python interface" ON "BUILD_PYTHON_INTERFACE;BUILD_WITH_OPENMP_SUPPORT" OFF) -cmake_dependent_option(BUILD_WITH_LIBPYTHON "Link to libpython to embed an interpreter for the python_parser feature" ON +cmake_dependent_option(BUILD_WITH_LIBPYTHON "Link to libpython to embed an interpreter for the python_parser feature" OFF "BUILD_PYTHON_INTERFACE" OFF) OPTION(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF) @@ -136,8 +136,8 @@ ENDIF(BUILD_WITH_OPENMP_SUPPORT) SET(BOOST_REQUIRED_COMPONENTS filesystem serialization system) -SET_BOOST_DEFAULT_OPTIONS() -EXPORT_BOOST_DEFAULT_OPTIONS() +# SET_BOOST_DEFAULT_OPTIONS() +# EXPORT_BOOST_DEFAULT_OPTIONS() ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS}) IF(Boost_VERSION_STRING VERSION_LESS 1.81) @@ -158,7 +158,7 @@ IF(BUILD_PYTHON_INTERFACE) set(PYTHON_COMPONENTS ${PYTHON_COMPONENTS} Development) ENDIF() - ADD_PROJECT_DEPENDENCY(eigenpy 2.7.10 REQUIRED) + # ADD_PROJECT_DEPENDENCY(eigenpy 2.7.10 REQUIRED) IF(BUILD_WITH_LIBPYTHON) # Check wether this a PyPy Python diff --git a/benchmark/AD_mdof_v1.cpp b/benchmark/AD_mdof_v1.cpp index dbd8ba2700..f45596f25b 100755 --- a/benchmark/AD_mdof_v1.cpp +++ b/benchmark/AD_mdof_v1.cpp @@ -29,7 +29,7 @@ This version compares the CPU Runtime for #include "pinocchio/parsers/urdf.hpp" #include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/container/aligned-vector.hpp" -#include "pinocchio/algorithm/rnea-derivatives-SO.hpp" +#include "pinocchio/algorithm/rnea-second-order-derivatives.hpp" #include "pinocchio/algorithm/rnea-derivatives-faster.hpp" #include "pinocchio/codegen/code-generator-algo.hpp" #include @@ -66,14 +66,14 @@ int main(int argc, const char* argv[]) std::cout << "(the time score in debug mode is not relevant) " << std::endl; #endif - int n_models = 1; + int n_models = 5; string str_robotname[n_models]; str_robotname[0] = "double_pendulum"; // double pendulum - // str_robotname[1] = "ur3_robot"; // UR3 - // str_robotname[2] = "hyq"; // hyq - // str_robotname[3] = "baxter_simple"; // baxter_simple - // str_robotname[4] = "atlas"; // atlas + str_robotname[1] = "ur3_robot"; // UR3 + str_robotname[2] = "hyq"; // hyq + str_robotname[3] = "baxter_simple"; // baxter_simple + str_robotname[4] = "atlas"; // atlas // str_robotname[5] = "talos_full_v2"; // talos_full_v2 char tmp[256]; @@ -155,10 +155,12 @@ int main(int argc, const char* argv[]) //----------------------------------------------------// // Compute RNEA FO derivatives faster-----------------// //----------------------------------------------------// + std::cout << "Running first case!" << std::endl; MatrixXd drnea_dq(MatrixXd::Zero(model.nv, model.nv)); MatrixXd drnea_dv(MatrixXd::Zero(model.nv, model.nv)); MatrixXd drnea_da(MatrixXd::Zero(model.nv, model.nv)); + std::cout << "Running first case algo!" << std::endl; timer.tic(); SMOOTH(NBT) @@ -173,7 +175,8 @@ int main(int argc, const char* argv[]) //----------------------------------------------------------// // Compute RNEA FO derivatives using CppAD (w/out codegen)--// //----------------------------------------------------------// - + + typedef double Scalar; typedef AD ADScalar; @@ -349,7 +352,7 @@ int main(int argc, const char* argv[]) timer.tic(); SMOOTH(NBT_SO) { - computeRNEADerivativesSO(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth], dtau2_dq_ana, dtau2_dv_ana, + ComputeRNEASecondOrderDerivatives(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth], dtau2_dq_ana, dtau2_dv_ana, dtau2_dqv_ana, M_FO_ana); } time_ABA[3] = timer.toc() / NBT_SO; // @@ -460,27 +463,27 @@ int main(int argc, const char* argv[]) typename Data::Tensor3x d2tau_dq2_ADc, d2tau_dv2_ADc, d2tau_dqv_ADc, d2tau_daq_ADc; std::string strfun_d2tdq = robot_name + std::string("_d2tau_dq"); - ::casadi::SX d2tau_dq = jacobian(cs_dtau_dq, cs_v_int); + ::casadi::SX d2tau_dqdq = jacobian(cs_dtau_dq, cs_v_int); ::casadi::Function eval_d2tau_dq( - strfun_d2tdq, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dq}); + strfun_d2tdq, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dqdq}); // d2tau_dv2 std::string strfun_d2tdv = robot_name + std::string("_d2tau_dv"); - ::casadi::SX d2tau_dv = jacobian(cs_dtau_dv, cs_v); + ::casadi::SX d2tau_dvdv = jacobian(cs_dtau_dv, cs_v); ::casadi::Function eval_d2tau_dv( - strfun_d2tdv, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dv}); + strfun_d2tdv, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dvdv}); // d2tau_dqdv std::string strfun_d2tdqv = robot_name + std::string("_d2tau_dqv"); - ::casadi::SX d2tau_dqv = jacobian(cs_dtau_dq, cs_v); + ::casadi::SX d2tau_dqdv = jacobian(cs_dtau_dq, cs_v); ::casadi::Function eval_d2tau_dqv( - strfun_d2tdqv, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dqv}); + strfun_d2tdqv, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dqdv}); // d2tau_dqda std::string strfun_d2tdaq = robot_name + std::string("_d2tau_daq"); - ::casadi::SX d2tau_daq = jacobian(cs_dtau_da, cs_v_int); + ::casadi::SX d2tau_dadq = jacobian(cs_dtau_da, cs_v_int); ::casadi::Function eval_d2tau_daq( - strfun_d2tdaq, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_daq}); + strfun_d2tdaq, ::casadi::SXVector {cs_q, cs_v_int, cs_v, cs_a}, ::casadi::SXVector {d2tau_dadq}); string compile_command; @@ -547,7 +550,7 @@ int main(int argc, const char* argv[]) //----------------------------------------------------------------------// //--------------NOT TIMED ----------------------------------------------// - computeRNEADerivativesSO(adc_model, adc_data, q_int_ad, v_ad, a_ad); + ComputeRNEASecondOrderDerivatives(adc_model, adc_data, q_int_ad, v_ad, a_ad); std::string strfun_d2tdq_ana_cg = robot_name + std::string("_d2tau_dq_ana_cg"); std::string strfun_d2tdv_ana_cg = robot_name + std::string("_d2tau_dv_ana_cg"); @@ -562,8 +565,8 @@ int main(int argc, const char* argv[]) cas_mat4(model.nv, model.nv); for (int k = 0; k < model.nv; k++) { - get_mat_from_tens3_v2(adc_data.d2tau_dq, cas_mat1, model.nv, k); - get_mat_from_tens3_v2(adc_data.d2tau_dv, cas_mat2, model.nv, k); + get_mat_from_tens3_v2(adc_data.d2tau_dqdq, cas_mat1, model.nv, k); + get_mat_from_tens3_v2(adc_data.d2tau_dvdv, cas_mat2, model.nv, k); get_mat_from_tens3_v2(adc_data.d2tau_dqdv, cas_mat3, model.nv, k); get_mat_from_tens3_v2(adc_data.d2tau_dadq, cas_mat4, model.nv, k); @@ -612,7 +615,10 @@ int main(int argc, const char* argv[]) = "gcc -fPIC -shared -O3 -march=native " + strfun_MFO_ana_cg + ".c -o " + strfun_MFO_ana_cg + ".so "; flag = system(compile_command.c_str()); } - + + + + //------------------------------------------------// // Writing all the timings to the file //------------------------------------------------// diff --git a/benchmark/AD_mdof_v2.cpp b/benchmark/AD_mdof_v2.cpp index ded5597e16..cc77d2ea51 100644 --- a/benchmark/AD_mdof_v2.cpp +++ b/benchmark/AD_mdof_v2.cpp @@ -58,14 +58,14 @@ int main(int argc, const char* argv[]) std::cout << "(the time score in debug mode is not relevant) " << std::endl; #endif - int n_models = 1; // no of robots to be used + int n_models = 4; // no of robots to be used string str_robotname[n_models]; str_robotname[0] = "double_pendulum"; // double pendulum - // str_robotname[1] = "ur3_robot"; // UR3 - // str_robotname[2] = "hyq"; // hyq - // str_robotname[3] = "baxter_simple"; // baxter_simple - // str_robotname[4] = "atlas"; // atlas + str_robotname[1] = "ur3_robot"; // UR3 + str_robotname[2] = "hyq"; // hyq + str_robotname[3] = "baxter_simple"; // baxter_simple + str_robotname[4] = "atlas"; // atlas // str_robotname[5] = "talos_full_v2"; // Talos char tmp[256]; diff --git a/benchmark/AD_mdof_v3.cpp b/benchmark/AD_mdof_v3.cpp index 8f34d03ad8..8ec8dcce03 100644 --- a/benchmark/AD_mdof_v3.cpp +++ b/benchmark/AD_mdof_v3.cpp @@ -567,9 +567,9 @@ int main(int argc, const char* argv[]) // cas_mat4(model.nv, model.nv); // for (int k = 0; k < model.nv; k++) { - // get_mat_from_tens3_v2(adc_data.d2tau_dq, cas_mat1, model.nv, k); - // get_mat_from_tens3_v2(adc_data.d2tau_dv, cas_mat2, model.nv, k); - // get_mat_from_tens3_v2(adc_data.d2tau_dqv, cas_mat3, model.nv, k); + // get_mat_from_tens3_v2(adc_data.d2tau_dqdq, cas_mat1, model.nv, k); + // get_mat_from_tens3_v2(adc_data.d2tau_dvdv, cas_mat2, model.nv, k); + // get_mat_from_tens3_v2(adc_data.d2tau_dqdv, cas_mat3, model.nv, k); // get_mat_from_tens3_v2(adc_data.M_FO, cas_mat4, model.nv, k); // for (int j = 0; j < model.nv; j++) { diff --git a/benchmark/timings-derivatives b/benchmark/timings-derivatives index 2236e3afe7..58087110ce 100755 Binary files a/benchmark/timings-derivatives and b/benchmark/timings-derivatives differ diff --git a/benchmark/timings_cg_v7.cpp b/benchmark/timings_cg_v7.cpp index f21c438d30..de840edcac 100644 --- a/benchmark/timings_cg_v7.cpp +++ b/benchmark/timings_cg_v7.cpp @@ -107,8 +107,8 @@ int main(int argc, const char **argv) { computeRNEADerivativesSO(model, data, q, v, a, dtau2_dq_ana, dtau2_dv_ana, dtau2_dqv_ana, dtau_dadq_ana); - // dtau2_dq_ana = data.d2tau_dq; - // dtau2_dv_ana = data.d2tau_dv; + // dtau2_dq_ana = data.d2tau_dqdq; + // dtau2_dv_ana = data.d2tau_dvdv; // dtau2_dqv_ana = data.d2tau_dqdv; // dtau_dadq_ana = data.d2tau_dadq; diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 05dd837537..4dd8f78e94 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -3,7 +3,7 @@ # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. # -include(${JRL_CMAKE_MODULES}/python-helpers.cmake) +# include(${JRL_CMAKE_MODULES}/python-helpers.cmake) # --- PYTHON TARGET --- # SET(PYWRAP ${PROJECT_NAME}_pywrap) diff --git a/src/algorithm/aba_v2.hpp b/include/pinocchio/algorithm/aba_v2.hpp similarity index 100% rename from src/algorithm/aba_v2.hpp rename to include/pinocchio/algorithm/aba_v2.hpp diff --git a/src/algorithm/aba_v2.hxx b/include/pinocchio/algorithm/aba_v2.hxx similarity index 100% rename from src/algorithm/aba_v2.hxx rename to include/pinocchio/algorithm/aba_v2.hxx diff --git a/src/algorithm/rnea-derivatives-SO.hpp b/include/pinocchio/algorithm/rnea-derivatives-SO.hpp similarity index 97% rename from src/algorithm/rnea-derivatives-SO.hpp rename to include/pinocchio/algorithm/rnea-derivatives-SO.hpp index 772f3ab9cd..50fce76bff 100644 --- a/src/algorithm/rnea-derivatives-SO.hpp +++ b/include/pinocchio/algorithm/rnea-derivatives-SO.hpp @@ -98,7 +98,7 @@ inline void computeRNEADerivativesSO( /// \param[in] v The joint velocity vector (dim model.nv). /// \param[in] a The joint acceleration vector (dim model.nv). /// -/// \returns The results are stored in data.d2tau_dq, data.d2tau_dv, +/// \returns The results are stored in data.d2tau_dqdq, data.d2tau_dvdv, /// data.d2tau_dqdv, and data.d2tau_dadq which respectively correspond to the /// Second-Order partial derivatives of the joint torque vector with respect to /// the joint configuration, velocity and cross Second-Order partial derivatives @@ -129,13 +129,13 @@ inline void computeRNEADerivativesSO( const Eigen::MatrixBase &q, const Eigen::MatrixBase &v, const Eigen::MatrixBase &a) { - (data.d2tau_dq).setZero(); - (data.d2tau_dv).setZero(); + (data.d2tau_dqdq).setZero(); + (data.d2tau_dvdv).setZero(); (data.d2tau_dqdv).setZero(); (data.d2tau_dadq).setZero(); computeRNEADerivativesSO(model, data, q.derived(), v.derived(), a.derived(), - data.d2tau_dq, data.d2tau_dv, data.d2tau_dqdv, + data.d2tau_dqdq, data.d2tau_dvdv, data.d2tau_dqdv, data.d2tau_dadq); } diff --git a/src/algorithm/rnea-derivatives-SO.hxx b/include/pinocchio/algorithm/rnea-derivatives-SO.hxx similarity index 100% rename from src/algorithm/rnea-derivatives-SO.hxx rename to include/pinocchio/algorithm/rnea-derivatives-SO.hxx diff --git a/src/algorithm/rnea-derivatives-faster.hpp b/include/pinocchio/algorithm/rnea-derivatives-faster.hpp similarity index 100% rename from src/algorithm/rnea-derivatives-faster.hpp rename to include/pinocchio/algorithm/rnea-derivatives-faster.hpp diff --git a/src/algorithm/rnea-derivatives-faster.hxx b/include/pinocchio/algorithm/rnea-derivatives-faster.hxx similarity index 100% rename from src/algorithm/rnea-derivatives-faster.hxx rename to include/pinocchio/algorithm/rnea-derivatives-faster.hxx diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index 2d54068831..62d1db1342 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -213,7 +213,6 @@ namespace pinocchio RowMatrix6 M6tmpR; RowMatrix6 M6tmpR2; -<<<<<<< HEAD:src/multibody/data.hpp /// \brief Temporary for RNEA SO derivative algorithms Motion S_dm; Motion psid_dm; @@ -252,8 +251,6 @@ namespace pinocchio Vector6r vecu11; Vector6r vecu12; Vector6c vecu13; -======= ->>>>>>> 97a00a983e66fc0a58667f9671e2d806ba9b730b:include/pinocchio/multibody/data.hpp /// \brief The joint accelerations computed from ABA TangentVectorType ddq; @@ -431,31 +428,8 @@ namespace pinocchio /// \brief Matrix related to joint torque regressor MatrixXs jointTorqueRegressor; -<<<<<<< HEAD:src/multibody/data.hpp - /// \brief SO Partial derivative of the joint torque vector with respect to - /// the joint configuration. - Tensor3x d2tau_dq; - - /// \brief SO Partial derivative of the joint torque vector with respect to - /// the joint velocity. - Tensor3x d2tau_dv; - - /// \brief SO Cross-Partial derivative of the joint torque vector with - /// respect to the joint configuration/velocity. - Tensor3x d2tau_dqdv; - - /// \brief SO Cross-Partial derivative of the joint torque vector with - /// respect to the joint acceleration/configuration. This also equals to the - /// First Order partial derivative of the Mass Matrix w.r.t joint - /// configuration - Tensor3x d2tau_dadq; - - /// \brief Tensor containing the kinematic Hessian of all the joints. - Tensor3x kinematic_hessians; -======= - - /// \brief Tensor containing the kinematic Hessian of all the joints. + /// \brief Tensor containing the kinematic Hessian of all the joints. Tensor3x kinematic_hessians; /// \brief SO Partial derivative of the joint torque vector with respect to @@ -476,7 +450,7 @@ namespace pinocchio /// configuration. Tensor3x d2tau_dadq; ->>>>>>> 97a00a983e66fc0a58667f9671e2d806ba9b730b:include/pinocchio/multibody/data.hpp + /// /// \brief Default constructor of pinocchio::Data from a pinocchio::Model. /// diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 0556e0dcc5..3c0b57a3c1 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -25,6 +25,7 @@ namespace pinocchio , a_gf((std::size_t)model.njoints,Motion::Zero()) , oa_gf((std::size_t)model.njoints,Motion::Zero()) , v((std::size_t)model.njoints,Motion::Zero()) + , vJ((std::size_t)model.njoints, Motion::Zero()) , ov((std::size_t)model.njoints,Motion::Zero()) , f((std::size_t)model.njoints,Force::Zero()) , of((std::size_t)model.njoints,Force::Zero()) @@ -38,6 +39,7 @@ namespace pinocchio , oMf((std::size_t)model.nframes,SE3::Identity()) , Ycrb((std::size_t)model.njoints,Inertia::Zero()) , dYcrb((std::size_t)model.njoints,Inertia::Zero()) + , oBcrb((std::size_t)model.njoints, Coriolis::Zero()) , M(MatrixXs::Zero(model.nv,model.nv)) , Minv(MatrixXs::Zero(model.nv,model.nv)) , C(MatrixXs::Zero(model.nv,model.nv)) @@ -79,6 +81,11 @@ namespace pinocchio , ddJ(Matrix6x::Zero(6, model.nv)) , psid(Matrix6x::Zero(6, model.nv)) , psidd(Matrix6x::Zero(6, model.nv)) + , vdJ(Matrix6x::Zero(6, model.nv)) + , Ftmp1(Matrix6x::Zero(6, model.nv)) + , Ftmp2(Matrix6x::Zero(6, model.nv)) + , Ftmp3(Matrix6x::Zero(6, model.nv)) + , Ftmp4(Matrix6x::Zero(6, model.nv)) , dVdq(Matrix6x::Zero(6,model.nv)) , dAdq(Matrix6x::Zero(6,model.nv)) , dAdv(Matrix6x::Zero(6,model.nv)) diff --git a/src/spatial/spatial-coriolis.hpp b/include/pinocchio/spatial/spatial-coriolis.hpp similarity index 100% rename from src/spatial/spatial-coriolis.hpp rename to include/pinocchio/spatial/spatial-coriolis.hpp diff --git a/src/utils/tensor_utils.hpp b/include/pinocchio/utils/tensor_utils.hpp similarity index 100% rename from src/utils/tensor_utils.hpp rename to include/pinocchio/utils/tensor_utils.hpp diff --git a/unittest/rnea-derivatives-SO.cpp b/unittest/rnea-derivatives-SO.cpp index abb462c933..c3538da9ef 100644 --- a/unittest/rnea-derivatives-SO.cpp +++ b/unittest/rnea-derivatives-SO.cpp @@ -208,8 +208,8 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives_SO) { Data data2(model); computeRNEADerivativesSO(model, data2, q, v, a); - Map mq2(data2.d2tau_dq.data(), (data2.d2tau_dq).size()); - Map mv2(data2.d2tau_dv.data(), (data2.d2tau_dv).size()); + Map mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size()); + Map mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size()); Map mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size()); Map maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());