Merge pull request #24 from Simple-Robotics/topic/data_mpc

ManifoldFR · web-flow · commit 0e3406628b59 · 2024-12-18T16:07:48.000+01:00
Changing ownership if the RobotData Object
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
diff --git a/src/kinodynamics.cpp b/src/kinodynamics.cpp
diff --git a/src/mpc.cpp b/src/mpc.cpp
diff --git a/src/ocp-handler.cpp b/src/ocp-handler.cpp
diff --git a/tests/mpc.cpp b/tests/mpc.cpp
diff --git a/tests/problem.cpp b/tests/problem.cpp

Original file line number	Diff line number	Diff line change
`@@ -6,8 +6,7 @@`
`6`	`6`	`namespace simple_mpc::python`
`7`	`7`	`{`
`8`	`8`
`9`		`- auto * createCentroidal(`
`10`		`- const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)`
	`9`	`+ auto * createCentroidal(const bp::dict & settings, const RobotModelHandler & model_handler)`
`11`	`10`	`{`
`12`	`11`	`CentroidalSettings conf;`
`13`	`12`	`conf.timestep = bp::extract<double>(settings["timestep"]);`
`@@ -25,7 +24,7 @@ namespace simple_mpc::python`
`25`	`24`	`conf.Lfoot = bp::extract<double>(settings["Lfoot"]);`
`26`	`25`	`conf.Wfoot = bp::extract<double>(settings["Wfoot"]);`
`27`	`26`
`28`		`- return new CentroidalOCP(conf, model_handler, data_handler);`
	`27`	`+ return new CentroidalOCP(conf, model_handler);`
`29`	`28`	`}`
`30`	`29`
`31`	`30`	`StageModel createCentStage(`
`@@ -113,8 +112,8 @@ namespace simple_mpc::python`
`113`	`112`
`114`	`113`	`bp::class_<CentroidalOCP, bp::bases<OCPHandler>, boost::noncopyable>("CentroidalOCP", bp::no_init)`
`115`	`114`	`.def(`
`116`		`- "__init__", bp::make_constructor(`
`117`		`- &createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))`
	`115`	`+ "__init__",`
	`116`	`+ bp::make_constructor(&createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler")))`
`118`	`117`	`.def("getSettings", &getSettingsCent)`
`119`	`118`	`.def("createStage", &createCentStage);`
`120`	`119`	`}`
Original file line number	Diff line number	Diff line change
`@@ -7,8 +7,7 @@`
`7`	`7`	`namespace simple_mpc::python`
`8`	`8`	`{`
`9`	`9`
`10`		`- auto * createFulldynamics(`
`11`		`- const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)`
	`10`	`+ auto * createFulldynamics(const bp::dict & settings, const RobotModelHandler & model_handler)`
`12`	`11`	`{`
`13`	`12`	`FullDynamicsSettings conf;`
`14`	`13`	`conf.timestep = bp::extract<double>(settings["timestep"]);`
`@@ -41,7 +40,7 @@ namespace simple_mpc::python`
`41`	`40`	`conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);`
`42`	`41`	`conf.force_cone = bp::extract<bool>(settings["force_cone"]);`
`43`	`42`
`44`		`- return new FullDynamicsOCP(conf, model_handler, data_handler);`
	`43`	`+ return new FullDynamicsOCP(conf, model_handler);`
`45`	`44`	`}`
`46`	`45`
`47`	`46`	`StageModel createFullStage(`
`@@ -138,8 +137,7 @@ namespace simple_mpc::python`
`138`	`137`	`bp::class_<FullDynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("FullDynamicsOCP", bp::no_init)`
`139`	`138`	`.def(`
`140`	`139`	`"__init__",`
`141`		`- bp::make_constructor(`
`142`		`- &createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))`
	`140`	`+ bp::make_constructor(&createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))`
`143`	`141`	`.def("getSettings", &getSettingsFull)`
`144`	`142`	`.def("createStage", &createFullStage);`
`145`	`143`	`}`
Original file line number	Diff line number	Diff line change
`@@ -6,8 +6,7 @@`
`6`	`6`	`namespace simple_mpc::python`
`7`	`7`	`{`
`8`	`8`
`9`		`- auto * createKinodynamics(`
`10`		`- const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)`
	`9`	`+ auto * createKinodynamics(const bp::dict & settings, const RobotModelHandler & model_handler)`
`11`	`10`	`{`
`12`	`11`	`KinodynamicsSettings conf;`
`13`	`12`	`conf.timestep = bp::extract<double>(settings["timestep"]);`
`@@ -30,7 +29,7 @@ namespace simple_mpc::python`
`30`	`29`	`conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);`
`31`	`30`	`conf.force_cone = bp::extract<bool>(settings["force_cone"]);`
`32`	`31`
`33`		`- return new KinodynamicsOCP(conf, model_handler, data_handler);`
	`32`	`+ return new KinodynamicsOCP(conf, model_handler);`
`34`	`33`	`}`
`35`	`34`
`36`	`35`	`bp::dict getSettingsKino(KinodynamicsOCP & self)`
`@@ -122,8 +121,7 @@ namespace simple_mpc::python`
`122`	`121`	`bp::class_<KinodynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("KinodynamicsOCP", bp::no_init)`
`123`	`122`	`.def(`
`124`	`123`	`"__init__",`
`125`		`- bp::make_constructor(`
`126`		`- &createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))`
	`124`	`+ bp::make_constructor(&createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))`
`127`	`125`	`.def("getSettings", &getSettingsKino)`
`128`	`126`	`.def("createStage", &createKinoStage);`
`129`	`127`	`}`
Original file line number	Diff line number	Diff line change
`@@ -64,7 +64,7 @@`
`64`	`64`	`)`
`65`	`65`	`T = 50`
`66`	`66`
`67`		`-dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)`
	`67`	`+dynproblem = FullDynamicsOCP(problem_conf, model_handler)`
`68`	`68`	`dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)`
`69`	`69`
`70`	`70`	`T_ds = 10`
Original file line number	Diff line number	Diff line change
`@@ -74,7 +74,7 @@`
`74`	`74`	`)`
`75`	`75`	`T = 50`
`76`	`76`
`77`		`-dynproblem = KinodynamicsOCP(problem_conf, model_handler, data_handler)`
	`77`	`+dynproblem = KinodynamicsOCP(problem_conf, model_handler)`
`78`	`78`	`dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)`
`79`	`79`
`80`	`80`	`T_ds = 10`
Original file line number	Diff line number	Diff line change
`@@ -68,7 +68,7 @@`
`68`	`68`	`)`
`69`	`69`	`T = 100`
`70`	`70`
`71`		`-problem = CentroidalOCP(problem_conf, model_handler, data_handler)`
	`71`	`+problem = CentroidalOCP(problem_conf, model_handler)`
`72`	`72`	`problem.createProblem(data_handler.getCentroidalState(), T, 6, gravity[2], False)`
`73`	`73`
`74`	`74`	`T_ds = 20`
Original file line number	Diff line number	Diff line change
`@@ -92,7 +92,7 @@`
`92`	`92`	`)`
`93`	`93`
`94`	`94`	`T = 100`
`95`		`-dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)`
	`95`	`+dynproblem = FullDynamicsOCP(problem_conf, model_handler)`
`96`	`96`	`dynproblem.createProblem(x0, T, 6, gravity[2], False)`
`97`	`97`
`98`	`98`	`""" Define feet trajectory """`