added fc SO derivs using fd WIP

shubhamsingh91 · Jan 11, 2025 · bddb366 · bddb366
1 parent ca9d465
commit bddb366
Show file tree

Hide file tree

Showing 3 changed files with 97 additions and 10 deletions.
diff --git a/benchmark/spatial_force b/benchmark/spatial_force
diff --git a/benchmark/test_spatial_force.cpp b/benchmark/test_spatial_force.cpp
@@ -31,7 +31,7 @@
 *  [[dfcn_dq1] [dfcn_dq2]  [dfcn_dqn]] - page n of tensor
 */
 
-#ifdef NFO
+#ifdef FO_DERIVS
 void calc_df(const pinocchio::Model & model, pinocchio::Data & data_fd,
              const Eigen::VectorXd & q,
              const Eigen::VectorXd & v,
@@ -115,6 +115,8 @@ int main(int argc, const char ** argv)
   using namespace Eigen;
   using namespace pinocchio;
 
+  typedef Eigen::Tensor<double, 3> ten3d;
+
   PinocchioTicToc timer(PinocchioTicToc::US);
   #ifdef NDEBUG
   const int NBT = 1;
@@ -126,8 +128,8 @@ int main(int argc, const char ** argv)
   Model model;
 
   //std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
-  // std::string filename =  std::string("../models/double_pendulum.urdf");
-  std::string filename =  std::string("../models/simple_humanoid.urdf");
+  std::string filename =  std::string("../models/double_pendulum.urdf");
+  // std::string filename =  std::string("../models/simple_humanoid.urdf");
 
   if(argc>1) filename = argv[1];
   bool with_ff = false;
@@ -166,19 +168,20 @@ int main(int argc, const char ** argv)
     qddots[i] = Eigen::VectorXd::Random(model.nv);
     taus[i] = Eigen::VectorXd::Random(model.nv);
   }
-
-  PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
-  PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
-  MatrixXd drnea_da(MatrixXd::Zero(model.nv,model.nv));
+
+  std::cout << "model.njoints = " << model.njoints << std::endl;
 
   SMOOTH(NBT)
   {
-    #ifdef NFO
+  #ifdef FO_DERIVS
+
+  PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
+  PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
+  MatrixXd drnea_da(MatrixXd::Zero(model.nv,model.nv));
 
     computeRNEADerivatives(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
                            drnea_dq,drnea_dv,drnea_da);
 
-    std::cout << "model.njoints = " << model.njoints << std::endl;
 
     auto f_vec = data.of;
 
@@ -307,7 +310,65 @@ int main(int argc, const char ** argv)
         d2f_da2_fd.emplace_back(6, model.nv, model.nv);  
     }
 
+    double alpha = 1e-6; // performs well
+
+    VectorXd v_eps(VectorXd::Zero(model.nv));
+    VectorXd a_eps(VectorXd::Zero(model.nv));
+    VectorXd q_plus(model.nq);
+    VectorXd qd_plus(model.nv);
+    VectorXd a_plus(model.nv);
+
+    ten3d df_dq_ana (6,model.nv, model.nv);
+    ten3d df_dv_ana (6,model.nv, model.nv);
+    ten3d df_da_ana (6,model.nv, model.nv);
+
+    computeSpatialForceDerivs(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
+      df_dq_ana, df_dv_ana, df_da_ana);
+
+    ten3d df_dq_ana_tensor_plus (6,model.nv, model.nv);
+    ten3d df_dv_ana_tensor_plus (6,model.nv, model.nv);
+    ten3d df_da_ana_tensor_plus (6,model.nv, model.nv);
+
+    ten3d t_d2fc_dq_dqk , t_d2fc_dv_dvk;
+    Eigen::MatrixXd m_d2fci_dq_dqk(6,model.nv);
+    Eigen::MatrixXd m_d2fci_dv_dvk(6,model.nv);
+
+    // Partial wrt q
+    for (int k = 0; k < model.nv; ++k) {
+        v_eps[k] += alpha;
+        q_plus = integrate(model, qs[_smooth], v_eps); // This is used to add the v_eps to q in the k^th direction
+
+        computeSpatialForceDerivs(model,data,q_plus,qdots[_smooth],qddots[_smooth],
+        df_dq_ana_tensor_plus, df_dv_ana_tensor_plus, df_da_ana_tensor_plus);
+
+        t_d2fc_dq_dqk = (df_dq_ana_tensor_plus - df_dq_ana) / alpha; // 3d tensor d2fc/dq dqk
+
+        for (int i = 0; i < model.nv; i++)
+        {
+          get_mat_from_tens3_v1_gen(t_d2fc_dq_dqk, m_d2fci_dq_dqk, 6, model.nv, i);
+          hess_assign_fd_v1_gen(d2f_dq2_fd.at(i), m_d2fci_dq_dqk, 6, model.nv, k); // slicing in the matrix along the kth page for ith tensor             
+        }
+
+        v_eps[k] -= alpha;
+    }
+
+    // Partial wrt v
+    for (int k = 0; k < model.nv; ++k) {
+        v_eps[k] += alpha;
+        qd_plus = qdots[_smooth] + v_eps; // This is used to add the v_eps to q in the k^th direction
+
+        computeSpatialForceDerivs(model,data,qs[_smooth], qd_plus ,qddots[_smooth],
+        df_dq_ana_tensor_plus, df_dv_ana_tensor_plus, df_da_ana_tensor_plus);
+
+        t_d2fc_dv_dvk = (df_dv_ana_tensor_plus - df_dv_ana) / alpha; // 3d tensor d2fc/dq dqk
+        for (int i = 0; i < model.nv; i++)
+        {
+          get_mat_from_tens3_v1_gen(t_d2fc_dv_dvk, m_d2fci_dv_dvk, 6, model.nv, i);
+          hess_assign_fd_v1_gen(d2f_dv2_fd.at(i), m_d2fci_dv_dvk, 6, model.nv, k); // slicing in the matrix along the kth page for ith tensor     
+        }
 
+        v_eps[k] -= alpha;
+    }
 
 
 

diff --git a/include/pinocchio/utils/tensor_utils.hpp b/include/pinocchio/utils/tensor_utils.hpp
@@ -129,7 +129,20 @@ void hess_assign_fd_v1(Eigen::Tensor<double, 3>& hess, const Eigen::MatrixBase<T
         }
     }
 }
-
+// CHEAP
+// chaning to col major- massive saving from original
+// Inserting a matrix in (along 1st and 2nd dim)
+// Assigning code here- along 1st and second dim, 3rd dim constant (input)
+// Templated
+template <typename T>
+void hess_assign_fd_v1_gen(Eigen::Tensor<double, 3>& hess, const Eigen::MatrixBase<T>& mat, int r, int c, int k)
+{
+    for (int ii = 0; ii < c; ii++) {
+        for (int jj = 0; jj < r; jj++) {
+            hess(jj, ii, k) = mat(jj, ii);
+        }
+    }
+}
 
 // CHEAP
 // chaning to col major- massive saving from original
@@ -346,6 +359,19 @@ void get_mat_from_tens3_v1(const Eigen::Tensor<double, 3>& tens, Eigen::MatrixBa
     }
 }
 
+// This is 30 x faster than row-major one
+// changing the tens access and matrix access to col-major
+// get matrix from a tensor along dimension-3 (or keeping third dim constant)
+template <typename T>
+void get_mat_from_tens3_v1_gen(const Eigen::Tensor<double, 3>& tens, Eigen::MatrixBase<T>& mat, int r, int c, int k)
+{
+    for (int ii = 0; ii < c; ii++) {
+        for (int jj = 0; jj < r; jj++) {
+            mat(jj, ii) = tens(jj, ii, k);
+        }
+    }
+}
+
 // This is 30 x faster than row-major one
 // changing the tens access and matrix access to col-major
 // get matrix from a tensor along dimension-3 (or keeping third dim constant)