Unverified Commit ac2cba4a authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #501 from proyan/devel

Documentation and unittest for forward dynamics with damped cholesky inverse
parents cc8b8d7b c5942ca9
...@@ -37,6 +37,7 @@ namespace se3 ...@@ -37,6 +37,7 @@ namespace se3
/// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$ </CENTER> <BR> /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$ </CENTER> <BR>
/// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), /// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints),
/// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift. /// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift.
/// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
...@@ -45,7 +46,9 @@ namespace se3 ...@@ -45,7 +46,9 @@ namespace se3
/// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] tau The joint torque vector (dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. /// \param[in] inv_damping Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\
/// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.
/// ///
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector. /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.
/// ///
......
...@@ -91,6 +91,62 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ...@@ -91,6 +91,62 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c); Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
BOOST_CHECK(dynamics_residual.norm() <= 1e-12); BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
q.segment <4> (3).normalize();
se3::computeJointJacobians(model, data, q);
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
const std::string RF = "rleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero();
getJointJacobian<LOCAL> (model, data, model.getJointId(RF), J_RF);
Eigen::MatrixXd J (12, model.nv);
J.setZero();
J.topRows<6> () = J_RF;
J.bottomRows<6> () = J_RF;
Eigen::VectorXd gamma (VectorXd::Ones(12));
// Forward Dynamics with damping
se3::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12,true);
// Matrix Definitions
Eigen::MatrixXd H(J.transpose());
data.M.triangularView<Eigen::StrictlyLower>() =
data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse());
MatrixXd JMinvJt (J * Minv * J.transpose());
// Check that JMinvJt is correctly formed
Eigen::MatrixXd G_ref(J.transpose());
cholesky::Uiv(model, data, G_ref);
for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
BOOST_CHECK(H_ref.isApprox(JMinvJt,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_AUTO_TEST_CASE ( test_ID ) BOOST_AUTO_TEST_CASE ( test_ID )
......
Supports Markdown
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