[unittest/dynamics] remove svd decomposition. only check the residual on constraint and dynamics

unittest/dynamics.cpp

@@ -23,8 +23,6 @@
 #include "pinocchio/parsers/sample-models.hpp"
 #include "pinocchio/utils/timer.hpp"
 
-#include <Eigen/SVD>
-
 #include <iostream>
 
 #include <boost/test/unit_test.hpp>
@@ -93,6 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
   
   Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
   BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
+  
 }
 
 BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
@@ -143,38 +142,11 @@ BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
   Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
   BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
 
-  //Find the ground truth
-  typedef Eigen::JacobiSVD<Eigen::MatrixXd> SVDType;
-  const double threshold = 1e-8;
-  SVDType svd(JMinvJt, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  SVDType::SingularValuesType m_singularValues=svd.singularValues();
-  SVDType::SingularValuesType singularValues_inv;
-  singularValues_inv.resizeLike(m_singularValues);
-  for (unsigned int i=0; i<m_singularValues.size(); ++i)
-  {
-    if ( m_singularValues(i) > threshold )
-      singularValues_inv(i)=1.0/m_singularValues(i);
-    else singularValues_inv(i)=0;
-  }
-  MatrixXd JMinvJt_inv (JMinvJt.rows(), JMinvJt.cols());  
-  JMinvJt_inv = svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose();
-
-  // Reference acceleration and lambda
-  VectorXd lambda_ref = -JMinvJt_inv * (J*Minv*(tau - data.nle) + gamma);
-  VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);
-  Eigen::VectorXd
-    dynamics_residual_ref(data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
-  BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11);
-
-  // Actual lambda and acceleration
-  BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
-  BOOST_CHECK(data.ddq.isApprox(a_ref, 1e-12));
-  
   // Actual Residuals
   Eigen::VectorXd constraint_residual (J * data.ddq + gamma);  
   Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
-  BOOST_CHECK(constraint_residual.norm() <= 1e-10);  
-  BOOST_CHECK(dynamics_residual.norm() <= 1e-12); 
+  BOOST_CHECK(constraint_residual.norm() <= 1e-10);
+  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
 }
 
 BOOST_AUTO_TEST_CASE ( test_ID )