diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4349318021..bcc5284fa4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,6 +42,7 @@ OPTION(ENABLE_VECTORIZATION "Enable vectorization and further processor-related
 OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 OPTION(BUILD_BENCHMARK "Build the benchmark" ON)
 OPTION(BUILD_EXAMPLES "Build the examples" ON)
+OPTION(BUILD_UNIT_TESTS "Build the unitary tests" OFF)
 
 IF(DEFINED BUILD_UNIT_TESTS)
   MESSAGE(AUTHOR_WARNING "BUILD_UNIT_TESTS is deprecated. Use BUILD_TESTING instead.\
diff --git a/Testing/Temporary/CTestCostData.txt b/Testing/Temporary/CTestCostData.txt
new file mode 100644
index 0000000000..ed97d539c0
--- /dev/null
+++ b/Testing/Temporary/CTestCostData.txt
@@ -0,0 +1 @@
+---
diff --git a/bindings/python/crocoddyl/__init__.py b/bindings/python/crocoddyl/__init__.py
index 6f552a8a41..b43783ddb6 100644
--- a/bindings/python/crocoddyl/__init__.py
+++ b/bindings/python/crocoddyl/__init__.py
@@ -180,7 +180,8 @@ def getForceTrajectoryFromSolver(self, solver):
                             joint = model.differential.state.pinocchio.frames[contact.frame].parent
                             oMf = contact.pinocchio.oMi[joint] * contact.jMf
                             fiMo = pinocchio.SE3(contact.pinocchio.oMi[joint].rotation.T, contact.jMf.translation)
-                            force = fiMo.actInv(contact.f)
+                            # force = fiMo.actInv(contact.f)
+                            force = contact.jMf.actInv(contact.f)
                             R = np.eye(3)
                             mu = 0.7
                             for k, c in model.differential.costs.costs.todict().items():
diff --git a/bindings/python/crocoddyl/core/core.cpp b/bindings/python/crocoddyl/core/core.cpp
index 4f2dff9164..c386d34e66 100644
--- a/bindings/python/crocoddyl/core/core.cpp
+++ b/bindings/python/crocoddyl/core/core.cpp
@@ -28,6 +28,7 @@ void exposeCore() {
   exposeIntegratedActionEuler();
   exposeIntegratedActionRK();
   exposeIntegratedActionRK4();
+  exposeIntegratedActionLPF();
   exposeCostAbstract();
   exposeResidualControl();
   exposeCostSum();
diff --git a/bindings/python/crocoddyl/core/core.hpp b/bindings/python/crocoddyl/core/core.hpp
index 6e2a29c9bd..3c11927dd6 100644
--- a/bindings/python/crocoddyl/core/core.hpp
+++ b/bindings/python/crocoddyl/core/core.hpp
@@ -30,6 +30,7 @@ void exposeDataCollectorActuation();
 void exposeIntegratedActionEuler();
 void exposeIntegratedActionRK();
 void exposeIntegratedActionRK4();
+void exposeIntegratedActionLPF();
 void exposeCostAbstract();
 void exposeResidualControl();
 void exposeCostSum();
diff --git a/bindings/python/crocoddyl/core/integrator/lpf.cpp b/bindings/python/crocoddyl/core/integrator/lpf.cpp
new file mode 100644
index 0000000000..82b7a24315
--- /dev/null
+++ b/bindings/python/crocoddyl/core/integrator/lpf.cpp
@@ -0,0 +1,95 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include <pinocchio/fwd.hpp> // to avoid compilation error (https://github.com/loco-3d/crocoddyl/issues/205)
+
+#include "python/crocoddyl/core/core.hpp"
+#include "python/crocoddyl/core/action-base.hpp"
+#include "crocoddyl/core/integrator/lpf.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+void exposeIntegratedActionLPF() {
+  bp::class_<IntegratedActionModelLPF, bp::bases<ActionModelAbstract> >(
+      "IntegratedActionModelLPF",
+      "Sympletic Euler integrator for differential action models.\n\n"
+      "This class implements a sympletic Euler integrator (a.k.a semi-implicit\n"
+      "integrator) give a differential action model, i.e.:\n"
+      "  [q+, v+, tau+] = StateLPF.integrate([q, v], [v + a * dt, a * dt] * dt, [alpha*tau + (1-alpha)*w]).",
+      bp::init<boost::shared_ptr<DifferentialActionModelAbstract>, bp::optional<double, bool, double, double, Eigen::VectorXd, bool, double, bool, int, bool> >(
+          bp::args("self", "diffModel", "stepTime", "withCostResidual", 
+          "fc", "cost_weight_w_reg", "cost_ref_w_reg", "w_gravity_reg", "cost_weight_w_lim", "tau_plus_integration", "filter", "is_terminal"),
+          "Initialize the sympletic Euler integrator.\n\n"
+          ":param diffModel: differential action model\n"
+          ":param stepTime: step time\n"
+          ":param withCostResidual: includes the cost residuals and derivatives\n"
+          ":param fc: LPF parameter depending on cut-off frequency alpha=1/(1+2*Pi*dt*fc)\n"
+          ":param cost_weight_w_reg: cost weight on unfiltered torque regularization\n"
+          ":param cost_ref_w_reg: cost reference on unfiltered torque regularization\n"
+          ":param w_gravity_reg: use gravity regularization on unfiltered torque (True) or user-provided constant reference (False) "
+          ":param cost_weight_w_lim: cost weight on unfiltered torque limit\n"
+          ":param tau_plus_integration: use tau+=LPF(tau,w) in acceleration computation, or tau\n"
+          ":param filter: type of low-pass filter (0 = Expo Moving Avg, 1 = Classical, 2 = Exact)\n"
+          ":param is_terminal: terminal model or not."))
+      .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&)>(
+          "calc", &IntegratedActionModelLPF::calc, bp::args("self", "data", "x", "u"),
+          "Compute the time-discrete evolution of a differential action model.\n\n"
+          "It describes the time-discrete evolution of action model.\n"
+          ":param data: action data\n"
+          ":param x: state vector\n"
+          ":param u: control input")
+      .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&)>(
+          "calc", &ActionModelAbstract::calc, bp::args("self", "data", "x"))
+      .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&)>(
+          "calcDiff", &IntegratedActionModelLPF::calcDiff, bp::args("self", "data", "x", "u"),
+          "Computes the derivatives of the integrated action model wrt state and control. \n\n"
+          "This function builds a quadratic approximation of the\n"
+          "action model (i.e. dynamical system and cost function).\n"
+          "It assumes that calc has been run first.\n"
+          ":param data: action data\n"
+          ":param x: state vector\n"
+          ":param u: control input\n")
+      .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&,
+                                              const Eigen::Ref<const Eigen::VectorXd>&)>(
+          "calcDiff", &ActionModelAbstract::calcDiff, bp::args("self", "data", "x"))
+      .def("createData", &IntegratedActionModelLPF::createData, bp::args("self"),
+           "Create the Euler integrator data.")
+      .add_property("differential",
+                    bp::make_function(&IntegratedActionModelLPF::get_differential,
+                                      bp::return_value_policy<bp::return_by_value>()),
+                    &IntegratedActionModelLPF::set_differential, "differential action model")
+      .add_property(
+          "dt", bp::make_function(&IntegratedActionModelLPF::get_dt, bp::return_value_policy<bp::return_by_value>()),
+          &IntegratedActionModelLPF::set_dt, "step time")
+      .add_property(
+          "fc", bp::make_function(&IntegratedActionModelLPF::get_fc, bp::return_value_policy<bp::return_by_value>()),
+          &IntegratedActionModelLPF::set_fc, "cut-off frequency of low-pass filter");
+
+  bp::register_ptr_to_python<boost::shared_ptr<IntegratedActionDataLPF> >();
+
+  bp::class_<IntegratedActionDataLPF, bp::bases<ActionDataAbstract> >(
+      "IntegratedActionDataLPF", "Sympletic Euler integrator data.",
+      bp::init<IntegratedActionModelLPF*>(bp::args("self", "model"),
+                                            "Create sympletic Euler integrator data.\n\n"
+                                            ":param model: sympletic Euler integrator model"))
+      .add_property(
+          "differential",
+          bp::make_getter(&IntegratedActionDataLPF::differential, bp::return_value_policy<bp::return_by_value>()),
+          "differential action data")
+      .add_property("dy", bp::make_getter(&IntegratedActionDataLPF::dy, bp::return_internal_reference<>()),
+                    "state rate.");
+}
+
+}  // namespace python
+}  // namespace crocoddyl
diff --git a/bindings/python/crocoddyl/multibody/multibody.cpp b/bindings/python/crocoddyl/multibody/multibody.cpp
index 149a8be10e..193174a195 100644
--- a/bindings/python/crocoddyl/multibody/multibody.cpp
+++ b/bindings/python/crocoddyl/multibody/multibody.cpp
@@ -17,6 +17,7 @@ void exposeMultibody() {
   exposeWrenchCone();
   exposeCoPSupport();
   exposeStateMultibody();
+  exposeStateLPF();
   exposeActuationFloatingBase();
   exposeActuationFull();
   exposeActuationModelMultiCopterBase();
diff --git a/bindings/python/crocoddyl/multibody/multibody.hpp b/bindings/python/crocoddyl/multibody/multibody.hpp
index 1477bbe8d5..c5f154bbc4 100644
--- a/bindings/python/crocoddyl/multibody/multibody.hpp
+++ b/bindings/python/crocoddyl/multibody/multibody.hpp
@@ -20,6 +20,7 @@ void exposeFrictionCone();
 void exposeWrenchCone();
 void exposeCoPSupport();
 void exposeStateMultibody();
+void exposeStateLPF();
 void exposeActuationFloatingBase();
 void exposeActuationFull();
 void exposeActuationModelMultiCopterBase();
diff --git a/bindings/python/crocoddyl/multibody/states/statelpf.cpp b/bindings/python/crocoddyl/multibody/states/statelpf.cpp
new file mode 100644
index 0000000000..7f3b7e3a12
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/states/statelpf.cpp
@@ -0,0 +1,86 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "python/crocoddyl/multibody/multibody.hpp"
+#include "python/crocoddyl/core/state-base.hpp"
+#include "crocoddyl/multibody/states/statelpf.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+void exposeStateLPF() {
+  bp::class_<StateLPF, bp::bases<StateAbstract> >(
+      "StateLPF",
+      "LPF state defined using Pinocchio.\n\n"
+      "Pinocchio defines operators for integrating or differentiating the robot's\n"
+      "configuration space. And here we assume that the state is defined by the\n"
+      "robot's configuration, joint velocities and torques (y=[q,v,u]). Generally speaking,\n"
+      "q lies on the manifold configuration manifold (M) and v in its tangent space\n"
+      "(Ty M). Additionally the Pinocchio allows us to compute analytically the\n"
+      "Jacobians for the differentiate and integrate operators. Note that this code\n"
+      "can be reused in any robot that is described through its Pinocchio model.",
+      bp::init<boost::shared_ptr<pinocchio::Model>, bp::optional<std::size_t> >(
+          bp::args("self", "pinocchioModel", "nu"),
+          "Initialize the multibody state given a Pinocchio model.\n\n"
+          ":param pinocchioModel: pinocchio model (i.e. multibody model)\n"
+          ":param nu: size of control vector.")[bp::with_custodian_and_ward<1, 2>()])
+      .def("zero", &StateLPF::zero, bp::args("self"),
+           "Return the neutral robot configuration with zero velocity.\n\n"
+           ":return neutral robot configuration with zero velocity")
+      .def("rand", &StateLPF::rand, bp::args("self"),
+           "Return a random reference state.\n\n"
+           ":return random reference state")
+      .def("diff", &StateLPF::diff_dx, bp::args("self", "y0", "y1"),
+           "Operator that differentiates the two robot states.\n\n"
+           "It returns the value of y1 [-] y0 operation. This operator uses the Lie\n"
+           "algebra since the robot's root could lie in the SE(3) manifold.\n"
+           ":param y0: current state (dim state.ny()).\n"
+           ":param y1: next state (dim state.ny()).\n"
+           ":return y1 - y0 value (dim state.ny()).")
+      .def("integrate", &StateLPF::integrate_x, bp::args("self", "y", "dy"),
+           "Operator that integrates the current robot state.\n\n"
+           "It returns the value of y [+] dy operation. This operator uses the Lie\n"
+           "algebra since the robot's root could lie in the SE(3) manifold.\n"
+           "Futhermore there is no timestep here (i.e. dy = v*dt), note this if you're\n"
+           "integrating a velocity v during an interval dt.\n"
+           ":param y: current state (dim state.ny()).\n"
+           ":param dy: displacement of the state (dim state.ndy()).\n"
+           ":return y + dy value (dim state.ny()).")
+      .def("Jdiff", &StateLPF::Jdiff_Js,
+           Jdiffs(bp::args("self", "y0", "y1", "firstsecond"),
+                  "Compute the partial derivatives of the diff operator.\n\n"
+                  "Both Jacobian matrices are represented throught an identity matrix, with the exception\n"
+                  "that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
+                  "function returns the derivatives of the first and second argument (i.e.\n"
+                  "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+                  "firstsecond='first' or firstsecond='second'.\n"
+                  ":param y0: current state (dim state.ny()).\n"
+                  ":param y1: next state (dim state.ny()).\n"
+                  ":param firstsecond: desired partial derivative\n"
+                  ":return the partial derivative(s) of the diff(y0, y1) function"))
+      .def("Jintegrate", &StateLPF::Jintegrate_Js,
+           Jintegrates(bp::args("self", "y", "dy", "firstsecond"),
+                       "Compute the partial derivatives of arithmetic addition.\n\n"
+                       "Both Jacobian matrices are represented throught an identity matrix. with the exception\n"
+                       "that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
+                       "function returns the derivatives of the first and second argument (i.e.\n"
+                       "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+                       "firstsecond='first' or firstsecond='second'.\n"
+                       ":param x: current state (dim state.ny()).\n"
+                       ":param dy: displacement of the state (dim state.ndy()).\n"
+                       ":param firstsecond: desired partial derivative\n"
+                       ":return the partial derivative(s) of the integrate(x, dy) function"))
+      .add_property("pinocchio",
+                    bp::make_function(&StateLPF::get_pinocchio, bp::return_value_policy<bp::return_by_value>()),
+                    "pinocchio model");
+     //  .add_property("ny", bp::make_getter(&StateLPF::ny_, bp::return_internal_reference<>()));
+     //  .add_property("nw", bp::make_getter(&StateLPF::ny_, bp::return_internal_reference<>()));
+}
+
+}  // namespace python
+}  // namespace crocoddyl
diff --git a/include/crocoddyl/core/fwd.hpp b/include/crocoddyl/core/fwd.hpp
index cee2702ae1..21c44429a5 100644
--- a/include/crocoddyl/core/fwd.hpp
+++ b/include/crocoddyl/core/fwd.hpp
@@ -76,6 +76,10 @@ template <typename Scalar>
 class ResidualModelAbstractTpl;
 template <typename Scalar>
 struct ResidualDataAbstractTpl;
+template <typename Scalar>
+class IntegratedActionModelLPFTpl;
+template <typename Scalar>
+struct IntegratedActionDataLPFTpl;
 
 // activation
 template <typename Scalar>
@@ -276,6 +280,10 @@ typedef ResidualModelAbstractTpl<double> ResidualModelAbstract;
 typedef ResidualDataAbstractTpl<double> ResidualDataAbstract;
 typedef ResidualModelControlTpl<double> ResidualModelControl;
 typedef ResidualDataControlTpl<double> ResidualDataControl;
+typedef IntegratedActionModelRK4Tpl<double> IntegratedActionModelRK4;
+typedef IntegratedActionDataRK4Tpl<double> IntegratedActionDataRK4;
+typedef IntegratedActionModelLPFTpl<double> IntegratedActionModelLPF;
+typedef IntegratedActionDataLPFTpl<double> IntegratedActionDataLPF;
 
 typedef ActivationDataQuadraticBarrierTpl<double> ActivationDataQuadraticBarrier;
 typedef ActivationModelQuadraticBarrierTpl<double> ActivationModelQuadraticBarrier;
diff --git a/include/crocoddyl/core/integrator/lpf.hpp b/include/crocoddyl/core/integrator/lpf.hpp
new file mode 100644
index 0000000000..8d63d78caa
--- /dev/null
+++ b/include/crocoddyl/core/integrator/lpf.hpp
@@ -0,0 +1,177 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_INTEGRATOR_LPF_HPP_
+#define CROCODDYL_CORE_INTEGRATOR_LPF_HPP_
+
+#include "crocoddyl/core/fwd.hpp"
+
+#include "crocoddyl/core/action-base.hpp"           // IAMAbs IADabs
+#include "crocoddyl/core/diff-action-base.hpp"      // DAMabs, DADabs
+
+#include "crocoddyl/multibody/states/statelpf.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp" // for static cast into state multibody
+#include "crocoddyl/multibody/data/multibody.hpp"   // for pinocchio data?
+
+#include "crocoddyl/multibody/fwd.hpp"              // DAMffwd and DADffwd
+
+#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
+
+#include "crocoddyl/core/activations/quadratic-barrier.hpp"
+
+#include <pinocchio/multibody/model.hpp>
+
+#include <pinocchio/algorithm/rnea-derivatives.hpp>
+#include <pinocchio/algorithm/rnea.hpp>
+
+namespace crocoddyl {
+
+template <typename _Scalar>
+class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef _Scalar Scalar;
+  typedef MathBaseTpl<Scalar> MathBase;
+  typedef ActionModelAbstractTpl<Scalar> Base;
+  typedef IntegratedActionDataLPFTpl<Scalar> Data;
+  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
+  typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
+  typedef typename MathBase::VectorXs VectorXs;
+  typedef typename MathBase::MatrixXs MatrixXs;
+  typedef StateLPFTpl<Scalar> StateLPF;
+  typedef StateMultibodyTpl<Scalar> StateMultibody;
+  typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
+  typedef ActivationModelQuadraticBarrierTpl<Scalar> ActivationModelQuadraticBarrier;
+  typedef ActivationBoundsTpl<Scalar> ActivationBounds;
+
+  IntegratedActionModelLPFTpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
+                              const Scalar& time_step = Scalar(1e-3), 
+                              const bool& with_cost_residual = true, 
+                              const Scalar& fc = 0,
+                              const Scalar& cost_weight_w_reg = 2e-2,
+                              const VectorXs& cost_ref_w_reg = VectorXs::Zero(7),
+                              const bool& w_gravity_reg = true,
+                              const Scalar& cost_weight_w_lim = 1.,
+                              const bool& tau_plus_integration = true,
+                              const int& filter = 0,
+                              const bool& is_terminal = false);
+  virtual ~IntegratedActionModelLPFTpl();
+
+  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& y,
+                    const Eigen::Ref<const VectorXs>& w);
+  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& y,
+                        const Eigen::Ref<const VectorXs>& w);
+  // virtual void setAlpha(const Scalar& f_c);
+  virtual boost::shared_ptr<ActionDataAbstract> createData();
+  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
+
+  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
+                           const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
+                           const Scalar& tol = Scalar(1e-9));
+
+  const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential() const;
+  const Scalar& get_dt() const;
+  const Scalar& get_fc() const;
+
+  void set_dt(const Scalar& dt);
+  void set_fc(const Scalar& fc);
+  void set_differential(boost::shared_ptr<DifferentialActionModelAbstract> model);
+  
+  void compute_alpha(const Scalar& fc);
+
+ protected:
+  using Base::has_control_limits_;    //!< Indicates whether any of the control limits are active
+  using Base::nr_;                    //!< Dimension of the cost residual
+  using Base::nu_;                    //!< Control dimension
+  using Base::u_lb_;                  //!< Lower control limits
+  using Base::u_ub_;                  //!< Upper control limits
+  using Base::unone_;                 //!< Neutral state
+  std::size_t nw_;                    //!< Unfiltered torque dimension
+  std::size_t ny_;                    //!< Augmented state dimension
+  using Base::state_;                 //!< Model of the state
+
+ public:
+  boost::shared_ptr<ActivationModelQuadraticBarrier> activation_model_w_lim_; //!< for lim cost
+  ActivationBounds activation_bounds_w_lim_;                                  //!< for lim cost
+
+ private:
+  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
+  Scalar time_step_;
+  Scalar time_step2_;
+  Scalar fc_;
+  Scalar alpha_;
+  bool with_cost_residual_;
+  bool enable_integration_;
+  Scalar cost_weight_w_reg_;                     //!< Cost weight for unfiltered torque regularization
+  VectorXs cost_ref_w_reg_;                      //!< Cost reference for unfiltered torque regularization
+  bool w_gravity_reg_;                           //!< Use gravity torque for unfiltered torque reg, or user-provided reference?
+  Scalar cost_weight_w_lim_;                     //!< Cost weight for unfiltered torque limits
+  bool tau_plus_integration_;                    //!< Use tau+ = LPF(tau,w) in acceleration computation, or tau
+  int filter_;                                   //!< Type of LPF used>
+  boost::shared_ptr<PinocchioModel> pin_model_;  //!< for reg cost
+  bool is_terminal_;                             //!< is it a terminal model or not ? (deactivate cost on w if true)
+
+};
+
+template <typename _Scalar>
+struct IntegratedActionDataLPFTpl : public ActionDataAbstractTpl<_Scalar> {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef _Scalar Scalar;
+  typedef MathBaseTpl<Scalar> MathBase;
+  typedef ActionDataAbstractTpl<Scalar> Base;
+  typedef typename MathBase::VectorXs VectorXs;
+  typedef typename MathBase::MatrixXs MatrixXs;
+  typedef pinocchio::DataTpl<Scalar> PinocchioData;   
+  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
+  typedef DifferentialActionDataFreeFwdDynamicsTpl<Scalar> DifferentialActionDataFreeFwdDynamics;
+  typedef ActivationDataQuadraticBarrierTpl<Scalar> ActivationDataQuadraticBarrier; // for lim cost
+  
+  template <template <typename Scalar> class Model>
+  explicit IntegratedActionDataLPFTpl(Model<Scalar>* const model) : Base(model) {
+        differential = model->get_differential()->createData();
+        const std::size_t& ndy = model->get_state()->get_ndx();
+        dy = VectorXs::Zero(ndy);
+        // for wreg grav cost
+        boost::shared_ptr<DifferentialActionDataFreeFwdDynamics> diff_data = 
+                boost::static_pointer_cast<DifferentialActionDataFreeFwdDynamics>(differential);
+        pinocchio = diff_data->pinocchio;
+        // for wlim cost
+        activation = boost::static_pointer_cast<ActivationDataQuadraticBarrier>(model->activation_model_w_lim_->createData());
+  }
+  virtual ~IntegratedActionDataLPFTpl() {}
+
+  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > differential;
+  VectorXs dy;
+
+  PinocchioData pinocchio;                                        // for reg cost
+  boost::shared_ptr<ActivationDataQuadraticBarrier> activation;   // for lim cost
+
+  using Base::cost;
+  using Base::r;
+  // use refs to "alias" base class member names 
+  VectorXs& ynext = Base::xnext;
+  MatrixXs& Fy = Base::Fx;    
+  MatrixXs& Fw = Base::Fu;     
+  VectorXs& Ly = Base::Lx;     
+  VectorXs& Lw = Base::Lu;     
+  MatrixXs& Lyy = Base::Lxx;   
+  MatrixXs& Lyw = Base::Lxu;   
+  MatrixXs& Lww = Base::Luu;   
+};
+
+}  // namespace crocoddyl
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "crocoddyl/core/integrator/lpf.hxx"
+
+#endif  // CROCODDYL_CORE_INTEGRATOR_LPF_HPP_
+
diff --git a/include/crocoddyl/core/integrator/lpf.hxx b/include/crocoddyl/core/integrator/lpf.hxx
new file mode 100644
index 0000000000..6fb71cf07d
--- /dev/null
+++ b/include/crocoddyl/core/integrator/lpf.hxx
@@ -0,0 +1,460 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include <iostream>
+
+#include <cmath> 
+
+#include "crocoddyl/core/utils/exception.hpp"
+#include "crocoddyl/core/integrator/lpf.hpp"
+
+namespace crocoddyl {
+
+template <typename Scalar>
+IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
+    boost::shared_ptr<DifferentialActionModelAbstract> model, 
+    const Scalar& time_step, 
+    const bool& with_cost_residual, 
+    const Scalar& fc, 
+    const Scalar& cost_weight_w_reg,
+    const VectorXs& cost_ref_w_reg,
+    const bool& w_gravity_reg,
+    const Scalar& cost_weight_w_lim,
+    const bool& tau_plus_integration, 
+    const int& filter,
+    const bool& is_terminal)
+    : Base(model->get_state(), model->get_nu(), model->get_nr()+2*model->get_nu()),
+      differential_(model),
+      time_step_(time_step),
+      time_step2_(time_step * time_step),
+      with_cost_residual_(with_cost_residual),
+      fc_(fc),
+      nw_(model->get_nu()),
+      ny_(model->get_state()->get_nx() + model->get_nu()),
+      enable_integration_(true),
+      filter_(filter), 
+      is_terminal_(is_terminal) {
+  Base::set_u_lb(differential_->get_u_lb());
+  Base::set_u_ub(differential_->get_u_ub());
+  if (time_step_ < Scalar(0.)) {
+    time_step_ = Scalar(1e-3);
+    time_step2_ = time_step_ * time_step_;
+    std::cerr << "Warning: dt should be positive, set to 1e-3" << std::endl;
+  }
+  if (time_step == Scalar(0.) || is_terminal_ == true) {
+    enable_integration_ = false;
+  }
+  // Compute alpha parameter from fc_
+  compute_alpha(fc_);
+  // Downcast DAM state (abstract --> multibody) 
+  boost::shared_ptr<StateMultibody> state = boost::static_pointer_cast<StateMultibody>(model->get_state());
+  // Instantiate stateLPF using pinocchio model of DAM state
+  state_ = boost::make_shared<StateLPF>(state->get_pinocchio(), model->get_nu());
+  // Set cost weight & ref for unfiltered torque regularization
+  cost_weight_w_reg_ = cost_weight_w_reg;
+  cost_ref_w_reg_ = cost_ref_w_reg;
+  w_gravity_reg_ = w_gravity_reg;
+  // Set cost weight for unfiltered torque limit cost
+  cost_weight_w_lim_ = cost_weight_w_lim;
+  // Use tau_plus or not in the integration
+  tau_plus_integration_ = tau_plus_integration;
+  // Get model and data to be used for grav cost?
+  pin_model_ = state->get_pinocchio();
+  // Activation model quad bar for wlim
+  VectorXs w_bounds = VectorXs(pin_model_.get()->effortLimit);
+  activation_bounds_w_lim_ = ActivationBounds(-w_bounds, w_bounds);
+  activation_model_w_lim_ = boost::make_shared<ActivationModelQuadraticBarrier>(activation_bounds_w_lim_);
+}
+
+
+template <typename Scalar>
+IntegratedActionModelLPFTpl<Scalar>::~IntegratedActionModelLPFTpl() {}
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
+                                               const Eigen::Ref<const VectorXs>& y,
+                                               const Eigen::Ref<const VectorXs>& w) {
+
+  const std::size_t& nv = differential_->get_state()->get_nv();
+  const std::size_t& nq = differential_->get_state()->get_nq();
+  const std::size_t& nx = differential_->get_state()->get_nx();
+  const std::size_t& ny = state_->get_nx();
+  const std::size_t& nu = differential_->get_nu();
+  const std::size_t& nw = nu;
+
+  if (static_cast<std::size_t>(y.size()) != ny) {
+    throw_pretty("Invalid argument: "
+                 << "y has wrong dimension (it should be " + std::to_string(ny) + ")");
+  }
+  if (static_cast<std::size_t>(w.size()) != nw) {
+    throw_pretty("Invalid argument: "
+                 << "w has wrong dimension (it should be " + std::to_string(nw) + ")");
+  }
+  // Static casting the data
+  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
+
+  // Extract x=(q,v) and tau from augmented state y
+  const Eigen::Ref<const VectorXs>& x = y.head(nx);                          // get q,v_q
+  const Eigen::Ref<const VectorXs>& tau = y.tail(nu);                        // get tau_q
+
+  // Compute acceleration and cost (DAM, i.e. CT model)
+  if(!tau_plus_integration_){                                               // TAU INTEGRATION
+    // a,cost = DAM(x,tau)
+    differential_->calc(d->differential, x, tau);                              // get a_q, cost = DAM(q, v_q, tau_q) 
+  } 
+  else {                                                                    // TAU PLUS INTEGRATION
+    // a,cost = DAM(x,tau+)
+    const Eigen::Ref<const VectorXs>& tau_plus = alpha_*tau + (1-alpha_)*w;    // get tau_q+ from (tau_q, w)
+    differential_->calc(d->differential, x, tau_plus);                         // get a_q, cost = DAM(q, v_q, tau_q+) 
+  } // DAM.calc
+  
+  // To store residuals of hard-coded costs on w regularization and w limit
+  VectorXs res_w_reg, res_w_lim;
+
+  // INTEGRATE (only for running nodes)
+  if(!is_terminal_){ 
+    // Computing the next state x+ = x + dx and cost+ = dt*cost
+    const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
+    const VectorXs& a = d->differential->xout;
+    d->dy.head(nv).noalias() = v * time_step_ + a * time_step2_;  // get     dq(a_q, dt)
+    d->dy.segment(nq, nv).noalias() = a * time_step_;             // get   dv_q(a_q, dt)
+    d->dy.tail(nu).noalias() = (1-alpha_)*(w-tau);                // get dtau_q(w, tau, alpha)
+    state_->integrate(y, d->dy, d->ynext);                        // integrate using stateLPF rule : tau+ = tau + dtau(tau, w)
+    d->cost = time_step_ * d->differential->cost;                 // get cost+ from cost
+    // Add hard-coded cost on unfiltered torque a[r(w)]  
+    if(w_gravity_reg_){                                      // Compute gravity torque residual w - g(q)
+      res_w_reg = w - pinocchio::computeGeneralizedGravity(*pin_model_, d->pinocchio, x.head(nq));
+    } else{                                                  // Compute torque residual w - wref
+      res_w_reg = w - cost_ref_w_reg_;
+    }
+    activation_model_w_lim_->calc(d->activation, w);         // Compute limit cost torque residual of w
+    res_w_lim = w;
+    d->cost += Scalar( 0.5 * time_step_ * cost_weight_w_reg_ * res_w_reg.transpose()*res_w_reg ); // w reg 
+    d->cost += Scalar( 0.5 * time_step_ * cost_weight_w_lim_ * d->activation->a_value );          // w lim
+  } // running model
+
+  // For TERMINAL node, no integration & no cost on control w
+  else {
+    d->dy.setZero();
+    d->ynext = y;
+    d->cost = d->differential->cost;
+  } // terminal model
+
+  // Update RESIDUAL
+  if (with_cost_residual_) {
+    d->r.head(differential_->get_nr() - 2*nw) = d->differential->r;
+    // Add unfiltered torque RESIDUAL(w) for RUNNING MODELS
+    if(!is_terminal_){
+      d->r.segment(differential_->get_nr(), nw) = res_w_reg;   // w reg 
+      d->r.tail(nw) = res_w_lim;                               // w lim
+    } // running residual
+  } // update residual
+
+} // calc
+
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
+                                                     const Eigen::Ref<const VectorXs>& y,
+                                                     const Eigen::Ref<const VectorXs>& w) {
+  
+  const std::size_t& nv = differential_->get_state()->get_nv();
+  const std::size_t& nq = differential_->get_state()->get_nq();
+  const std::size_t& nx = differential_->get_state()->get_nx();
+  const std::size_t& ny = state_->get_nx();
+  const std::size_t& nu = differential_->get_nu();
+  const std::size_t& nw = nu;
+
+  if (static_cast<std::size_t>(y.size()) != ny) {
+    throw_pretty("Invalid argument: "
+                 << "y has wrong dimension (it should be " + std::to_string(ny) + ")");
+  }
+  if (static_cast<std::size_t>(w.size()) != nw) {
+    throw_pretty("Invalid argument: "
+                 << "w has wrong dimension (it should be " + std::to_string(nw) + ")");
+  }
+  
+  // Static casting the data
+  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
+
+  // Computing the derivatives for the time-continuous model (i.e. differential model)
+  const Eigen::Ref<const VectorXs>& x = y.head(nx);                         // get q,v_q
+  const Eigen::Ref<const VectorXs>& tau = y.tail(nu);                       // get tau_q
+
+  // TAU INTEGRATION
+  if(!tau_plus_integration_)
+  {
+    // Get partials of CT model a_q ('f'), cost w.r.t. (q,v,tau)
+    differential_->calcDiff(d->differential, x, tau);     
+    // Get cost lim w derivatives
+    if(!is_terminal_){
+      activation_model_w_lim_->calcDiff(d->activation, w); 
+    }   
+
+    // Fill RUNNING MODELS partials of (y+,cost+) w.r.t. (y,w) 
+    if (!is_terminal_) { 
+      const MatrixXs& da_dx = d->differential->Fx;
+      const MatrixXs& da_du = d->differential->Fu;
+      // d(x+)/dy
+      d->Fy.block(0,0,nv,nx).noalias() = da_dx * time_step2_; 
+      d->Fy.block(nv,0,nv,nx).noalias() = da_dx * time_step_; 
+      d->Fy.block(0,nq,nv,nv).diagonal().array() += Scalar(time_step_); 
+      d->Fy.block(0,nx,nv,nu).noalias() = da_du * time_step2_; 
+      d->Fy.block(nv,nx,nv,nu).noalias() = da_du * time_step_; 
+      d->Fy.bottomRightCorner(nv,nu).diagonal().array() = Scalar(alpha_); 
+      state_->Jintegrate(y, d->dy, d->Fy, d->Fy, first, addto);  // add identity to Fx = d(x+dx)/dx = d(q,v)/d(q,v)
+      state_->JintegrateTransport(y, d->dy, d->Fy, second); 
+      // d(x+)/dw
+      d->Fw.setZero();
+      d->Fw.bottomRows(nu).diagonal().array() = Scalar(1-alpha_);
+      state_->JintegrateTransport(y, d->dy, d->Fw, second); 
+      // d(cost+)/dy
+      d->Ly.head(nx).noalias() = time_step_ * d->differential->Lx;
+      d->Ly.tail(nu).noalias() = time_step_ * d->differential->Lu;
+      d->Lyy.topLeftCorner(nx,nx).noalias() = time_step_ * d->differential->Lxx;
+      d->Lyy.block(0,nx,nx,nu).noalias() = time_step_ * d->differential->Lxu;
+      d->Lyy.block(nx,0,nu,nx).noalias() = time_step_ * d->differential->Lxu.transpose();
+      d->Lyy.bottomRightCorner(nu,nu).noalias() = time_step_ * d->differential->Luu; 
+      
+      // Partials of hard-coded cost+(wreg) & cost+(wlim) w.r.t. (y,w)   
+      if(w_gravity_reg_){ // depends on q --> impacts Ly, Lyy, Lyw
+        MatrixXs Rq = MatrixXs::Zero(nw, nq);
+        pinocchio::computeGeneralizedGravityDerivatives(*pin_model_, d->pinocchio, x.head(nq), Rq);
+        Rq *= -1;
+        d->Ly.head(nq).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose() * d->r.segment(differential_->get_nr(), nw);
+        // Neglect tensor term ( second derivative of gravity w.r.t. q )
+        d->Lyy.topLeftCorner(nq,nw).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose() * Rq;
+        d->Lyw.topLeftCorner(nq,nw).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose();
+      }
+      d->Lw.noalias() = time_step_ * cost_weight_w_reg_ * d->r.segment(differential_->get_nr(), nw); // w reg
+      d->Lw.noalias() += time_step_ * cost_weight_w_lim_ * d->activation->Ar;                        // w lim
+      d->Lww.diagonal().array() = Scalar(time_step_ * cost_weight_w_reg_ );                          // w reg
+      d->Lww.diagonal() += time_step_ * cost_weight_w_lim_ * d->activation->Arr.diagonal();          // w lim
+    } // running model
+    
+    // Fill TERMINAL MODEL partials of ( a(yT), cost(yT) ) w.r.t. y 
+    else {
+      state_->Jintegrate(y, d->dy, d->Fy, d->Fy);
+      d->Fw.setZero();
+      d->Ly.head(nx).noalias() = d->differential->Lx;
+      d->Ly.tail(nu).noalias() = d->differential->Lu;
+      d->Lyy.topLeftCorner(nx,nx).noalias() = d->differential->Lxx;
+      d->Lyy.block(0,nx,nx,nu).noalias() = d->differential->Lxu;
+      d->Lyy.block(nx,0,nu,nx).noalias() = d->differential->Lxu.transpose();
+      d->Lyy.bottomRightCorner(nu,nu).noalias() = d->differential->Luu; 
+      d->Lw.setZero();
+      d->Lww.setZero();
+      d->Lyw.setZero();
+    } // terminal model
+
+  } // tau integration
+
+  // TAU PLUS INTEGRATION
+  else
+  {
+    // get tau_q+ from (tau_q, w)
+    const Eigen::Ref<const VectorXs>& tau_plus = alpha_*tau + (1-alpha_)*w;   
+    // Get partials of CT model a_q ('f'), cost w.r.t. (q,v,tau+)
+    differential_->calcDiff(d->differential, x, tau_plus);     
+    // Get cost lim w 
+    if(!is_terminal_){
+      activation_model_w_lim_->calcDiff(d->activation, w); 
+    }   
+    // Fill out partials of IAM 
+    if (enable_integration_) {
+      const MatrixXs& da_dx = d->differential->Fx;
+      const MatrixXs& da_du = d->differential->Fu;
+      d->Fy.block(0,0,nv,nx).noalias() = da_dx * time_step2_;
+      d->Fy.block(nv,0,nv,nx).noalias() = da_dx * time_step_;
+      d->Fy.block(0,nx,nv,nu).noalias() = alpha_ * alpha_ * da_du * time_step2_;
+      d->Fy.block(nv,nx,nv,nu).noalias() = alpha_ * da_du * time_step_;
+      d->Fy.block(0,nq,nv,nv).diagonal().array() += Scalar(time_step_); // dt*identity top row middle col (eq. Jsecond = d(xnext)/d(dx))
+      // d->Fy.topLeftCorner(nx, nx).diagonal().array() += Scalar(1.);     // managed by Jintegrate                  (eq. Jsecond = d(xnext)/d(dx))
+      d->Fy.bottomRightCorner(nv,nu).diagonal().array() = Scalar(alpha_);
+      d->Fw.topRows(nv).noalias() = da_du * time_step2_ * (1-alpha_);
+      d->Fw.block(nv,0,nv,nu).noalias() = da_du * time_step_ * (1-alpha_);
+      d->Fw.bottomRows(nv).diagonal().array() = Scalar(1-alpha_);
+      state_->JintegrateTransport(y, d->dy, d->Fy, second); // it this correct?
+      state_->Jintegrate(y, d->dy, d->Fy, d->Fy, first, addto); // for d(x+dx)/d(x)  
+      state_->JintegrateTransport(y, d->dy, d->Fw, second); // it this correct?
+      d->Ly.head(nx).noalias() = time_step_ * d->differential->Lx;
+      d->Ly.tail(nu).noalias() = alpha_ * time_step_ * d->differential->Lu;
+      d->Lw.noalias() = (1-alpha_) * time_step_ * d->differential->Lu;
+      d->Lyy.topLeftCorner(nx,nx).noalias() = time_step_ * d->differential->Lxx;
+      d->Lyy.block(0,nx,nx,nu).noalias() = alpha_ * time_step_ * d->differential->Lxu;
+      d->Lyy.block(nx,0,nu,nx).noalias() = alpha_ * time_step_ * d->differential->Lxu.transpose();
+      d->Lyy.bottomRightCorner(nu,nu).noalias() = alpha_ * alpha_ * time_step_ * d->differential->Luu; 
+      d->Lyw.topRows(nx).noalias() = (1-alpha_) * time_step_ * d->differential->Lxu;
+      d->Lyw.bottomRows(nu).noalias() = (1-alpha_) * alpha_ * time_step_ * d->differential->Luu;
+      d->Lww.noalias() = (1-alpha_) * (1-alpha_) * time_step_ * d->differential->Luu;
+      // Add partials related to unfiltered torque costs w_reg, w_lim (only for running models)
+      if(!is_terminal_){
+        // Torque gravity regularization : depends on q --> impacts Ly
+        if(w_gravity_reg_){
+          MatrixXs Rq = MatrixXs::Zero(nw, nq);
+          pinocchio::computeGeneralizedGravityDerivatives(*pin_model_, d->pinocchio, x.head(nq), Rq);
+          Rq *= -1;
+          d->Ly.head(nq).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose() * d->r.tail(nw);
+          // Neglect tensor term ( second derivative of gravity w.r.t. q )
+          d->Lyy.topLeftCorner(nq,nw).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose() * Rq;
+          d->Lyw.topLeftCorner(nq,nw).noalias() += time_step_ * cost_weight_w_reg_ * Rq.transpose();
+        }
+        // Torque reg and lim
+        d->Lw.noalias() = time_step_ * cost_weight_w_reg_ * d->r.segment(differential_->get_nr(), nw); // w reg
+        d->Lw.noalias() += time_step_ * cost_weight_w_lim_ * d->activation->Ar;              // w lim
+        d->Lww.diagonal().array() = Scalar(time_step_ * cost_weight_w_reg_ );                // reg
+        d->Lww.diagonal() += time_step_ * cost_weight_w_lim_ * d->activation->Arr.diagonal(); // lim
+      }
+
+    } else {
+      // state_->Jintegrate(y, d->dy, d->Fy, d->Fy);
+      d->Fw.setZero();
+      d->Ly.head(nx).noalias() = d->differential->Lx;
+      d->Ly.tail(nu).noalias() = alpha_ * d->differential->Lu;
+      d->Lw.noalias() = (1-alpha_) * d->differential->Lu;
+      d->Lyy.topLeftCorner(nx,nx).noalias() = d->differential->Lxx;
+      d->Lyy.block(0,nx,nx,nu).noalias() = alpha_ * d->differential->Lxu;
+      d->Lyy.block(nx,0,nu,nx).noalias() = alpha_ * d->differential->Lxu.transpose();
+      d->Lyy.bottomRightCorner(nu,nu).noalias() = alpha_ * alpha_ * d->differential->Luu; 
+      d->Lyw.topRows(nx).noalias() = (1-alpha_) * d->differential->Lxu;
+      d->Lyw.bottomRows(nu).noalias() = (1-alpha_) * alpha_ * d->differential->Luu;
+      d->Lww.noalias() = (1-alpha_) * (1-alpha_) * d->differential->Luu;
+      // Add partials related to unfiltered torque costs w_reg, w_lim (only for running models)
+      if(!is_terminal_){
+        if(w_gravity_reg_){
+          MatrixXs Rq = MatrixXs::Zero(nw, nq);
+          pinocchio::computeGeneralizedGravityDerivatives(*pin_model_, d->pinocchio, x.head(nq), Rq);
+          Rq *= -1;
+          d->Ly.head(nq).noalias() += cost_weight_w_reg_ * Rq.transpose() * d->r.segment(differential_->get_nr(), nw);
+          // Neglect tensor term ( second derivative of gravity w.r.t. q )
+          d->Lyy.topLeftCorner(nq,nw).noalias() += cost_weight_w_reg_ * Rq.transpose() * Rq;
+          d->Lyw.topLeftCorner(nq,nw).noalias() += cost_weight_w_reg_ * Rq.transpose();
+        }
+        d->Lw.noalias() += cost_weight_w_reg_ * d->r.segment(differential_->get_nr(), nw);   // reg
+        d->Lw.noalias() += cost_weight_w_lim_ * d->activation->Ar;               // lim
+        d->Lww.diagonal().array() += Scalar( cost_weight_w_reg_ );               // w reg
+        d->Lww.diagonal() += cost_weight_w_lim_ * d->activation->Arr.diagonal(); // w lim
+      }
+    }
+  } // tau_plus_integration
+}
+
+template <typename Scalar>
+boost::shared_ptr<ActionDataAbstractTpl<Scalar> > IntegratedActionModelLPFTpl<Scalar>::createData() {
+  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
+}
+
+template <typename Scalar>
+bool IntegratedActionModelLPFTpl<Scalar>::checkData(const boost::shared_ptr<ActionDataAbstract>& data) {
+  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
+  if (data != NULL) {
+    return differential_->checkData(d->differential);
+  } else {
+    return false;
+  }
+}
+
+template <typename Scalar>
+const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
+IntegratedActionModelLPFTpl<Scalar>::get_differential() const {
+  return differential_;
+}
+
+template <typename Scalar>
+const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_dt() const {
+  return time_step_;
+}
+
+template <typename Scalar>
+const Scalar& IntegratedActionModelLPFTpl<Scalar>::get_fc() const {
+  return fc_;
+}
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::set_dt(const Scalar& dt) {
+  if (dt < 0.) {
+    throw_pretty("Invalid argument: "
+                 << "dt has positive value");
+  }
+  time_step_ = dt;
+  time_step2_ = dt * dt;
+}
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::set_fc(const Scalar& fc) {
+  // Set the cut-off frequency
+  if (fc <= 0.) {
+    throw_pretty("Invalid argument: "
+                 << "fc must be positive");
+  }
+  else {
+    fc_ = fc;
+  }
+}
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::compute_alpha(const Scalar& fc) {
+  // Update alpha parameter
+  if (fc > 0 && time_step_ != 0){
+    const Scalar& pi = 3.14159;
+    // Exponential Moving Average (EMA) (IIR filter) >> quite sharp
+    if (filter_ == 0) {
+      alpha_ = exp(-2.* pi * fc * time_step_);
+    }
+    // Using the formula "fc = 1/2*pi*RC" ??? >> less sharp
+    if (filter_ == 1) {
+      double omega = 1 / (2. * pi * time_step_ * fc);
+      alpha_ = omega/(omega + 1);
+    }
+    // Exact formula to get fc out of EMA's alpha >> inbetween sharp
+    if (filter_ == 2) {
+      double y = cos(2.* pi * time_step_ * fc);
+      alpha_ = 1 - (y - 1 + sqrt(y*y - 4*y +3));
+    }
+  }
+  else {
+    alpha_ = 0;
+  }
+}
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::set_differential(
+    boost::shared_ptr<DifferentialActionModelAbstract> model) {
+  const std::size_t& nu = model->get_nu();
+  if (nu_ != nu) {
+    nu_ = nu;
+    unone_ = VectorXs::Zero(nu_);
+  }
+  nr_ = model->get_nr()+nu;
+  state_ = boost::static_pointer_cast<StateLPF>(model->get_state()); // cast StateAbstract from DAM as StateLPF for IAM
+  differential_ = model;
+  Base::set_u_lb(differential_->get_u_lb());
+  Base::set_u_ub(differential_->get_u_ub());
+}
+
+
+template <typename Scalar>
+void IntegratedActionModelLPFTpl<Scalar>::quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data,
+                                                        Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
+                                                        const std::size_t& maxiter, const Scalar& tol) {
+  if (static_cast<std::size_t>(u.size()) != nu_) {
+    throw_pretty("Invalid argument: "
+                 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
+  }
+  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
+    throw_pretty("Invalid argument: "
+                 << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
+  }
+
+  // Static casting the data
+  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
+
+  differential_->quasiStatic(d->differential, u, x, maxiter, tol);
+}
+
+}  // namespace crocoddyl
\ No newline at end of file
diff --git a/include/crocoddyl/core/optctrl/shooting.hxx b/include/crocoddyl/core/optctrl/shooting.hxx
index abd21a11d8..fbd80707a3 100644
--- a/include/crocoddyl/core/optctrl/shooting.hxx
+++ b/include/crocoddyl/core/optctrl/shooting.hxx
@@ -163,7 +163,7 @@ Scalar ShootingProblemTpl<Scalar>::calc(const std::vector<VectorXs>& xs, const s
   for (std::size_t i = 0; i < T_; ++i) {
     const std::size_t nu = running_models_[i]->get_nu();
     if (nu != 0) {
-      running_models_[i]->calc(running_datas_[i], xs[i], us[i]);
+      running_models_[i]->calc(running_datas_[i], xs[i], us[i].head(nu));
     } else {
       running_models_[i]->calc(running_datas_[i], xs[i]);
     }
diff --git a/include/crocoddyl/core/state-base.hxx b/include/crocoddyl/core/state-base.hxx
index 42de6854f0..cc11c94ad3 100644
--- a/include/crocoddyl/core/state-base.hxx
+++ b/include/crocoddyl/core/state-base.hxx
@@ -6,7 +6,7 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 #include "crocoddyl/core/mathbase.hpp"
-
+#include <iostream> //
 namespace crocoddyl {
 template <typename Scalar>
 StateAbstractTpl<Scalar>::StateAbstractTpl(const std::size_t nx, const std::size_t ndx)
diff --git a/include/crocoddyl/multibody/fwd.hpp b/include/crocoddyl/multibody/fwd.hpp
index b9900e4540..a7bda328c8 100644
--- a/include/crocoddyl/multibody/fwd.hpp
+++ b/include/crocoddyl/multibody/fwd.hpp
@@ -303,6 +303,8 @@ class CoPSupportTpl;
 // state
 template <typename Scalar>
 class StateMultibodyTpl;
+template <typename Scalar>
+class StateLPFTpl;
 
 // data collector
 template <typename Scalar>
@@ -488,6 +490,7 @@ typedef ContactModel6DTpl<double> ContactModel6D;
 typedef ContactData6DTpl<double> ContactData6D;
 
 typedef StateMultibodyTpl<double> StateMultibody;
+typedef StateLPFTpl<double> StateLPF;
 
 typedef DataCollectorMultibodyTpl<double> DataCollectorMultibody;
 typedef DataCollectorActMultibodyTpl<double> DataCollectorActMultibody;
diff --git a/include/crocoddyl/multibody/states/statelpf.hpp b/include/crocoddyl/multibody/states/statelpf.hpp
new file mode 100644
index 0000000000..640cf75721
--- /dev/null
+++ b/include/crocoddyl/multibody/states/statelpf.hpp
@@ -0,0 +1,78 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_STATES_STATELPF_HPP_
+#define CROCODDYL_MULTIBODY_STATES_STATELPF_HPP_
+#include "crocoddyl/multibody/fwd.hpp"
+#include "crocoddyl/core/state-base.hpp"
+
+#include <pinocchio/multibody/model.hpp>
+
+namespace crocoddyl {
+
+template <typename _Scalar>
+class StateLPFTpl : public StateAbstractTpl<_Scalar> {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef _Scalar Scalar;
+  typedef MathBaseTpl<Scalar> MathBase;
+  typedef StateAbstractTpl<Scalar> Base;
+  typedef typename MathBase::VectorXs VectorXs;
+  typedef typename MathBase::MatrixXs MatrixXs;
+
+  enum JointType { FreeFlyer = 0, Spherical, Simple };
+
+  explicit StateLPFTpl(boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model, std::size_t nu = 0);
+  virtual ~StateLPFTpl();
+
+  virtual VectorXs zero() const;
+  virtual VectorXs rand() const;
+  virtual void diff(const Eigen::Ref<const VectorXs>& y0, const Eigen::Ref<const VectorXs>& y1,
+                    Eigen::Ref<VectorXs> dyout) const;
+  virtual void integrate(const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
+                         Eigen::Ref<VectorXs> yout) const;
+  virtual void Jdiff(const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&, Eigen::Ref<MatrixXs> Jfirst,
+                     Eigen::Ref<MatrixXs> Jsecond, const Jcomponent firstsecond = both) const;
+
+  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
+                          Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
+                          const Jcomponent firstsecond = both, const AssignmentOp = setto) const;
+  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
+                                   Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const;
+
+  const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >& get_pinocchio() const;
+  const std::size_t& get_nw() const;
+  const std::size_t& get_ny() const;
+
+ protected:
+  using Base::has_limits_;
+  using Base::lb_;
+  using Base::ndx_;
+  using Base::nq_;
+  using Base::nv_;
+  using Base::nx_;
+  using Base::ub_;
+  std::size_t nw_;
+  std::size_t ny_;
+  std::size_t ndy_;
+
+ private:
+  boost::shared_ptr<pinocchio::ModelTpl<Scalar> > pinocchio_;
+  VectorXs y0_;
+  JointType joint_type_;
+};
+
+}  // namespace crocoddyl
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "crocoddyl/multibody/states/statelpf.hxx"
+
+#endif  // CROCODDYL_MULTIBODY_STATES_STATELPF_HPP_
\ No newline at end of file
diff --git a/include/crocoddyl/multibody/states/statelpf.hxx b/include/crocoddyl/multibody/states/statelpf.hxx
new file mode 100644
index 0000000000..3eb00fef27
--- /dev/null
+++ b/include/crocoddyl/multibody/states/statelpf.hxx
@@ -0,0 +1,265 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "crocoddyl/core/utils/exception.hpp"
+#include "crocoddyl/multibody/states/statelpf.hpp"
+#include <pinocchio/algorithm/joint-configuration.hpp>
+
+namespace crocoddyl {
+
+template <typename Scalar>
+StateLPFTpl<Scalar>::StateLPFTpl(boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model, std::size_t nu)
+    : Base(model->nq + model->nv + nu, 2 * model->nv + nu), pinocchio_(model), y0_(VectorXs::Zero(model->nq + model->nv + nu)) {
+  y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
+
+  // In a multibody system, we could define the first joint using Lie groups.
+  // The current cases are free-flyer (SE3) and spherical (S03).
+  // Instead simple represents any joint that can model within the Euclidean manifold.
+  // The rest of joints use Euclidean algebra. We use this fact for computing Jdiff.
+
+  nq_ = model->nq;    // joint pos
+  nv_ = model->nv;    // joint vel
+  ny_ = nx_;     // full state: joint pos, vel, torques
+  ndy_ = ndx_; //+ nu;  
+  nw_ = nu;           // unfiltered torque (control input) 
+
+  // Define internally the limits of the first joint
+  const std::size_t nq0 = model->joints[1].nq();
+
+  lb_.head(nq0) = -std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
+  ub_.head(nq0) = std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
+  lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
+  ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
+  lb_.segment(nq_, nv_) = -pinocchio_->velocityLimit;
+  ub_.segment(nq_, nv_) = pinocchio_->velocityLimit;
+  lb_.tail(nw_) = -pinocchio_->effortLimit;
+  ub_.tail(nw_) = pinocchio_->effortLimit;
+  Base::update_has_limits();
+
+  y0_.tail(nv_ + nu) = VectorXs::Zero(nv_ + nu); // BUG of else some values were uninitialized
+  y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
+}
+
+template <typename Scalar>
+StateLPFTpl<Scalar>::~StateLPFTpl() {}
+
+template <typename Scalar>
+const std::size_t& StateLPFTpl<Scalar>::get_nw() const {
+  return nw_;
+}
+
+template <typename Scalar>
+const std::size_t& StateLPFTpl<Scalar>::get_ny() const {
+  return ny_;
+}
+
+template <typename Scalar>
+typename MathBaseTpl<Scalar>::VectorXs StateLPFTpl<Scalar>::zero() const {
+  return y0_;
+}
+
+template <typename Scalar>
+typename MathBaseTpl<Scalar>::VectorXs StateLPFTpl<Scalar>::rand() const {
+  VectorXs yrand = VectorXs::Random(ny_);
+  yrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
+  return yrand;
+}
+
+template <typename Scalar>
+void StateLPFTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& y0, const Eigen::Ref<const VectorXs>& y1,
+                                     Eigen::Ref<VectorXs> dyout) const {
+  if (static_cast<std::size_t>(y0.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "y0 has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+  if (static_cast<std::size_t>(y1.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "y1 has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+  if (static_cast<std::size_t>(dyout.size()) != ndy_) {
+    throw_pretty("Invalid argument: "
+                 << "dyout has wrong dimension (it should be " + std::to_string(ndy_) + ")");
+  }
+
+  pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), dyout.head(nv_));
+  dyout.segment(nq_, nv_) = y1.segment(nq_, nv_) - y0.segment(nq_, nv_);
+  dyout.tail(nw_) = y1.tail(nw_) - y0.tail(nw_);
+}
+
+template <typename Scalar>
+void StateLPFTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
+                                          Eigen::Ref<VectorXs> yout) const {
+  if (static_cast<std::size_t>(y.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "y has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+  if (static_cast<std::size_t>(dy.size()) != ndy_) {
+    throw_pretty("Invalid argument: "
+                 << "dy has wrong dimension (it should be " + std::to_string(ndy_) + ")");
+  }
+  if (static_cast<std::size_t>(yout.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "yout has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+
+  pinocchio::integrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), yout.head(nq_));
+  yout.segment(nq_, nv_) = y.segment(nq_, nv_) + dy.segment(nq_, nv_);
+  yout.tail(nw_) = y.tail(nw_) + dy.tail(nw_);
+}
+
+template <typename Scalar>
+void StateLPFTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& y0, const Eigen::Ref<const VectorXs>& y1,
+                                      Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
+                                      const Jcomponent firstsecond) const {
+  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
+  if (static_cast<std::size_t>(y0.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "y0 has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+  if (static_cast<std::size_t>(y1.size()) != ny_) {
+    throw_pretty("Invalid argument: "
+                 << "y1 has wrong dimension (it should be " + std::to_string(ny_) + ")");
+  }
+
+  if (firstsecond == first) {
+    if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ || static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jfirst has wrong dimension (it should be " + std::to_string(ndy_) + "," + std::to_string(ndy_) +
+                          ")");
+    }
+
+    pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jfirst.topLeftCorner(nv_, nv_),
+                           pinocchio::ARG0);
+    Jfirst.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)-1;
+    Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
+  } else if (firstsecond == second) {
+    if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ || static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jsecond has wrong dimension (it should be " + std::to_string(ndy_) + "," +
+                          std::to_string(ndy_) + ")");
+    }
+    pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jsecond.topLeftCorner(nv_, nv_),
+                           pinocchio::ARG1);
+    Jsecond.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)1;
+    Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
+  } else {  // computing both
+    if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ || static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jfirst has wrong dimension (it should be " + std::to_string(ndy_) + "," + std::to_string(ndy_) +
+                          ")");
+    }
+    if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ || static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jsecond has wrong dimension (it should be " + std::to_string(ndy_) + "," +
+                          std::to_string(ndy_) + ")");
+    }
+    pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jfirst.topLeftCorner(nv_, nv_),
+                           pinocchio::ARG0);
+    pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jsecond.topLeftCorner(nv_, nv_),
+                           pinocchio::ARG1);
+    Jfirst.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)-1;
+    Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
+    Jsecond.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)1;
+    Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
+  }
+}
+
+template <typename Scalar>
+void StateLPFTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
+                                           Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
+                                           const Jcomponent firstsecond, const AssignmentOp op) const {
+  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
+  assert_pretty(is_a_AssignmentOp(op), ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
+  if (firstsecond == first || firstsecond == both) {
+    if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ || static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jfirst has wrong dimension (it should be " + std::to_string(ndy_) + "," + std::to_string(ndy_) +
+                          ")");
+    }
+    switch (op) {
+      case setto:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG0, pinocchio::SETTO);
+        Jfirst.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)1;
+        // Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
+        break;
+      case addto:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG0, pinocchio::ADDTO);
+        Jfirst.block(nq_, nq_, nv_, nv_).diagonal().array() += (Scalar)1;
+        // Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
+        break;
+      case rmfrom:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG0, pinocchio::RMTO);
+        Jfirst.block(nq_, nq_, nv_, nv_).diagonal().array() -= (Scalar)1;
+        // Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
+        break;
+      default:
+        throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
+        break;
+    }
+  }
+  if (firstsecond == second || firstsecond == both) {
+    if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ || static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
+      throw_pretty("Invalid argument: "
+                   << "Jsecond has wrong dimension (it should be " + std::to_string(ndy_) + "," +
+                          std::to_string(ndy_) + ")");
+    }
+    switch (op) {
+      case setto:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG1, pinocchio::SETTO);
+        Jsecond.block(nq_, nq_, nv_, nv_).diagonal().array() = (Scalar)1;
+        // Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
+        break;
+      case addto:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG1, pinocchio::ADDTO);
+        Jsecond.block(nq_, nq_, nv_, nv_).diagonal().array() += (Scalar)1;
+        // Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
+        break;
+      case rmfrom:
+        pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
+                              pinocchio::ARG1, pinocchio::RMTO);
+        Jsecond.block(nq_, nq_, nv_, nv_).diagonal().array() -= (Scalar)1;
+        // Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
+        break;
+      default:
+        throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
+        break;
+    }
+  }
+}
+
+template <typename Scalar>
+void StateLPFTpl<Scalar>::JintegrateTransport(const Eigen::Ref<const VectorXs>& y,
+                                                    const Eigen::Ref<const VectorXs>& dy, Eigen::Ref<MatrixXs> Jin,
+                                                    const Jcomponent firstsecond) const {
+  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
+
+  switch (firstsecond) {
+    case first:
+      pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jin.topLeftCorner(nv_,nx_), pinocchio::ARG0);
+      break;
+    case second:
+      pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_), dy.head(nv_), Jin.topLeftCorner(nv_,nx_), pinocchio::ARG1);
+      break;
+    default:
+      throw_pretty(
+          "Invalid argument: firstsecond must be either first or second. both not supported for this operation.");
+      break;
+  }
+}
+
+template <typename Scalar>
+const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >& StateLPFTpl<Scalar>::get_pinocchio() const {
+  return pinocchio_;
+}
+
+}  // namespace crocoddyl
\ No newline at end of file