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
bp::def("forwardDynamics",
(const VectorXd & (*)(const Model &, Data &,
const VectorXd &, const VectorXd &, const VectorXd &,
const MatrixXd &, const VectorXd &, const bool))
const MatrixXd &, const VectorXd &, const double, const bool))
&forwardDynamics,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
......@@ -38,7 +38,8 @@ namespace se3
"Joint torque tau (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)",
"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",
bp::return_value_policy<bp::return_by_value>());
......
......@@ -56,6 +56,7 @@ namespace se3
const Eigen::VectorXd & tau,
const Eigen::MatrixXd & J,
const Eigen::VectorXd & gamma,
const double inv_damping = 0.,
const bool updateKinematics = true
)
{
......@@ -85,6 +86,8 @@ namespace se3
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.diagonal().array() += inv_damping;
data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers
......
......@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
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>();
MatrixXd Minv (data.M.inverse());
......@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
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);
......
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