[Python] Expose forward dynamics with contact

src/python/algorithms.hpp

@@ -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)",

src/python/data.hpp

@@ -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()
       {