added the basic SO derivs algo WIP

benchmark/test_spatial_force.cpp

@@ -9,6 +9,7 @@
 #include "pinocchio/algorithm/spatial-force-derivatives.hpp"
 #include "pinocchio/algorithm/rnea-derivatives-faster.hpp"
 #include "pinocchio/algorithm/rnea-second-order-derivatives.hpp"
+#include "pinocchio/algorithm/spatial-force-second-order-derivatives.hpp"
 #include "pinocchio/algorithm/aba-derivatives.hpp"
 #include "pinocchio/algorithm/aba.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
@@ -300,14 +301,18 @@ int main(int argc, const char ** argv)
     //--------------------------------------------------------------------------------
     //------------------------- SO partials of cumulative force-----------------------
     //--------------------------------------------------------------------------------
-    std::vector<Eigen::Tensor<double,3>> d2f_dq2_fd;
-    std::vector<Eigen::Tensor<double,3>> d2f_dv2_fd;
-    std::vector<Eigen::Tensor<double,3>> d2f_da2_fd;
+    std::vector<Eigen::Tensor<double,3>> d2f_dq2_fd, d2f_dv2_fd, d2f_da2_fd;
+    std::vector<Eigen::Tensor<double,3>> d2f_dq2_ana, d2f_dv2_ana, d2f_da2_ana, d2f_daq_ana;
 
     for (int i = 0; i < model.njoints - 1; i++) {
         d2f_dq2_fd.emplace_back(6, model.nv, model.nv);  
         d2f_dv2_fd.emplace_back(6, model.nv, model.nv);  
         d2f_da2_fd.emplace_back(6, model.nv, model.nv);  
+
+        d2f_dq2_ana.emplace_back(6, model.nv, model.nv);  
+        d2f_dv2_ana.emplace_back(6, model.nv, model.nv);  
+        d2f_da2_ana.emplace_back(6, model.nv, model.nv);  
+        d2f_daq_ana.emplace_back(6, model.nv, model.nv);  
     }
 
     double alpha = 1e-6; // performs well
@@ -369,8 +374,38 @@ int main(int argc, const char ** argv)
 
         v_eps[k] -= alpha;
     }
+   //------- Analytical algorithm
+
+       ComputeSpatialForceSecondOrderDerivatives(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth],
+                                             d2f_dq2_ana, d2f_dv2_ana,d2f_da2_ana, d2f_daq_ana);
 
 
+    // Comparing the results
+    for (int i = 0; i < model.nv; ++i)
+    {
+
+      Eigen::Tensor<double,3> concrete_tensor = (d2f_dq2_fd.at(i) - d2f_dq2_ana.at(i)).eval();
+      auto diff_eq = tensorNorm(concrete_tensor);
+
+      // auto diff_dv = (d2f_dv2_fd.at(i) - d2f_dv2_ana.at(i)).norm();
+      // auto diff_da = (d2f_da2_fd.at(i) - d2f_da2_ana.at(i)).norm();
+
+      if (diff_eq > 1e-3)
+      {
+        std::cout << "d2f_dq2_fd.at(i) = \n" << d2f_dq2_fd.at(i) << std::endl;
+        std::cout << "d2f_dq2_ana.at(i) = \n" << d2f_dq2_ana.at(i) << std::endl;
+      }
+
+      // if (diff_dv > 1e-3)
+      // {
+      //   std::cout << "diff_dv = " << diff_dv << std::endl;
+      // }
+
+      // if (diff_da > 1e-3)
+      // {
+      //   std::cout << "diff_da = " << diff_da << std::endl;
+      // }
+    }
 
   }
 

include/pinocchio/algorithm/spatial-force-second-order-derivatives.hpp

@@ -0,0 +1,146 @@
+//
+// Copyright (c) 2017-2019 CNRS INRIA
+
+#ifndef __pinocchio_spatial_force_second_order_derivatives_hpp__
+#define __pinocchio_spatial_force_second_order_derivatives_hpp__
+
+#include "pinocchio/container/aligned-vector.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+namespace pinocchio {
+///
+/// \brief Computes the Second-Order partial derivatives of the Recursive Newton
+/// Euler Algorithm w.r.t the joint configuration, the joint velocity and the
+/// joint acceleration.
+///
+/// \tparam JointCollection Collection of Joint types.
+/// \tparam ConfigVectorType Type of the joint configuration vector.
+/// \tparam TangentVectorType1 Type of the joint velocity vector.
+/// \tparam TangentVectorType2 Type of the joint acceleration vector.
+/// \tparam Tensor1 Type of the 3D-Tensor containing the SO partial
+/// derivative with respect to the joint configuration vector. The elements of
+/// Torque vector are along the 1st dim, and joint config along 2nd,3rd
+/// dimensions.
+/// \tparam Tensor2 Type of the 3D-Tensor containing the
+/// Second-Order partial derivative with respect to the joint velocity vector.
+/// The elements of Torque vector are along the 1st dim, and the velocity
+/// along 2nd,3rd dimensions.
+/// \tparam Tensor3 Type of the 3D-Tensor
+/// containing the cross Second-Order partial derivative with respect to the
+/// joint configuration and velocty vector. The elements of Torque vector are
+/// along the 1st dim, and the config. vector along 2nd dimension, and velocity
+/// along the third dimension.
+///\tparam Tensor4 Type of the 3D-Tensor containing the cross Second-Order
+/// partial derivative with respect to the joint configuration and acceleration
+/// vector. This is also the First-order partial derivative of Mass-Matrix (M)
+/// with respect to configuration vector. The elements of Torque vector are
+/// along the 1st dim, and the acceleration vector along 2nd dimension, while
+/// configuration along the third dimension.
+///
+/// \param[in] model The model structure of the rigid body system.
+/// \param[in] data The data structure of the rigid body system.
+/// \param[in] q The joint configuration vector (dim model.nq).
+/// \param[in] v The joint velocity vector (dim model.nv).
+/// \param[in] a The joint acceleration vector (dim model.nv).
+/// \param[out] d2tau_dqdq Second-Order Partial derivative of the generalized
+/// torque vector with respect to the joint configuration.
+/// \param[out] d2tau_dvdv Second-Order Partial derivative of the generalized
+/// torque vector with respect to the joint velocity
+/// \param[out] dtau_dqdv Cross Second-Order Partial derivative of the
+/// generalized torque vector with respect to the joint configuration and
+/// velocity.
+/// \param[out] dtau_dadq Cross Second-Order Partial derivative of
+/// the generalized torque vector with respect to the joint configuration and
+/// accleration.
+/// \remarks d2tau_dqdq,
+/// d2tau_dvdv, dtau_dqdv and dtau_dadq must be first initialized with zeros
+/// (d2tau_dqdq.setZero(), etc). The storage order of the 3D-tensor derivatives is
+/// important. For d2tau_dqdq, the elements of generalized torque varies along
+/// the rows, while elements of q vary along the columns and pages of the
+/// tensor. For dtau_dqdv, the elements of generalized torque varies along the
+/// rows, while elements of v vary along the columns and elements of q along the
+/// pages of the tensor. Hence, dtau_dqdv is essentially d (d tau/dq)/dv, with
+/// outer-most derivative representing the third dimension (pages) of the
+/// tensor.  The tensor dtau_dadq reduces down to dM/dq, and hence the elements
+/// of q vary along the pages of the tensor. In other words, this tensor
+/// derivative is d(d tau/da)/dq. All other remaining combinations of
+/// second-order derivatives of generalized torque are zero.  \sa
+///
+template <typename Scalar, int Options,
+          template <typename, int> class JointCollectionTpl,
+          typename ConfigVectorType, typename TangentVectorType1,
+          typename TangentVectorType2, typename Tensor1,
+          typename Tensor2, typename Tensor3, typename Tensor4>
+inline void ComputeSpatialForceSecondOrderDerivatives(
+    const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
+    DataTpl<Scalar, Options, JointCollectionTpl> &data,
+    const Eigen::MatrixBase<ConfigVectorType> &q,
+    const Eigen::MatrixBase<TangentVectorType1> &v,
+    const Eigen::MatrixBase<TangentVectorType2> &a,
+    const std::vector<Tensor1> &d2fc_dqdq, const std::vector<Tensor2> &d2fc_dvdv,
+    const std::vector<Tensor3> &d2fc_dada, const std::vector<Tensor4> &d2fc_dadq);
+
+///
+/// \brief Computes the Second-Order partial derivatives of the Recursive Newton
+/// Euler Algorithms
+///        with respect to the joint configuration, the joint velocity and the
+///        joint acceleration.
+///
+/// \tparam JointCollection Collection of Joint types.
+/// \tparam ConfigVectorType Type of the joint configuration vector.
+/// \tparam TangentVectorType1 Type of the joint velocity vector.
+/// \tparam TangentVectorType2 Type of the joint acceleration vector.
+///
+/// \param[in] model The model structure of the rigid body system.
+/// \param[in] data The data structure of the rigid body system.
+/// \param[in] q The joint configuration vector (dim model.nq).
+/// \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_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
+/// with respect to configuration/velocity and configuration/acceleration
+/// respectively.
+///
+/// \remarks d2tau_dqdq,
+/// d2tau_dvdv2, d2tau_dqdv and d2tau_dadq must be first initialized with zeros
+/// (d2tau_dqdq.setZero(),etc). The storage order of the 3D-tensor derivatives is
+/// important. For d2tau_dqdq, the elements of generalized torque varies along
+/// the rows, while elements of q vary along the columns and pages of the
+/// tensor. For d2tau_dqdv, the elements of generalized torque varies along the
+/// rows, while elements of v vary along the columns and elements of q along the
+/// pages of the tensor. Hence, d2tau_dqdv is essentially d (d tau/dq)/dv, with
+/// outer-most derivative representing the third dimension (pages) of the
+/// tensor.  The tensor d2tau_dadq reduces down to dM/dq, and hence the elements
+/// of q vary along the pages of the tensor. In other words, this tensor
+/// derivative is d(d tau/da)/dq. All other remaining combinations of
+/// second-order derivatives of generalized torque are zero.  \sa
+
+// template <typename Scalar, int Options,
+//           template <typename, int> class JointCollectionTpl,
+//           typename ConfigVectorType, typename TangentVectorType1,
+//           typename TangentVectorType2>
+// inline void ComputeSpatialForceSecondOrderDerivatives(
+//     const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
+//     DataTpl<Scalar, Options, JointCollectionTpl> &data,
+//     const Eigen::MatrixBase<ConfigVectorType> &q,
+//     const Eigen::MatrixBase<TangentVectorType1> &v,
+//     const Eigen::MatrixBase<TangentVectorType2> &a) {
+//   (data.d2tau_dqdq).setZero();
+//   (data.d2tau_dvdv).setZero();
+//   (data.d2tau_dqdv).setZero();
+//   (data.d2tau_dadq).setZero();
+
+//   ComputeSpatialForceSecondOrderDerivatives(model, data, q.derived(), v.derived(), a.derived(),
+//                            data.d2tau_dqdq, data.d2tau_dvdv, data.d2tau_dqdv,
+//                            data.d2tau_dadq);
+// }
+
+} // namespace pinocchio
+
+#include "pinocchio/algorithm/spatial-force-second-order-derivatives.hxx"
+
+#endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__