Commit 4118189f authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

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

parent 86593939
......@@ -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 )
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment