placo 0.6.5__tar.gz → 0.7.0__tar.gz

{placo-0.6.5 → placo-0.7.0}/CMakeLists.txt

@@ -127,7 +127,7 @@ target_link_libraries(libplaco PUBLIC
     ${PROTOBUF_LIBRARIES}
   ${PROTOBUF_LIBRARIES}
 )
-target_compile_definitions(libplaco PUBLIC -DPINOCCHIO_WITH_HPP_FCL)
+target_compile_definitions(libplaco PUBLIC)
 
 if(TARGET rhoban_utils)
   message("Placo: Rhoban utils is present, enabling it")
@@ -151,7 +151,7 @@ add_library(placo MODULE
     bindings/expose-walk-pattern-generator.cpp
     bindings/expose-dynamics.cpp
     bindings/module.cpp
-    bindings/registry.cpp
+    bindings/doxystub/registry.cpp
 )
 set_target_properties(placo PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB})
 
@@ -159,8 +159,8 @@ target_link_libraries(placo ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} libplaco)
 target_include_directories(placo PRIVATE ${PYTHON_INCLUDE_DIRS})
 
 add_custom_command(TARGET placo POST_BUILD
-    COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/stubs.py > placo.pyi
-    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}
+    COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/bindings/doxystub/stubs.sh "${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}" >  "${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}/placo.pyi"
+    WORKING DIRECTORY "/"
     COMMENT "Generating stubs..."
 )
 
@@ -175,4 +175,4 @@ set_target_properties(libplaco PROPERTIES INSTALL_RPATH "\$ORIGIN")
 install(TARGETS libplaco DESTINATION lib)
 install(TARGETS placo DESTINATION ${PYTHON_SITELIB})
 install(FILES ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}/placo.pyi DESTINATION ${PYTHON_SITELIB})
-install(DIRECTORY ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}/placo_utils DESTINATION ${PYTHON_SITELIB})
+install(DIRECTORY ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB}/placo_utils DESTINATION ${PYTHON_SITELIB})

{placo-0.6.5 → placo-0.7.0}/PKG-INFO

@@ -1,8 +1,8 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.6.5
+Version: 0.7.0
 Summary: PlaCo: Rhoban Planning and Control
-Requires-Python: >= 3.8
+Requires-Python: >= 3.9
 License-Expression: MIT
 Author-email: Rhoban team <team@rhoban.com>
 Project-URL: Changelog, https://github.com/rhoban/placo/blob/main/CHANGELOG.md
@@ -10,14 +10,12 @@ Home-page: https://placo.readthedocs.io/en/latest/
 Project-URL: Repository, https://github.com/rhoban/placo.git
 Requires-Dist: cmeel
 Requires-Dist: eiquadprog >= 1.2.6, < 2
-Requires-Dist: pin >= 2.6.18, < 3
+Requires-Dist: pin >= 3
 Requires-Dist: rhoban-cmeel-jsoncpp
 Requires-Dist: meshcat
-Requires-Dist: numpy<2
 Requires-Dist: ischedule
 Provides-Extra: build
-Requires-Dist: pin[build] >= 2.6.18, < 3 ; extra == "build"
-Requires-Dist: cmeel-urdfdom[build] ; extra == "build"
+Requires-Dist: pin[build] >=  3 ; extra == "build"
 Description-Content-Type: text/markdown
 
 <img width="400" src="https://placo.readthedocs.io/en/latest/_static/placo.png" />

placo-0.7.0/bindings/doxystub/README.md

@@ -0,0 +1,15 @@
+# Generate Doxygen stubs 
+
+This code allow to generate a `module.pyi` file containing useful auto-completion stubs, based on Doxygen and introspection.
+
+## Step1: configure your project
+
+In `config.py`, set the name of the module and the path to the source directory (where the `Doxyfile` is)
+
+## Step2: expose through registry
+
+Include `registry.h` in your Python bindings, and use `class__` instead of `class_`, so that the registered classes will be wrapped in the registry
+
+## Step3: call `stubs.py` after compilation
+
+Execute the `stubs.py` script after compilation, it will output the `pyi` file.

placo-0.7.0/bindings/doxystub/config.py

@@ -0,0 +1,5 @@
+# Module name to document
+module_name = "placo"
+
+# Path for the doxygen generation, relative to this script
+doxygen_path = "../../"

{placo-0.6.5 → placo-0.7.0/bindings/doxystub}/stubs.py

@@ -1,27 +1,28 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 import re
 import inspect
 import sys
 import os
 import argparse
+from config import module_name, doxygen_path
 
 # Current script directory:
-repo_directory = os.path.dirname(os.path.realpath(__file__))
+repo_directory = os.path.dirname(os.path.realpath(__file__ + "/" + doxygen_path))
 
-# If placo.pyi file already exists next to stubs.py, we read it directly. This is a way to
-# avoid running Doxygen when building placo sdist release.
-if os.path.exists(f"{repo_directory}/placo.pyi"):
-    with open(f"{repo_directory}/placo.pyi", "r") as f:
+# If .pyi file already exists next to stubs.py, we read it directly. This is a way to
+# avoid running Doxygen when building sdist release.
+if os.path.exists(f"{repo_directory}/{module_name}.pyi"):
+    with open(f"{repo_directory}/{module_name}.pyi", "r") as f:
         print(f.read())
     exit(0)
 
 # Prepending current directory to PYTHONPATH
 sys.path = ["."] + sys.path
 
-import placo
+exec(f"import {module_name}")
 from doxygen_parse import parse_directory, get_members, get_metadata
 
-module: str = "placo"
+module = eval(f"{module_name}")
 
 # Ensure Doxygen is run
 if not os.path.exists(f"/usr/bin/doxygen"):
@@ -53,7 +54,7 @@ rewrite_types: dict = {
 }
 
 # Building registry and reverse registry for class names
-cxx_registry = placo.get_classes_registry()
+cxx_registry = module.get_classes_registry()
 py_registry = {"root": "root"}
 for entry in cxx_registry:
     rewrite_types[entry] = cxx_registry[entry]
@@ -219,18 +220,18 @@ def print_class_method(class_name: str, method_name: str, doc: str, prefix: str
     else:
         print_def(method_name, doc, prefix)
 
-
+print("# Doxygen stubs generation")
 print("import numpy")
 print("import typing")
 
-for name, object in inspect.getmembers(placo):
+for name, object in inspect.getmembers(module):
     if isinstance(object, type):
         class_name = object.__name__
         print(f'{class_name} = typing.NewType("{class_name}", None)')
 
 groups = {}
 
-for name, object in inspect.getmembers(placo):
+for name, object in inspect.getmembers(module):
     if isinstance(object, type):
         class_name = object.__name__
         print(f"class {class_name}:")

placo-0.7.0/bindings/doxystub/stubs.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+export PYTHONPATH="$1:$PYTHONPATH"
+$SCRIPT_DIR/stubs.py

{placo-0.6.5 → placo-0.7.0}/bindings/expose-dynamics.cpp

@@ -2,7 +2,8 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+
+#include "doxystub/registry.h"
 #include "placo/model/robot_wrapper.h"
 #include "placo/dynamics/dynamics_solver.h"
 #include <Eigen/Dense>

{placo-0.6.5 → placo-0.7.0}/bindings/expose-footsteps.cpp

@@ -2,7 +2,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/humanoid/footsteps_planner.h"
 #include "placo/humanoid/footsteps_planner_naive.h"
 #include "placo/humanoid/footsteps_planner_repetitive.h"
@@ -41,9 +41,10 @@ void exposeFootsteps()
           "set_start", +[](FootstepsPlanner::Support& support, bool b) { support.start = b; })
       .def(
           "set_end", +[](FootstepsPlanner::Support& support, bool b) { support.end = b; })
-      .add_property("footsteps", &FootstepsPlanner::Support::footsteps)
+      .add_property("footsteps", &FootstepsPlanner::Support::footsteps, &FootstepsPlanner::Support::footsteps)
       .add_property("t_start", &FootstepsPlanner::Support::t_start, &FootstepsPlanner::Support::t_start)
-      .add_property("elapsed_ratio", &FootstepsPlanner::Support::elapsed_ratio, &FootstepsPlanner::Support::elapsed_ratio)
+      .add_property("elapsed_ratio", &FootstepsPlanner::Support::elapsed_ratio,
+                    &FootstepsPlanner::Support::elapsed_ratio)
       .add_property("time_ratio", &FootstepsPlanner::Support::time_ratio, &FootstepsPlanner::Support::time_ratio)
       .add_property("start", &FootstepsPlanner::Support::start, &FootstepsPlanner::Support::start)
       .add_property("end", &FootstepsPlanner::Support::end, &FootstepsPlanner::Support::end)

{placo-0.6.5 → placo-0.7.0}/bindings/expose-kinematics.cpp

@@ -2,7 +2,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/kinematics/kinematics_solver.h"
 #include <boost/python/return_internal_reference.hpp>
 #include <Eigen/Dense>

{placo-0.6.5 → placo-0.7.0}/bindings/expose-parameters.cpp

@@ -2,7 +2,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/model/robot_wrapper.h"
 #include "placo/humanoid/humanoid_robot.h"
 #include "placo/humanoid/humanoid_parameters.h"
@@ -18,10 +18,14 @@ using namespace placo::humanoid;
 void exposeParameters()
 {
   class__<HumanoidParameters>("HumanoidParameters")
-      .add_property("single_support_duration", &HumanoidParameters::single_support_duration, &HumanoidParameters::single_support_duration)
-      .add_property("single_support_timesteps", &HumanoidParameters::single_support_timesteps, &HumanoidParameters::single_support_timesteps)
-      .add_property("double_support_ratio", &HumanoidParameters::double_support_ratio, &HumanoidParameters::double_support_ratio)
-      .add_property("startend_double_support_ratio", &HumanoidParameters::startend_double_support_ratio, &HumanoidParameters::startend_double_support_ratio)
+      .add_property("single_support_duration", &HumanoidParameters::single_support_duration,
+                    &HumanoidParameters::single_support_duration)
+      .add_property("single_support_timesteps", &HumanoidParameters::single_support_timesteps,
+                    &HumanoidParameters::single_support_timesteps)
+      .add_property("double_support_ratio", &HumanoidParameters::double_support_ratio,
+                    &HumanoidParameters::double_support_ratio)
+      .add_property("startend_double_support_ratio", &HumanoidParameters::startend_double_support_ratio,
+                    &HumanoidParameters::startend_double_support_ratio)
       .add_property("planned_timesteps", &HumanoidParameters::planned_timesteps, &HumanoidParameters::planned_timesteps)
       .add_property("replan_timesteps", &HumanoidParameters::replan_timesteps, &HumanoidParameters::replan_timesteps)
       .add_property("zmp_margin", &HumanoidParameters::zmp_margin, &HumanoidParameters::zmp_margin)
@@ -45,6 +49,8 @@ void exposeParameters()
       .add_property("walk_max_dtheta", &HumanoidParameters::walk_max_dtheta, &HumanoidParameters::walk_max_dtheta)
       .add_property("walk_dtheta_spacing", &HumanoidParameters::walk_dtheta_spacing,
                     &HumanoidParameters::walk_dtheta_spacing)
+      .add_property("op_space_polygon", &HumanoidParameters::op_space_polygon, &HumanoidParameters::op_space_polygon)
+      .add_property("dcm_offset_polygon", &HumanoidParameters::dcm_offset_polygon, &HumanoidParameters::dcm_offset_polygon)
       .def("dt", &HumanoidParameters::dt)
       .def("double_support_timesteps", &HumanoidParameters::double_support_timesteps)
       .def("startend_double_support_timesteps", &HumanoidParameters::startend_double_support_timesteps)

{placo-0.6.5 → placo-0.7.0}/bindings/expose-problem.cpp

@@ -5,7 +5,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/problem/problem.h"
 #include "placo/problem/variable.h"
 #include "placo/problem/expression.h"

{placo-0.6.5 → placo-0.7.0}/bindings/expose-robot-wrapper.cpp

@@ -2,7 +2,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/model/robot_wrapper.h"
 #include "placo/humanoid/humanoid_robot.h"
 #include "placo/kinematics/kinematics_solver.h"

{placo-0.6.5 → placo-0.7.0}/bindings/expose-tools.cpp

@@ -3,7 +3,7 @@
 #include <Eigen/Dense>
 #include <boost/python.hpp>
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/tools/utils.h"
 #include "placo/tools/cubic_spline.h"
 #include "placo/tools/cubic_spline_3d.h"
@@ -89,10 +89,14 @@ void exposeTools()
   class__<Segment>("Segment", init<Eigen::Vector2d, Eigen::Vector2d>())
       .add_property("start", &Segment::start, &Segment::start)
       .add_property("end", &Segment::end, &Segment::start)
-      .def("is_parallel", +[](Segment& s1, const Segment& s2) { return s1.is_parallel(s2); })
-      .def("is_point_aligned", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_aligned(point); })
-      .def("is_collinear", +[](Segment& s1, const Segment& s2) { return s1.is_collinear(s2); })
-      .def("is_point_in_segment", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_in_segment(point); })
+      .def(
+          "is_parallel", +[](Segment& s1, const Segment& s2) { return s1.is_parallel(s2); })
+      .def(
+          "is_point_aligned", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_aligned(point); })
+      .def(
+          "is_collinear", +[](Segment& s1, const Segment& s2) { return s1.is_collinear(s2); })
+      .def(
+          "is_point_in_segment", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_in_segment(point); })
       .def("intersects", &Segment::intersects)
       .def("line_pass_through", &Segment::line_pass_through)
       .def("half_line_pass_through", &Segment::half_line_pass_through)
@@ -104,6 +108,7 @@ void exposeTools()
   // History collection
   class__<HistoryCollection>("HistoryCollection")
       .def("loadReplays", &HistoryCollection::loadReplays, loadReplays_overloads())
+      .def("exportToCSV", &HistoryCollection::exportToCSV)
       .def("smallestTimestamp", &HistoryCollection::smallestTimestamp)
       .def("biggestTimestamp", &HistoryCollection::biggestTimestamp)
       .def("startNamedLog", &HistoryCollection::startNamedLog)

{placo-0.6.5 → placo-0.7.0}/bindings/expose-utils.hpp

@@ -40,6 +40,15 @@ struct custom_vector_from_seq
   }
 };
 
+template <typename T>
+bool is_registered()
+{
+  boost::python::type_info info = boost::python::type_id<T>();
+  const boost::python::converter::registration* reg = boost::python::converter::registry::query(info);
+
+  return reg == NULL || (*reg).m_to_python == NULL;
+}
+
 /**
  * @brief Exposes a given type for std::vector
  * @tparam T type
@@ -50,9 +59,12 @@ void exposeStdVector(const std::string& class_name)
 {
   typedef typename std::vector<T> vector_T;
 
-  class_<vector_T>(class_name.c_str()).def(vector_indexing_suite<vector_T>());
+  if (!is_registered<T>())
+  {
+    class_<vector_T>(class_name.c_str()).def(vector_indexing_suite<vector_T>());
 
-  custom_vector_from_seq<T>();
+    custom_vector_from_seq<T>();
+  }
 }
 
 template <typename K, typename V>

{placo-0.6.5 → placo-0.7.0}/bindings/expose-walk-pattern-generator.cpp

@@ -2,7 +2,7 @@
 
 #include "expose-utils.hpp"
 #include "module.h"
-#include "registry.h"
+#include "doxystub/registry.h"
 #include "placo/humanoid/walk_pattern_generator.h"
 #include "placo/kinematics/kinematics_solver.h"
 #include "placo/humanoid/footsteps_planner.h"
@@ -51,7 +51,9 @@ void exposeWalkPatternGenerator()
       .def("get_support", &WalkPatternGenerator::Trajectory::get_support)
       .def("get_next_support", &WalkPatternGenerator::Trajectory::get_next_support)
       .def("get_prev_support", &WalkPatternGenerator::Trajectory::get_prev_support)
+      .def("get_part_t_end", &WalkPatternGenerator::Trajectory::get_part_t_end)
       .def("get_part_t_start", &WalkPatternGenerator::Trajectory::get_part_t_start)
+      .def("get_part_end_dcm", &WalkPatternGenerator::Trajectory::get_part_end_dcm)
       .def("apply_transform", &WalkPatternGenerator::Trajectory::apply_transform)
       .def("print_parts_timings", &WalkPatternGenerator::Trajectory::print_parts_timings);
 
@@ -60,7 +62,8 @@ void exposeWalkPatternGenerator()
       .def("replan", &WalkPatternGenerator::replan)
       .def("can_replan_supports", &WalkPatternGenerator::can_replan_supports)
       .def("replan_supports", &WalkPatternGenerator::replan_supports)
-      .def("compute_next_support", &WalkPatternGenerator::compute_next_support);
+      .def("update_supports", &WalkPatternGenerator::update_supports)
+      .def("get_optimal_zmp", &WalkPatternGenerator::get_optimal_zmp);
 
   class__<SwingFoot>("SwingFoot", init<>())
       .def("make_trajectory", &SwingFoot::make_trajectory)
@@ -137,7 +140,8 @@ void exposeWalkPatternGenerator()
       .def("dzmp", &LIPM::Trajectory::dzmp)
       .def("dcm", &LIPM::Trajectory::dcm);
 
-  class__<LIPM>("LIPM", init<problem::Problem&, double, int, double, Eigen::Vector2d, Eigen::Vector2d, Eigen::Vector2d>())
+  class__<LIPM>("LIPM",
+                init<problem::Problem&, double, int, double, Eigen::Vector2d, Eigen::Vector2d, Eigen::Vector2d>())
       .def("compute_omega", &LIPM::compute_omega)
       .def("get_trajectory", &LIPM::get_trajectory)
       .def("pos", &LIPM::pos)

placo-0.7.0/build_wheel.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+export PYTHONPATH="."
+export PATH=$(getconf PATH)
+export PKG_CONFIG_PATH=$(getconf PKG_CONFIG_PATH)
+export LD_LIBRARY_PATH=$(getconf LD_LIBRARY_PATH)
+export PYTHONPATH=$(getconf PYTHONPATH)
+export CMAKE_PREFIX_PATH=$(getconf CMAKE_PREFIX_PATH)
+
+make
+

{placo-0.6.5 → placo-0.7.0}/pyproject.toml

@@ -3,9 +3,8 @@ build-backend = "cmeel"
 requires = [
   "cmeel[build]",
   "eiquadprog >= 1.2.6, < 2",
-  "pin[build] >= 2.6.18, < 3",
-  "rhoban-cmeel-jsoncpp",
-  "cmeel-urdfdom[build]",
+  "pin[build] >=  3",
+  "rhoban-cmeel-jsoncpp"
 ]
 
 [project]
@@ -13,17 +12,16 @@ authors = [{ email = "team@rhoban.com", name = "Rhoban team" }]
 classifiers = []
 dependencies = [
   "eiquadprog >= 1.2.6, < 2",
-  "pin >= 2.6.18, < 3",
+  "pin >= 3",
   "rhoban-cmeel-jsoncpp",
   "meshcat",
-  "numpy<2",
   "ischedule"
 ]
 description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
-requires-python = ">= 3.8"
-version = "0.6.5"
+requires-python = ">= 3.9"
+version = "0.7.0"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"
@@ -34,7 +32,7 @@ repository = "https://github.com/rhoban/placo.git"
 profile = "black"
 
 [tool.ruff]
-target-version = "py38"
+target-version = "py39"
 
 [tool.tomlsort]
 all = true

{placo-0.6.5 → placo-0.7.0}/python/placo_utils/visualization.py

@@ -1,5 +1,6 @@
 import meshcat
 import pinocchio as pin
+import pinocchio.visualize as pin_viz
 import numpy as np
 import meshcat.geometry as g
 import meshcat.transformations as tf
@@ -26,7 +27,7 @@ def get_viewer() -> meshcat.Visualizer:
 
 def robot_viz(
     robot: placo.RobotWrapper, name: str = "robot"
-) -> pin.visualize.MeshcatVisualizer:
+) -> pin_viz.MeshcatVisualizer:
     """
     Builds an instance of pinocchio MeshcatVisualizer, which allows to push the model to the meshcat
     visualizer passed as parameter
@@ -38,7 +39,7 @@ def robot_viz(
     global robot_names
 
     robot_names[robot] = name
-    viz = pin.visualize.MeshcatVisualizer(
+    viz = pin_viz.MeshcatVisualizer(
         robot.model, robot.collision_model, robot.visual_model
     )
     viz.initViewer(viewer=get_viewer())

{placo-0.6.5 → placo-0.7.0}/src/placo/humanoid/humanoid_parameters.h

@@ -155,7 +155,6 @@ public:
   double zmp_reference_weight = 1e-1;
 
   // TODO: use this operational space with the FootstepsPlanners
-  
   // Operational space of the flying foot (half-ellipse)
   double op_space_y_offset = 0.1;
   double op_space_x_radius = 0.35;
@@ -164,6 +163,9 @@ public:
   // Operational space of the flying foot (polygon)
   std::vector<Eigen::Vector2d> op_space_polygon;
 
+  // DCM offset bounds (polygon)
+  std::vector<Eigen::Vector2d> dcm_offset_polygon;
+
   /**
    * @brief Applies the ellipsoid clipping to a given step size (dx, dy, dtheta)
    */

{placo-0.6.5 → placo-0.7.0}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -279,6 +279,12 @@ double WalkPatternGenerator::Trajectory::get_part_t_end(double t)
   return part.t_end;
 }
 
+Eigen::Vector2d WalkPatternGenerator::Trajectory::get_part_end_dcm(double t, double omega)
+{
+  TrajectoryPart& part = _findPart(parts, t);
+  return get_p_world_DCM(part.t_end, omega);
+}
+
 void WalkPatternGenerator::Trajectory::add_supports(double t, FootstepsPlanner::Support& support)
 {
   for (auto footstep : support.footsteps)
@@ -311,6 +317,9 @@ void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_ind
     Eigen::Vector3d start_vel = old_trajectory->get_v_world_foot(flying_side, t_replan);
     part.swing_trajectory = SwingFootCubic::make_trajectory(part.t_start, virt_duration, parameters.walk_foot_height, 
           parameters.walk_foot_rise_ratio, start, T_world_end.translation(), part.support.elapsed_ratio, start_vel);
+
+    double replan_yaw = old_trajectory->foot_yaw(flying_side).pos(t_replan);
+    trajectory.foot_yaw(flying_side).add_point(t_replan, replan_yaw, 0);
   }
   else
   {
@@ -569,74 +578,118 @@ void WalkPatternGenerator::Trajectory::print_parts_timings()
   }
 }
 
-std::pair<Eigen::Vector2d, double> WalkPatternGenerator::compute_next_support(double t, FootstepsPlanner::Support& current_support, 
-  FootstepsPlanner::Support& next_support, Eigen::Vector2d world_measured_dcm, Eigen::Vector2d world_initial_dcm)
+std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Support> supports, 
+  Eigen::Vector2d world_measured_dcm, Eigen::Vector2d world_end_dcm)
 {
+  FootstepsPlanner::Support current_support = supports[0];
+  FootstepsPlanner::Support next_support = supports[1];
+
   if (current_support.is_both())
   {
     throw std::runtime_error("Can't modify flying target and step duration if the current support is both");
   }
+  if (next_support.is_both())
+  {
+    throw std::runtime_error("Next support is both, not supported for now");
+  }
 
   placo::problem::Problem problem;
-  double w1 = 1;
-  double w2 = 5;
+  double w1 = 5;
+  double w2 = 100;
   double w3 = 1000;
   double w_viability = 1e6;
 
-  // Decision variables
-  placo::problem::Variable support_next_zmp = problem.add_variable(2); // ZMP of the next support expressed in the current support frame
-  placo::problem::Variable tau = problem.add_variable(1); // exp(omega * T) where T is the end of the current support
-  placo::problem::Variable support_dcm_offset = problem.add_variable(2); // Offset of the DCM from the ZMP in the current support frame
-
-  // LIPM Dynamics (expressed in the current support frame)
-  Eigen::Matrix3d R_world_support = current_support.frame().rotation();
+  double elapsed_time = t - current_support.t_start;
+  Eigen::Matrix2d R_world_support = current_support.frame().rotation().topLeftCorner(2, 2);
   Eigen::Vector2d p_world_support = current_support.frame().translation().head(2);
-  Eigen::Vector2d support_measured_dcm = p_world_support + R_world_support.topRightCorner(2, 2) * world_measured_dcm;
-  problem.add_constraint(support_next_zmp.expr() + support_dcm_offset.expr() == support_measured_dcm * exp(-omega * t) * tau.expr()).configure(ProblemConstraint::Hard);
 
-  // ----------------- Objective functions: -----------------
-  // ZMP Reference (expressed in the world frame)
-  Expression world_next_zmp_expr = p_world_support + R_world_support.topRightCorner(2, 2) * support_next_zmp.expr();
-  problem.add_constraint(world_next_zmp_expr == next_support.frame().translation().head(2)).configure(ProblemConstraint::Soft, w1);
+  // Decision variables
+  placo::problem::Variable* support_next_zmp = &problem.add_variable(2); // ZMP of the next support expressed in the current support frame
+  placo::problem::Variable* tau = &problem.add_variable(1); // exp(omega * T) where T is the end of the current support
+  placo::problem::Variable* support_dcm_offset = &problem.add_variable(2); // Offset of the DCM from the ZMP in the current support frame
 
+  // ----------------- Objective functions: -----------------
   // Time reference
   double T = support_default_duration(current_support);
-  problem.add_constraint(tau.expr() == exp(omega * T)).configure(ProblemConstraint::Soft, w2);
+  problem.add_constraint(tau->expr() == exp(omega * T)).configure(ProblemConstraint::Soft, w1);
+
+  // ZMP Reference (expressed in the world frame)
+  Expression world_next_zmp_expr = p_world_support + R_world_support * support_next_zmp->expr();
+  problem.add_constraint(world_next_zmp_expr == next_support.frame().translation().head(2)).configure(ProblemConstraint::Soft, w2);
 
   // DCM offset reference (expressed in the world frame)
-  Eigen::Vector2d world_target_dcm_offset = (world_initial_dcm - p_world_support) * exp(omega * T) + p_world_support - next_support.frame().translation().head(2);
-  Expression world_dcm_offset_expr = R_world_support.topRightCorner(2, 2) * support_dcm_offset.expr();
+  Eigen::Vector2d world_target_dcm_offset = world_end_dcm - next_support.frame().translation().head(2);
+  Expression world_dcm_offset_expr = R_world_support * support_dcm_offset->expr();
   problem.add_constraint(world_dcm_offset_expr == world_target_dcm_offset).configure(ProblemConstraint::Soft, w3);
 
   // --------------------- Constraints: ---------------------
-  // ZMP constraints - operationnal space of the flying foot (expressed in the current support frame)
+  // Time constraints
+  double T_min = std::max(0.15, elapsed_time); // Should probably add an offset to elapsed_time
+  double T_max = std::max(3., elapsed_time); // Arbitrary value of 3s
+  problem.add_constraint(tau->expr() >= exp(omega * (T_min))).configure(ProblemConstraint::Hard);
+  problem.add_constraint(tau->expr() <= exp(omega * (T_max))).configure(ProblemConstraint::Hard);
+
+  // ZMP and DCM offset constraints (expressed in the current support frame)
   if (current_support.side() == HumanoidRobot::Side::Right)
   {
-    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_next_zmp.expr(), parameters.op_space_polygon)).configure(ProblemConstraint::Hard);
+    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_next_zmp->expr(), parameters.op_space_polygon)).configure(ProblemConstraint::Hard);
+
+    std::vector<Eigen::Vector2d> dcm_offset_polygon = parameters.dcm_offset_polygon;
+    for (auto& point : dcm_offset_polygon)
+    {
+      point(0) = -point(0);
+      point(1) = -point(1);
+    }
+    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_dcm_offset->expr(), parameters.dcm_offset_polygon)).configure(ProblemConstraint::Soft, w_viability);
   }
   else
   {
     std::vector<Eigen::Vector2d> op_space_polygon = parameters.op_space_polygon;
     for (auto& point : op_space_polygon)
     {
+      point(0) = -point(0);
       point(1) = -point(1);
     }
-    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_next_zmp.expr(), op_space_polygon)).configure(ProblemConstraint::Hard);
+    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_next_zmp->expr(), op_space_polygon)).configure(ProblemConstraint::Hard);
+
+    problem.add_constraint(PolygonConstraint::in_polygon_xy(support_dcm_offset->expr(), parameters.dcm_offset_polygon)).configure(ProblemConstraint::Soft, w_viability);
   }
   
-  // Time constraints
-  double T_min = std::max(0.15, t); // Should probably add an offset to t
-  double T_max = 3; // Arbitrary value of 3s
-  problem.add_constraint(tau.expr() >= exp(omega * (T_min))).configure(ProblemConstraint::Hard);
-  problem.add_constraint(tau.expr() <= exp(omega * (T_max))).configure(ProblemConstraint::Hard);
-
-  // DCM offset constraints - viability constraint (expressed in the current support frame)
-  // TODO
+  // LIPM Dynamics (expressed in the world frame)
+  double duration = std::max(0., T - elapsed_time);
+  Eigen::Vector2d world_virtual_zmp = get_optimal_zmp(world_measured_dcm, world_end_dcm, duration, current_support);
+  problem.add_constraint(world_next_zmp_expr + world_dcm_offset_expr == (world_measured_dcm - world_virtual_zmp) * exp(-omega * elapsed_time) * tau->expr() + world_virtual_zmp).configure(ProblemConstraint::Hard);
 
   problem.solve();
-  Eigen::Vector2d world_next_zmp_val = p_world_support + R_world_support.topRightCorner(2, 2) * support_next_zmp.value;
-  double support_t_end = current_support.t_start + log(tau.value(0)) / omega;
 
-  return std::make_pair(world_next_zmp_val, support_t_end);
+  // Updating next support position
+  Eigen::Vector2d world_next_zmp_val = p_world_support + R_world_support * support_next_zmp->value;
+  supports[1].footsteps[0].frame.translation().x() = world_next_zmp_val(0);
+  supports[1].footsteps[0].frame.translation().y() = world_next_zmp_val(1);
+
+  // Updating current support remaining duration
+  double support_remaining_time = log(tau->value(0)) / omega - elapsed_time;
+  supports[0].time_ratio = support_remaining_time / (support_default_duration(current_support) * (1 - current_support.elapsed_ratio));
+
+  return supports;
+}
+
+Eigen::Vector2d WalkPatternGenerator::get_optimal_zmp(Eigen::Vector2d world_dcm_start, 
+  Eigen::Vector2d world_dcm_end, double duration, FootstepsPlanner::Support& support)
+{
+  placo::problem::Problem problem;
+
+  // Decision variables
+  placo::problem::Variable* world_zmp = &problem.add_variable(2);
+
+  // LIPM Dynamics
+  // problem.add_constraint(world_dcm_end == (world_dcm_start - world_zmp->expr()) * exp(omega * duration) + world_zmp->expr()).configure(ProblemConstraint::Soft, 1);
+  problem.add_constraint(world_zmp->expr() == (world_dcm_end - world_dcm_start * exp(omega * duration)) / (1 - exp(omega * duration))).configure(ProblemConstraint::Soft, 1);
+
+  // ZMP constrained to stay in the support polygon
+  problem.add_constraint(PolygonConstraint::in_polygon_xy(world_zmp->expr(), support.support_polygon(), parameters.zmp_margin)).configure(ProblemConstraint::Hard);
+
+  problem.solve();
+  return world_zmp->value;
 }
 }  // namespace placo::humanoid