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

Merge pull request #500 from proyan/devel

[algorithm/dynamics] add option to take damped inverse when doing cholesky factorization of JMinvJt
parents 3c827a52 fb57bb2d
...@@ -30,7 +30,7 @@ namespace se3 ...@@ -30,7 +30,7 @@ namespace se3
bp::def("forwardDynamics", bp::def("forwardDynamics",
(const VectorXd & (*)(const Model &, Data &, (const VectorXd & (*)(const Model &, Data &,
const VectorXd &, const VectorXd &, const VectorXd &, const VectorXd &, const VectorXd &, const VectorXd &,
const MatrixXd &, const VectorXd &, const bool)) const MatrixXd &, const VectorXd &, const double, const bool))
&forwardDynamics, &forwardDynamics,
bp::args("Model","Data", bp::args("Model","Data",
"Joint configuration q (size Model::nq)", "Joint configuration q (size Model::nq)",
...@@ -38,7 +38,8 @@ namespace se3 ...@@ -38,7 +38,8 @@ namespace se3
"Joint torque tau (size Model::nv)", "Joint torque tau (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)", "Contact Jacobian J (size nb_constraint * Model::nv)",
"Contact drift gamma (size nb_constraint)", "Contact drift gamma (size nb_constraint)",
"Update kinematics (if true, it updates the dynamic variable according to the current state)"), "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.",
"Update kinematics (if true, it updates the dynamic variable according to the current state )"),
"Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c", "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c",
bp::return_value_policy<bp::return_by_value>()); bp::return_value_policy<bp::return_by_value>());
......
...@@ -56,6 +56,7 @@ namespace se3 ...@@ -56,6 +56,7 @@ namespace se3
const Eigen::VectorXd & tau, const Eigen::VectorXd & tau,
const Eigen::MatrixXd & J, const Eigen::MatrixXd & J,
const Eigen::VectorXd & gamma, const Eigen::VectorXd & gamma,
const double inv_damping = 0.,
const bool updateKinematics = true const bool updateKinematics = true
) )
{ {
...@@ -85,6 +86,8 @@ namespace se3 ...@@ -85,6 +86,8 @@ namespace se3
for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]); for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
data.JMinvJt.diagonal().array() += inv_damping;
data.llt_JMinvJt.compute(data.JMinvJt); data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers // Compute the Lagrange Multipliers
......
...@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ...@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Eigen::MatrixXd H(J.transpose()); Eigen::MatrixXd H(J.transpose());
se3::forwardDynamics(model, data, q, v, tau, J, gamma, true); se3::forwardDynamics(model, data, q, v, tau, J, gamma, 0.,true);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse()); MatrixXd Minv (data.M.inverse());
...@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt) ...@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
{ {
se3::forwardDynamics(model, data, q, v, tau, J, gamma, true); se3::forwardDynamics(model, data, q, v, tau, J, gamma, 0., true);
} }
timer.toc(std::cout,NBT); timer.toc(std::cout,NBT);
......
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