Commit 3c047ae3 authored by jcarpent's avatar jcarpent
Browse files

[Python] Expose forward dynamics with contact

parent 86cc0b49
......@@ -28,6 +28,7 @@
#include "pinocchio/algorithm/non-linear-effects.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/dynamics.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/operational-frames.hpp"
......@@ -94,6 +95,19 @@ namespace se3
aba(*model,*data,q,v,tau);
return data->ddq;
}
static Eigen::MatrixXd fd_llt_proxy(const ModelHandler & model,
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & v,
const VectorXd_fx & tau,
const eigenpy::MatrixXd_fx & J,
const VectorXd_fx & gamma,
const bool update_kinematics = true)
{
forwardDynamics(*model,*data,q,v,tau,J,gamma,update_kinematics);
return data->ddq;
}
static SE3::Vector3
com_0_proxy(const ModelHandler& model,
......@@ -327,6 +341,16 @@ namespace se3
"Joint torque tau (size Model::nv)"),
"Compute ABA, put the result in Data::ddq and return it.");
bp::def("forwardDynamics",fd_llt_proxy,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"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)"),
"Solve the forward dynamics problem with contacts, put the result in Data::ddq and return it.");
bp::def("centerOfMass",com_0_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
......
......@@ -112,6 +112,8 @@ namespace se3
.ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
;
}
......@@ -159,6 +161,8 @@ namespace se3
IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
/* --- Expose --------------------------------------------------------- */
static void expose()
{
......
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