diff --git a/bindings/expose-centroidal.cpp b/bindings/expose-centroidal.cpp
index 9f9165a6..dce6c39f 100644
--- a/bindings/expose-centroidal.cpp
+++ b/bindings/expose-centroidal.cpp
@@ -6,8 +6,7 @@
 namespace simple_mpc::python
 {
 
-  auto * createCentroidal(
-    const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
+  auto * createCentroidal(const bp::dict & settings, const RobotModelHandler & model_handler)
   {
     CentroidalSettings conf;
     conf.timestep = bp::extract<double>(settings["timestep"]);
@@ -25,7 +24,7 @@ namespace simple_mpc::python
     conf.Lfoot = bp::extract<double>(settings["Lfoot"]);
     conf.Wfoot = bp::extract<double>(settings["Wfoot"]);
 
-    return new CentroidalOCP(conf, model_handler, data_handler);
+    return new CentroidalOCP(conf, model_handler);
   }
 
   StageModel createCentStage(
@@ -113,8 +112,8 @@ namespace simple_mpc::python
 
     bp::class_<CentroidalOCP, bp::bases<OCPHandler>, boost::noncopyable>("CentroidalOCP", bp::no_init)
       .def(
-        "__init__", bp::make_constructor(
-                      &createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
+        "__init__",
+        bp::make_constructor(&createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler")))
       .def("getSettings", &getSettingsCent)
       .def("createStage", &createCentStage);
   }
diff --git a/bindings/expose-fulldynamics.cpp b/bindings/expose-fulldynamics.cpp
index 7fee90b8..24e20fb0 100644
--- a/bindings/expose-fulldynamics.cpp
+++ b/bindings/expose-fulldynamics.cpp
@@ -7,8 +7,7 @@
 namespace simple_mpc::python
 {
 
-  auto * createFulldynamics(
-    const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
+  auto * createFulldynamics(const bp::dict & settings, const RobotModelHandler & model_handler)
   {
     FullDynamicsSettings conf;
     conf.timestep = bp::extract<double>(settings["timestep"]);
@@ -41,7 +40,7 @@ namespace simple_mpc::python
     conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
     conf.force_cone = bp::extract<bool>(settings["force_cone"]);
 
-    return new FullDynamicsOCP(conf, model_handler, data_handler);
+    return new FullDynamicsOCP(conf, model_handler);
   }
 
   StageModel createFullStage(
@@ -138,8 +137,7 @@ namespace simple_mpc::python
     bp::class_<FullDynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("FullDynamicsOCP", bp::no_init)
       .def(
         "__init__",
-        bp::make_constructor(
-          &createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
+        bp::make_constructor(&createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))
       .def("getSettings", &getSettingsFull)
       .def("createStage", &createFullStage);
   }
diff --git a/bindings/expose-kinodynamics.cpp b/bindings/expose-kinodynamics.cpp
index 70cc1055..cc5b5aab 100644
--- a/bindings/expose-kinodynamics.cpp
+++ b/bindings/expose-kinodynamics.cpp
@@ -6,8 +6,7 @@
 namespace simple_mpc::python
 {
 
-  auto * createKinodynamics(
-    const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
+  auto * createKinodynamics(const bp::dict & settings, const RobotModelHandler & model_handler)
   {
     KinodynamicsSettings conf;
     conf.timestep = bp::extract<double>(settings["timestep"]);
@@ -30,7 +29,7 @@ namespace simple_mpc::python
     conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
     conf.force_cone = bp::extract<bool>(settings["force_cone"]);
 
-    return new KinodynamicsOCP(conf, model_handler, data_handler);
+    return new KinodynamicsOCP(conf, model_handler);
   }
 
   bp::dict getSettingsKino(KinodynamicsOCP & self)
@@ -122,8 +121,7 @@ namespace simple_mpc::python
     bp::class_<KinodynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("KinodynamicsOCP", bp::no_init)
       .def(
         "__init__",
-        bp::make_constructor(
-          &createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
+        bp::make_constructor(&createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))
       .def("getSettings", &getSettingsKino)
       .def("createStage", &createKinoStage);
   }
diff --git a/bindings/expose-problem.cpp b/bindings/expose-problem.cpp
index c57dcde9..2fc76d6e 100644
--- a/bindings/expose-problem.cpp
+++ b/bindings/expose-problem.cpp
@@ -44,7 +44,7 @@ namespace simple_mpc
     {
       bp::register_ptr_to_python<std::shared_ptr<OCPHandler>>();
       bp::class_<PyOCPHandler, boost::noncopyable>("OCPHandler", bp::no_init)
-        .def(bp::init<const RobotModelHandler &, const RobotDataHandler &>(("self"_a, "model_handler", "data_handler")))
+        .def(bp::init<const RobotModelHandler &>(("self"_a, "model_handler")))
         .def(
           "createStage", bp::pure_virtual(&OCPHandler::createStage),
           ("self"_a, "contact_map", "force_refs", "land_constraint"))
@@ -69,7 +69,7 @@ namespace simple_mpc
         .def("getVelocityBase", bp::pure_virtual(&OCPHandler::getVelocityBase), bp::args("self", "t"))
         .def("setPoseBase", bp::pure_virtual(&OCPHandler::setPoseBase), bp::args("self", "t", "pose_base"))
         .def("getPoseBase", bp::pure_virtual(&OCPHandler::getPoseBase), bp::args("self", "t"))
-        .def("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self"))
+        .def("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self", "data_handler"))
         .def("getContactSupport", bp::pure_virtual(&OCPHandler::getContactSupport), bp::args("self", "t"))
         .def(
           "createProblem", &OCPHandler::createProblem,
diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py
index 51e1c330..e328afe6 100644
--- a/examples/go2_fulldynamics.py
+++ b/examples/go2_fulldynamics.py
@@ -64,7 +64,7 @@
 )
 T = 50
 
-dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)
+dynproblem = FullDynamicsOCP(problem_conf, model_handler)
 dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)
 
 T_ds = 10
diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py
index 411501bb..07e95791 100644
--- a/examples/go2_kinodynamics.py
+++ b/examples/go2_kinodynamics.py
@@ -74,7 +74,7 @@
 )
 T = 50
 
-dynproblem = KinodynamicsOCP(problem_conf, model_handler, data_handler)
+dynproblem = KinodynamicsOCP(problem_conf, model_handler)
 dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)
 
 T_ds = 10
diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py
index e6239231..a99c02ea 100644
--- a/examples/talos_centroidal.py
+++ b/examples/talos_centroidal.py
@@ -68,7 +68,7 @@
 )
 T = 100
 
-problem = CentroidalOCP(problem_conf, model_handler, data_handler)
+problem = CentroidalOCP(problem_conf, model_handler)
 problem.createProblem(data_handler.getCentroidalState(), T, 6, gravity[2], False)
 
 T_ds = 20
diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py
index 21ef44c9..6b328632 100644
--- a/examples/talos_fulldynamics.py
+++ b/examples/talos_fulldynamics.py
@@ -92,7 +92,7 @@
 )
 
 T = 100
-dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)
+dynproblem = FullDynamicsOCP(problem_conf, model_handler)
 dynproblem.createProblem(x0, T, 6, gravity[2], False)
 
 """ Define feet trajectory """
diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py
index 79516a0d..63f75916 100644
--- a/examples/talos_kinodynamics.py
+++ b/examples/talos_kinodynamics.py
@@ -104,7 +104,7 @@
 
 T = 100
 
-problem = KinodynamicsOCP(problem_conf, model_handler, data_handler)
+problem = KinodynamicsOCP(problem_conf, model_handler)
 problem.createProblem(model_handler.getReferenceState(), T, 6, gravity[2], False)
 
 """ Define MPC object """
diff --git a/include/simple-mpc/centroidal-dynamics.hpp b/include/simple-mpc/centroidal-dynamics.hpp
index 2fde260b..8d625385 100644
--- a/include/simple-mpc/centroidal-dynamics.hpp
+++ b/include/simple-mpc/centroidal-dynamics.hpp
@@ -59,10 +59,7 @@ namespace simple_mpc
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     // Constructor
-    explicit CentroidalOCP(
-      const CentroidalSettings & settings,
-      const RobotModelHandler & model_handler,
-      const RobotDataHandler & data_handler);
+    explicit CentroidalOCP(const CentroidalSettings & settings, const RobotModelHandler & model_handler);
 
     SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(CentroidalOCP);
 
@@ -77,7 +74,7 @@ namespace simple_mpc
 
     // Manage terminal cost and constraint
     CostStack createTerminalCost() override;
-    void createTerminalConstraint() override;
+    void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
     void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;
 
     // Getters and setters for pose not implemented
@@ -98,7 +95,7 @@ namespace simple_mpc
     void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
     void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
-    const Eigen::VectorXd getProblemState() override;
+    const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
 
     CentroidalSettings getSettings()
diff --git a/include/simple-mpc/fulldynamics.hpp b/include/simple-mpc/fulldynamics.hpp
index e0341072..5e0f4a87 100644
--- a/include/simple-mpc/fulldynamics.hpp
+++ b/include/simple-mpc/fulldynamics.hpp
@@ -69,10 +69,7 @@ namespace simple_mpc
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     // Constructors
-    explicit FullDynamicsOCP(
-      const FullDynamicsSettings & settings,
-      const RobotModelHandler & model_handler,
-      const RobotDataHandler & data_handler);
+    explicit FullDynamicsOCP(const FullDynamicsSettings & settings, const RobotModelHandler & model_handler);
 
     SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(FullDynamicsOCP);
 
@@ -87,7 +84,7 @@ namespace simple_mpc
 
     // Manage terminal cost and constraint
     CostStack createTerminalCost() override;
-    void createTerminalConstraint() override;
+    void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
     void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;
 
     // Getters and setters
@@ -103,7 +100,7 @@ namespace simple_mpc
     void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
     void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
-    const Eigen::VectorXd getProblemState() override;
+    const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
     FullDynamicsSettings getSettings()
     {
diff --git a/include/simple-mpc/kinodynamics.hpp b/include/simple-mpc/kinodynamics.hpp
index 745e6a85..dd25401a 100644
--- a/include/simple-mpc/kinodynamics.hpp
+++ b/include/simple-mpc/kinodynamics.hpp
@@ -57,10 +57,7 @@ namespace simple_mpc
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     // Constructor
-    explicit KinodynamicsOCP(
-      const KinodynamicsSettings & settings,
-      const RobotModelHandler & model_handler,
-      const RobotDataHandler & data_handler);
+    explicit KinodynamicsOCP(const KinodynamicsSettings & settings, const RobotModelHandler & model_handler);
 
     SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(KinodynamicsOCP);
 
@@ -75,7 +72,7 @@ namespace simple_mpc
 
     // Manage terminal cost and constraint
     CostStack createTerminalCost() override;
-    void createTerminalConstraint() override;
+    void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
     void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;
 
     // Getters and setters
@@ -91,7 +88,7 @@ namespace simple_mpc
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
     void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
     void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
-    const Eigen::VectorXd getProblemState() override;
+    const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
 
     void computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs);
diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp
index 6bb66201..ddeb4cc2 100644
--- a/include/simple-mpc/mpc.hpp
+++ b/include/simple-mpc/mpc.hpp
@@ -76,6 +76,8 @@ namespace simple_mpc
     Eigen::Vector3d com0_;
     LocomotionType now_;
 
+    std::shared_ptr<RobotDataHandler> data_handler_;
+
   public:
     std::shared_ptr<SolverProxDDP> solver_;
     Vector6d velocity_base_;
@@ -136,7 +138,7 @@ namespace simple_mpc
     }
     const RobotDataHandler & getDataHandler() const
     {
-      return ocp_handler_->getDataHandler();
+      return *data_handler_;
     }
     const RobotModelHandler & getModelHandler() const
     {
diff --git a/include/simple-mpc/ocp-handler.hpp b/include/simple-mpc/ocp-handler.hpp
index 08fbe30a..ef2f7860 100644
--- a/include/simple-mpc/ocp-handler.hpp
+++ b/include/simple-mpc/ocp-handler.hpp
@@ -47,7 +47,7 @@ namespace simple_mpc
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     /// Constructor
-    explicit OCPHandler(const RobotModelHandler & model_handler, const RobotDataHandler & data_handler);
+    explicit OCPHandler(const RobotModelHandler & model_handler);
     SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(OCPHandler);
     virtual ~OCPHandler();
 
@@ -71,7 +71,7 @@ namespace simple_mpc
 
     virtual void updateTerminalConstraint(const Eigen::Vector3d & com_ref) = 0;
 
-    virtual void createTerminalConstraint() = 0;
+    virtual void createTerminalConstraint(const Eigen::Vector3d & com_ref) = 0;
 
     // Setter and getter for poses reference
     virtual void
@@ -93,7 +93,7 @@ namespace simple_mpc
     virtual void
     setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) = 0;
     virtual const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) = 0;
-    virtual const Eigen::VectorXd getProblemState() = 0;
+    virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0;
     virtual size_t getContactSupport(const std::size_t t) = 0;
 
     /// Common functions for all problems
@@ -135,14 +135,6 @@ namespace simple_mpc
     {
       return model_handler_;
     }
-    const RobotDataHandler & getDataHandler() const
-    {
-      return data_handler_;
-    }
-    RobotDataHandler & getDataHandler()
-    {
-      return data_handler_;
-    }
     int getNu()
     {
       return nu_;
@@ -158,7 +150,6 @@ namespace simple_mpc
     bool terminal_constraint_ = false;
 
     /// The robot model
-    RobotDataHandler data_handler_;
     RobotModelHandler model_handler_;
 
     /// The reference shooting problem storing all shooting nodes
diff --git a/include/simple-mpc/python/py-ocp-handler.hpp b/include/simple-mpc/python/py-ocp-handler.hpp
index 70d01b55..1a04acdb 100644
--- a/include/simple-mpc/python/py-ocp-handler.hpp
+++ b/include/simple-mpc/python/py-ocp-handler.hpp
@@ -89,9 +89,9 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(aligator::CostStackTpl<double>, "createTerminalCost", );
       }
 
-      void createTerminalConstraint() override
+      void createTerminalConstraint(const Eigen::Vector3d & com_ref) override
       {
-        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "createTerminalConstraint", );
+        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "createTerminalConstraint", com_ref);
       }
 
       void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override
@@ -155,9 +155,9 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getPoseBase", t);
       }
 
-      const Eigen::VectorXd getProblemState() override
+      const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override
       {
-        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getProblemState", );
+        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getProblemState", data_handler);
       }
 
       std::size_t getContactSupport(const std::size_t t) override
diff --git a/src/centroidal-dynamics.cpp b/src/centroidal-dynamics.cpp
index 803486ca..011945ae 100644
--- a/src/centroidal-dynamics.cpp
+++ b/src/centroidal-dynamics.cpp
@@ -24,9 +24,8 @@ namespace simple_mpc
   using CentroidalFrictionConeResidual = CentroidalFrictionConeResidualTpl<double>;
   using IntegratorEuler = dynamics::IntegratorEulerTpl<double>;
 
-  CentroidalOCP::CentroidalOCP(
-    const CentroidalSettings & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
-  : Base(model_handler, data_handler)
+  CentroidalOCP::CentroidalOCP(const CentroidalSettings & settings, const RobotModelHandler & model_handler)
+  : Base(model_handler)
   , settings_(settings)
   {
     nx_ = 9;
@@ -258,9 +257,9 @@ namespace simple_mpc
     cfr->setReference(com_ref_);
   }
 
-  const Eigen::VectorXd CentroidalOCP::getProblemState()
+  const Eigen::VectorXd CentroidalOCP::getProblemState(const RobotDataHandler & data_handler)
   {
-    return data_handler_.getCentroidalState();
+    return data_handler.getCentroidalState();
   }
 
   size_t CentroidalOCP::getContactSupport(const std::size_t t)
@@ -291,13 +290,13 @@ namespace simple_mpc
     return term_cost;
   }
 
-  void CentroidalOCP::createTerminalConstraint()
+  void CentroidalOCP::createTerminalConstraint(const Eigen::Vector3d & com_ref)
   {
     if (!problem_initialized_)
     {
       throw std::runtime_error("Create problem first!");
     }
-    CentroidalCoMResidual com_cstr = CentroidalCoMResidual(ndx_, nu_, data_handler_.getData().com[0]);
+    CentroidalCoMResidual com_cstr = CentroidalCoMResidual(ndx_, nu_, com_ref);
 
     // problem_->addTerminalConstraint(com_cstr, EqualityConstraint());
     terminal_constraint_ = false;
diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp
index bfe30fe8..45a41f13 100644
--- a/src/fulldynamics.cpp
+++ b/src/fulldynamics.cpp
@@ -29,11 +29,8 @@ namespace simple_mpc
   using CenterOfMassTranslationResidual = CenterOfMassTranslationResidualTpl<double>;
   using IntegratorSemiImplEuler = dynamics::IntegratorSemiImplEulerTpl<double>;
 
-  FullDynamicsOCP::FullDynamicsOCP(
-    const FullDynamicsSettings & settings,
-    const RobotModelHandler & model_handler,
-    const RobotDataHandler & data_handler)
-  : Base(model_handler, data_handler)
+  FullDynamicsOCP::FullDynamicsOCP(const FullDynamicsSettings & settings, const RobotModelHandler & model_handler)
+  : Base(model_handler)
   , settings_(settings)
   {
 
@@ -57,7 +54,7 @@ namespace simple_mpc
       auto frame_ids = model_handler_.getFootId(name);
       auto joint_ids = model_handler_.getModel().frames[frame_ids].parentJoint;
       pinocchio::SE3 pl1 = model_handler_.getModel().frames[frame_ids].placement;
-      pinocchio::SE3 pl2 = data_handler_.getFootPose(name);
+      pinocchio::SE3 pl2 = pinocchio::SE3::Identity();
       if (settings_.force_size == 6)
       {
         pinocchio::RigidConstraintModel constraint_model = pinocchio::RigidConstraintModel(
@@ -363,9 +360,9 @@ namespace simple_mpc
     qc->setTarget(x0_);
   }
 
-  const Eigen::VectorXd FullDynamicsOCP::getProblemState()
+  const Eigen::VectorXd FullDynamicsOCP::getProblemState(const RobotDataHandler & data_handler)
   {
-    return data_handler_.getState();
+    return data_handler.getState();
   }
 
   size_t FullDynamicsOCP::getContactSupport(const std::size_t t)
@@ -391,18 +388,17 @@ namespace simple_mpc
     return term_cost;
   }
 
-  void FullDynamicsOCP::createTerminalConstraint()
+  void FullDynamicsOCP::createTerminalConstraint(const Eigen::Vector3d & com_ref)
   {
     if (!problem_initialized_)
     {
       throw std::runtime_error("Create problem first!");
     }
     CenterOfMassTranslationResidual com_cstr =
-      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), data_handler_.getData().com[0]);
+      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), com_ref);
 
-    double tau = sqrt(data_handler_.getData().com[0][2] / 9.81);
-    DCMPositionResidual dcm_cstr =
-      DCMPositionResidual(ndx_, nu_, model_handler_.getModel(), data_handler_.getData().com[0], tau);
+    double tau = sqrt(com_ref[2] / 9.81);
+    DCMPositionResidual dcm_cstr = DCMPositionResidual(ndx_, nu_, model_handler_.getModel(), com_ref, tau);
 
     problem_->addTerminalConstraint(dcm_cstr, EqualityConstraint());
 
diff --git a/src/kinodynamics.cpp b/src/kinodynamics.cpp
index edd27447..5577d656 100644
--- a/src/kinodynamics.cpp
+++ b/src/kinodynamics.cpp
@@ -26,11 +26,8 @@ namespace simple_mpc
   using CenterOfMassTranslationResidual = CenterOfMassTranslationResidualTpl<double>;
   using IntegratorSemiImplEuler = dynamics::IntegratorSemiImplEulerTpl<double>;
 
-  KinodynamicsOCP::KinodynamicsOCP(
-    const KinodynamicsSettings & settings,
-    const RobotModelHandler & model_handler,
-    const RobotDataHandler & data_handler)
-  : Base(model_handler, data_handler)
+  KinodynamicsOCP::KinodynamicsOCP(const KinodynamicsSettings & settings, const RobotModelHandler & model_handler)
+  : Base(model_handler)
   , settings_(settings)
   {
 
@@ -304,9 +301,9 @@ namespace simple_mpc
     qc->setTarget(x0_);
   }
 
-  const Eigen::VectorXd KinodynamicsOCP::getProblemState()
+  const Eigen::VectorXd KinodynamicsOCP::getProblemState(const RobotDataHandler & data_handler)
   {
-    return data_handler_.getState();
+    return data_handler.getState();
   }
 
   size_t KinodynamicsOCP::getContactSupport(const std::size_t t)
@@ -338,14 +335,14 @@ namespace simple_mpc
     return term_cost;
   }
 
-  void KinodynamicsOCP::createTerminalConstraint()
+  void KinodynamicsOCP::createTerminalConstraint(const Eigen::Vector3d & com_ref)
   {
     if (!problem_initialized_)
     {
       throw std::runtime_error("Create problem first!");
     }
     CenterOfMassTranslationResidual com_cstr =
-      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), data_handler_.getData().com[0]);
+      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), com_ref);
 
     problem_->addTerminalConstraint(com_cstr, EqualityConstraint());
     terminal_constraint_ = true;
diff --git a/src/mpc.cpp b/src/mpc.cpp
index b726d96b..8762a75b 100644
--- a/src/mpc.cpp
+++ b/src/mpc.cpp
@@ -29,20 +29,21 @@ namespace simple_mpc
   {
     settings_ = settings;
     ocp_handler_ = problem;
+    data_handler_ = std::make_shared<RobotDataHandler>(ocp_handler_->getModelHandler());
+    data_handler_->updateInternalData(ocp_handler_->getModelHandler().getReferenceState(), true);
     std::map<std::string, Eigen::Vector3d> starting_poses;
     for (auto const & name : ocp_handler_->getModelHandler().getFeetNames())
     {
-      starting_poses.insert({name, ocp_handler_->getDataHandler().getFootPose(name).translation()});
+      starting_poses.insert({name, data_handler_->getFootPose(name).translation()});
 
       relative_feet_poses_.insert(
-        {name, ocp_handler_->getDataHandler().getBaseFramePose().inverse()
-                 * ocp_handler_->getDataHandler().getFootPose(name)});
+        {name, data_handler_->getBaseFramePose().inverse() * data_handler_->getFootPose(name)});
     }
     foot_trajectories_ = FootTrajectory(
       starting_poses, settings_.swing_apex, settings_.T_fly, settings_.T_contact, ocp_handler_->getSize());
 
     foot_trajectories_.updateApex(settings.swing_apex);
-    x0_ = ocp_handler_->getProblemState();
+    x0_ = ocp_handler_->getProblemState(*data_handler_);
 
     solver_ = std::make_shared<SolverProxDDP>(settings_.TOL, settings_.mu_init, maxiters, aligator::QUIET);
     solver_->rollout_type_ = aligator::RolloutType::LINEAR;
@@ -69,7 +70,7 @@ namespace simple_mpc
     {
       contact_states.insert({name, true});
       land_constraint.insert({name, false});
-      contact_poses.insert({name, ocp_handler_->getDataHandler().getFootPose(name)});
+      contact_poses.insert({name, data_handler_->getFootPose(name)});
       force_map.insert({name, force_ref});
     }
 
@@ -94,7 +95,7 @@ namespace simple_mpc
 
     solver_->max_iters = settings_.max_iters;
 
-    com0_ = ocp_handler_->getDataHandler().getData().com[0];
+    com0_ = data_handler_->getData().com[0];
     now_ = WALKING;
     pose_base_ = x0_.head<7>();
     velocity_base_.setZero();
@@ -161,7 +162,7 @@ namespace simple_mpc
 
       for (auto const & name : ee_names_)
       {
-        contact_poses.insert({name, ocp_handler_->getDataHandler().getFootPose(name)});
+        contact_poses.insert({name, data_handler_->getFootPose(name)});
         if (state.at(name))
           force_map.insert({name, force_ref});
         else
@@ -191,7 +192,7 @@ namespace simple_mpc
   void MPC::iterate(const Eigen::VectorXd & x)
   {
 
-    ocp_handler_->getDataHandler().updateInternalData(x, false);
+    data_handler_->updateInternalData(x, false);
 
     // Recede all horizons
     recedeWithCycle();
@@ -200,7 +201,7 @@ namespace simple_mpc
     updateStepTrackerReferences();
 
     // Recede previous solutions
-    x0_ << ocp_handler_->getProblemState();
+    x0_ << ocp_handler_->getProblemState(*data_handler_);
     xs_.erase(xs_.begin());
     xs_[0] = x0_;
     xs_.push_back(xs_.back());
@@ -291,17 +292,16 @@ namespace simple_mpc
 
       // Use the Raibert heuristics to compute the next foot pose
       twist_vect_[0] =
-        -(ocp_handler_->getDataHandler().getRefFootPose(name).translation()[1]
-          - ocp_handler_->getDataHandler().getBaseFramePose().translation()[1]);
-      twist_vect_[1] = ocp_handler_->getDataHandler().getRefFootPose(name).translation()[0]
-                       - ocp_handler_->getDataHandler().getBaseFramePose().translation()[0];
-      next_pose_.head(2) = ocp_handler_->getDataHandler().getRefFootPose(name).translation().head(2);
+        -(data_handler_->getRefFootPose(name).translation()[1] - data_handler_->getBaseFramePose().translation()[1]);
+      twist_vect_[1] =
+        data_handler_->getRefFootPose(name).translation()[0] - data_handler_->getBaseFramePose().translation()[0];
+      next_pose_.head(2) = data_handler_->getRefFootPose(name).translation().head(2);
       next_pose_.head(2) += (velocity_base_.head(2) + velocity_base_[5] * twist_vect_)
                             * (settings_.T_fly + settings_.T_contact) * settings_.timestep;
-      next_pose_[2] = ocp_handler_->getDataHandler().getFootPose(name).translation()[2];
+      next_pose_[2] = data_handler_->getFootPose(name).translation()[2];
 
       foot_trajectories_.updateTrajectory(
-        update, foot_land_time, ocp_handler_->getDataHandler().getFootPose(name).translation(), next_pose_, name);
+        update, foot_land_time, data_handler_->getFootPose(name).translation(), next_pose_, name);
       pinocchio::SE3 pose = pinocchio::SE3::Identity();
       for (unsigned long time = 0; time < ocp_handler_->getSize(); time++)
       {
diff --git a/src/ocp-handler.cpp b/src/ocp-handler.cpp
index 879d3fef..f767f9b1 100644
--- a/src/ocp-handler.cpp
+++ b/src/ocp-handler.cpp
@@ -8,9 +8,8 @@ namespace simple_mpc
   {
   }
 
-  OCPHandler::OCPHandler(const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
-  : data_handler_(data_handler)
-  , model_handler_(model_handler)
+  OCPHandler::OCPHandler(const RobotModelHandler & model_handler)
+  : model_handler_(model_handler)
   , problem_(nullptr)
   {
     nq_ = model_handler.getModel().nq;
@@ -115,7 +114,7 @@ namespace simple_mpc
     for (auto & name : model_handler_.getFeetNames())
     {
       contact_phase.insert({name, true});
-      contact_pose.insert({name, data_handler_.getFootPose(name)});
+      contact_pose.insert({name, pinocchio::SE3::Identity()});
       contact_force.insert({name, force_ref});
     }
 
@@ -133,7 +132,7 @@ namespace simple_mpc
 
     if (terminal_constraint)
     {
-      createTerminalConstraint();
+      createTerminalConstraint(x0.head(3));
     }
   }
 } // namespace simple_mpc
diff --git a/tests/mpc.cpp b/tests/mpc.cpp
index 155159a8..8f88c422 100644
--- a/tests/mpc.cpp
+++ b/tests/mpc.cpp
@@ -18,7 +18,7 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics)
   RobotDataHandler data_handler(model_handler);
 
   FullDynamicsSettings settings = getFullDynamicsSettings(model_handler);
-  auto problem = std::make_shared<FullDynamicsOCP>(settings, model_handler, data_handler);
+  auto problem = std::make_shared<FullDynamicsOCP>(settings, model_handler);
   FullDynamicsOCP & fdproblem = *problem;
 
   const size_t T = 100;
@@ -96,7 +96,7 @@ BOOST_AUTO_TEST_CASE(mpc_kinodynamics)
   RobotDataHandler data_handler(model_handler);
 
   KinodynamicsSettings settings = getKinodynamicsSettings(model_handler);
-  auto problem = std::make_shared<KinodynamicsOCP>(settings, model_handler, data_handler);
+  auto problem = std::make_shared<KinodynamicsOCP>(settings, model_handler);
   KinodynamicsOCP & kinoproblem = *problem;
   const std::size_t T = 100;
   const double support_force = -model_handler.getMass() * settings.gravity[2];
@@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(mpc_centroidal)
   RobotDataHandler data_handler(model_handler);
 
   CentroidalSettings settings = getCentroidalSettings();
-  auto problem = std::make_shared<CentroidalOCP>(settings, model_handler, data_handler);
+  auto problem = std::make_shared<CentroidalOCP>(settings, model_handler);
   CentroidalOCP & centproblem = *problem;
 
   std::vector<std::string> contact_names = {"left_sole_link", "right_sole_link"};
diff --git a/tests/problem.cpp b/tests/problem.cpp
index e934e016..e98c2d11 100644
--- a/tests/problem.cpp
+++ b/tests/problem.cpp
@@ -41,7 +41,7 @@ BOOST_AUTO_TEST_CASE(fulldynamics)
   force_refs.insert({"right_sole_link", Eigen::VectorXd::Zero(6)});
 
   FullDynamicsSettings settings = getFullDynamicsSettings(model_handler);
-  FullDynamicsOCP fdproblem(settings, model_handler, data_handler);
+  FullDynamicsOCP fdproblem(settings, model_handler);
   StageModel sm = fdproblem.createStage(contact_states, contact_poses, force_refs, land_constraint);
   CostStack * cs = dynamic_cast<CostStack *>(&*sm.cost_);
 
@@ -117,7 +117,7 @@ BOOST_AUTO_TEST_CASE(kinodynamics)
   contact_poses.insert({contact_names[1], p2});
 
   KinodynamicsSettings settings = getKinodynamicsSettings(model_handler);
-  KinodynamicsOCP knproblem(settings, model_handler, data_handler);
+  KinodynamicsOCP knproblem(settings, model_handler);
 
   std::map<std::string, Eigen::VectorXd> force_refs;
   Eigen::VectorXd f1(6);
@@ -183,7 +183,7 @@ BOOST_AUTO_TEST_CASE(centroidal)
   RobotDataHandler data_handler(model_handler);
 
   CentroidalSettings settings = getCentroidalSettings();
-  CentroidalOCP cproblem(settings, model_handler, data_handler);
+  CentroidalOCP cproblem(settings, model_handler);
 
   std::vector<std::string> contact_names = {"left_sole_link", "right_sole_link"};
   std::map<std::string, bool> contact_states;
@@ -268,7 +268,7 @@ BOOST_AUTO_TEST_CASE(centroidal_solo)
   CentroidalSettings settings = getCentroidalSettings();
   settings.force_size = 3;
 
-  CentroidalOCP cproblem(settings, model_handler, data_handler);
+  CentroidalOCP cproblem(settings, model_handler);
 
   std::vector<std::string> contact_names = {"FR_FOOT", "FL_FOOT", "HR_FOOT", "HL_FOOT"};
   std::map<std::string, bool> contact_states;