updated the latest code from stack-of-tasks-master

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

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 <iostream>
@@ -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<Scalar> 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
         //------------------------------------------------//

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];

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++) {

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;
 

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)

src/algorithm/rnea-derivatives-SO.hpp → 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<ConfigVectorType> &q,
     const Eigen::MatrixBase<TangentVectorType1> &v,
     const Eigen::MatrixBase<TangentVectorType2> &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);
 }
 

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.
     ///

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))

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<VectorXd> mq2(data2.d2tau_dq.data(), (data2.d2tau_dq).size());
-  Map<VectorXd> mv2(data2.d2tau_dv.data(), (data2.d2tau_dv).size());
+  Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size());
+  Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size());
   Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size());
   Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());