placo 0.7.6__tar.gz → 0.7.10__tar.gz

placo-0.7.10/.github/workflows/wheels.yml

@@ -0,0 +1,71 @@
+name: Build
+
+on:
+  release:
+    types:
+      - published
+
+env:
+  CIBW_BUILD: "cp310-*64"
+  CIBW_SKIP: "*musl*"
+  CIBW_REPAIR_WHEEL_COMMAND: ""
+
+jobs:
+  build_wheels:
+    name: Build wheels on ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [macos-latest, ubuntu-latest]
+
+    steps:
+      - uses: actions/checkout@v4
+
+      # Used to host cibuildwheel
+      - uses: actions/setup-python@v5
+
+      - name: Install cibuildwheel
+        run: python -m pip install cibuildwheel==3.0.0b1
+
+      - name: Build wheels
+        run: python -m cibuildwheel --output-dir wheelhouse
+        # to supply options, put them in 'env', like:
+        # env:
+        #   CIBW_SOME_OPTION: value
+        #   ...
+
+      - uses: actions/upload-artifact@v4
+        with:
+          name: cibw-wheels-${{ matrix.os }}-${{ strategy.job-index }}
+          path: ./wheelhouse/*.whl
+
+  build_sdist:
+    name: Build source distribution
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Build sdist
+        run: pipx run build --sdist
+
+      - uses: actions/upload-artifact@v4
+        with:
+          name: cibw-sdist
+          path: dist/*.tar.gz
+
+  upload_pypi:
+    needs: [build_sdist, build_wheels]
+    runs-on: ubuntu-latest
+    environment: pypi
+    permissions:
+      id-token: write
+    if: github.event_name == 'release' && github.event.action == 'published'
+    steps:
+      - uses: actions/download-artifact@v4
+        with:
+          # unpacks all CIBW artifacts into dist/
+          pattern: cibw-*
+          path: dist
+          merge-multiple: true
+
+      - uses: pypa/gh-action-pypi-publish@release/v1

{placo-0.7.6 → placo-0.7.10}/CMakeLists.txt

@@ -154,6 +154,7 @@ add_library(placo MODULE
     bindings/module.cpp
 )
 set_target_properties(placo PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${PYTHON_SITELIB})
+target_compile_definitions(placo PUBLIC -DBOOST_DISABLE_PRAGMA_MESSAGE)
 
 target_link_libraries(placo ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} libplaco)
 target_include_directories(placo PRIVATE ${PYTHON_INCLUDE_DIRS})

{placo-0.7.6 → placo-0.7.10}/Doxyfile

@@ -803,7 +803,7 @@ QUIET                  = NO
 # Tip: Turn warnings on while writing the documentation.
 # The default value is: YES.
 
-WARNINGS               = YES
+WARNINGS               = NO
 
 # If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
 # warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
@@ -818,7 +818,7 @@ WARN_IF_UNDOCUMENTED   = NO
 # markup commands wrongly.
 # The default value is: YES.
 
-WARN_IF_DOC_ERROR      = YES
+WARN_IF_DOC_ERROR      = NO
 
 # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
 # are documented, but have no documentation for their parameters or return
@@ -869,7 +869,6 @@ INPUT                  =  src/placo/dynamics \
 	src/placo/tools \
 	src/placo/model \
 	src/placo/kinematics \
-	src/placo/planning \
 	src/placo/problem \
 	src/placo 
 
@@ -2361,7 +2360,7 @@ HIDE_UNDOC_RELATIONS   = YES
 # set to NO
 # The default value is: YES.
 
-HAVE_DOT               = YES
+HAVE_DOT               = NO
 
 # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
 # to run in parallel. When set to 0 doxygen will base this on the number of

{placo-0.7.6 → placo-0.7.10}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.7.6
+Version: 0.7.10
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.9
 License-Expression: MIT
@@ -16,9 +16,8 @@ Requires-Dist: meshcat
 Requires-Dist: ischedule
 Provides-Extra: build
 Requires-Dist: pin[build] >=  3 ; extra == "build"
-Requires-Dist: cmeel-eigen ; extra == "build"
-Requires-Dist: eigenpy ; extra == "build"
-Requires-Dist: doxystub ; extra == "build"
+Requires-Dist: doxystub >= 0.0.13 ; extra == "build"
+Requires-Dist: cmake<4 ; extra == "build"
 Description-Content-Type: text/markdown
 
 <img width="400" src="https://placo.readthedocs.io/en/latest/_static/placo.png" />

{placo-0.7.6 → placo-0.7.10}/bindings/expose-footsteps.cpp

@@ -48,7 +48,8 @@ void exposeFootsteps()
       .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)
-      .add_property("replanned", &FootstepsPlanner::Support::replanned, &FootstepsPlanner::Support::replanned);
+      .add_property("target_world_dcm", &FootstepsPlanner::Support::target_world_dcm, 
+                    &FootstepsPlanner::Support::target_world_dcm);
 
   class__<FootstepsPlanner, boost::noncopyable>("FootstepsPlanner", no_init)
       .def("make_supports", &FootstepsPlanner::make_supports)

{placo-0.7.6 → placo-0.7.10}/bindings/expose-parameters.cpp

@@ -27,7 +27,6 @@ void exposeParameters()
       .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)
       .add_property("walk_foot_height", &HumanoidParameters::walk_foot_height, &HumanoidParameters::walk_foot_height)
       .add_property("walk_foot_rise_ratio", &HumanoidParameters::walk_foot_rise_ratio,

{placo-0.7.6 → placo-0.7.10}/bindings/expose-robot-wrapper.cpp

@@ -57,6 +57,7 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("get_T_world_fbase", &RobotType::get_T_world_fbase)
       .def("set_T_world_fbase", &RobotType::set_T_world_fbase)
       .def("com_world", &RobotType::com_world)
+      .def("dcom_world", &RobotType::dcom_world)
       .def("centroidal_map", &RobotType::centroidal_map)
       .def("joint_names", &RobotType::joint_names, joint_names_overloads())
       .def("frame_names", &RobotType::frame_names)
@@ -178,11 +179,15 @@ void exposeRobotWrapper()
   humanoidWrapper
       .def<void (HumanoidRobot::*)(const std::string&)>("update_support_side", &HumanoidRobot::update_support_side)
       .def("ensure_on_floor", &HumanoidRobot::ensure_on_floor)
+      .def("ensure_on_floor_oriented", &HumanoidRobot::ensure_on_floor_oriented)
       .def("get_T_world_left", &HumanoidRobot::get_T_world_left)
       .def("get_T_world_right", &HumanoidRobot::get_T_world_right)
       .def("get_T_world_trunk", &HumanoidRobot::get_T_world_trunk)
       .def("get_com_velocity", &HumanoidRobot::get_com_velocity)
-      .def("dcm", &HumanoidRobot::dcm)
+      .def("dcm", +[](HumanoidRobot& robot, double omega) { return robot.dcm(omega); })
+      .def("dcm_from_com_vel", +[](HumanoidRobot& robot, double omega, Eigen::Vector2d com_velocity) {
+        return robot.dcm(omega, com_velocity);
+      })
       .def("zmp", &HumanoidRobot::zmp)
       .def("other_side", &HumanoidRobot::other_side)
       .def(

{placo-0.7.6 → placo-0.7.10}/bindings/expose-walk-pattern-generator.cpp

@@ -27,7 +27,7 @@ void exposeWalkPatternGenerator()
       .add_property("t_end", &WalkPatternGenerator::TrajectoryPart::t_end, &WalkPatternGenerator::TrajectoryPart::t_end)
       .add_property("support", &WalkPatternGenerator::TrajectoryPart::support);
 
-  class__<WalkPatternGenerator::Trajectory>("WPGTrajectory", init<double, double, double>())
+  class__<WalkPatternGenerator::Trajectory>("WPGTrajectory", init<double, double, double, double>())
       .add_property("t_start", &WalkPatternGenerator::Trajectory::t_start)
       .add_property("t_end", &WalkPatternGenerator::Trajectory::t_end)
       .add_property("com_target_z", &WalkPatternGenerator::Trajectory::com_target_z)
@@ -63,7 +63,9 @@ void exposeWalkPatternGenerator()
       .def("can_replan_supports", &WalkPatternGenerator::can_replan_supports)
       .def("replan_supports", &WalkPatternGenerator::replan_supports)
       .def("update_supports", &WalkPatternGenerator::update_supports)
-      .def("get_optimal_zmp", &WalkPatternGenerator::get_optimal_zmp);
+      .def("get_optimal_zmp", &WalkPatternGenerator::get_optimal_zmp)
+      .def("support_default_timesteps", &WalkPatternGenerator::support_default_timesteps)
+      .def("support_default_duration", &WalkPatternGenerator::support_default_duration);
 
   class__<SwingFoot>("SwingFoot", init<>())
       .def("make_trajectory", &SwingFoot::make_trajectory)

{placo-0.7.6 → placo-0.7.10}/pyproject.toml

@@ -5,9 +5,8 @@ requires = [
   "eiquadprog >= 1.2.6, < 2",
   "pin[build] >=  3",
   "rhoban-cmeel-jsoncpp",
-  "cmeel-eigen",
-  "eigenpy",
-  "doxystub"
+  "doxystub >= 0.0.13",
+  "cmake<4"
 ]
 
 [project]
@@ -24,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.7.6"
+version = "0.7.10"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"
@@ -42,3 +41,15 @@ all = true
 
 [tool.cmeel]
 run-tests = false
+
+
+[tool.cibuildwheel.macos]
+before-all = "brew install doxygen"
+
+[tool.cibuildwheel.linux]
+before-all = "yum install -y doxygen"
+
+[[tool.cibuildwheel.overrides]]
+select = "*-musllinux*"
+before-all = "apk add doxygen"
+

placo-0.7.10/scripts/requirements.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+# Installing doxystub
+pip install doxystub
+
+# APT dependencies
+sudo apt install -qqy lsb-release curl libboost-python-dev libjsoncpp-dev doxygen
+
+# Installing robotpkg
+bash scripts/robotpkg.sh
+
+# Installing robotpkg dependencies
+sudo apt install -qqy robotpkg-py3*-pinocchio robotpkg-coal robotpkg-eiquadprog robotpkg-*-eigenpy

placo-0.7.10/scripts/robotpkg.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+# Installing robotpkg
+# see https://stack-of-tasks.github.io/pinocchio/download.html
+if [ ! -f /etc/apt/sources.list.d/robotpkg.list ]
+then
+  echo "* Setting up robotpkg apt..."
+  sudo mkdir -p /etc/apt/keyrings
+  curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \
+      | sudo tee /etc/apt/keyrings/robotpkg.asc
+
+  echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \
+     | sudo tee /etc/apt/sources.list.d/robotpkg.list
+
+  sudo apt update
+else
+  echo "* robotpkg is already present"
+fi
+
+PATH_TEST=`echo $PATH|grep openrob|wc -l`
+if [ $PATH_TEST -eq 0 ]
+then
+  echo "Adding environment variables to .bashrc: you might have to reload your shell"
+
+  PYTHON_VERSION=`python --version|cut -d" " -f2|cut -d"." -f-2`
+
+  echo "# Openrobot packages" >> ~/.bashrc
+  echo "export PATH=/opt/openrobots/bin:\$PATH" >> ~/.bashrc
+  echo "export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:\$PKG_CONFIG_PATH" >> ~/.bashrc
+  echo "export LD_LIBRARY_PATH=/opt/openrobots/lib:\$LD_LIBRARY_PATH" >> ~/.bashrc
+  echo "export PYTHONPATH=/opt/openrobots/lib/python$PYTHON_VERSION/site-packages:\$PYTHONPATH" >> ~/.bashrc
+  echo "export CMAKE_PREFIX_PATH=/opt/openrobots:\$CMAKE_PREFIX_PATH" >> ~/.bashrc
+fi

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/footsteps_planner.h

@@ -52,10 +52,10 @@ public:
     double elapsed_ratio = 0.; // Elapsed ratio of the support phase, ranging from 0 to 1
 
     double time_ratio = 1.; // Time ratio for the remaining part of the support phase
+    Eigen::Vector2d target_world_dcm = Eigen::Vector2d::Zero(); // Target DCM in the world frame at the end of the support phase
 
     bool start = false;
     bool end = false;
-    bool replanned = false;
 
     std::vector<Eigen::Vector2d> polygon;
     bool computed_polygon = false;

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/humanoid_parameters.h

@@ -69,11 +69,6 @@ public:
    */
   int planned_timesteps = 100;
 
-  /**
-   * @brief Number of timesteps between each replan for the CoM trajectory
-   */
-  int replan_timesteps = 10;
-
   /**
    * @brief Margin for the ZMP to live in the support polygon [m]
    */

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/humanoid_robot.cpp

@@ -39,6 +39,7 @@ void HumanoidRobot::initialize()
 void HumanoidRobot::init_config()
 {
   support_side = Left;
+  support_is_both = false;
 
   T_world_support.setIdentity();
 
@@ -94,6 +95,19 @@ void HumanoidRobot::ensure_on_floor()
   update_kinematics();
 }
 
+void HumanoidRobot::ensure_on_floor_oriented(Eigen::Matrix3d R_world_trunk)
+{
+  update_kinematics();
+
+  // Getting the support orientation
+  Eigen::Affine3d T_trunk_support = get_T_a_b(trunk, support_frame());
+  Eigen::Matrix3d R_world_support = R_world_trunk * T_trunk_support.linear();
+  T_world_support.linear() = R_world_support;
+
+  set_T_world_frame(support_frame(), T_world_support);
+  update_kinematics();
+}
+
 void HumanoidRobot::update_from_imu(Eigen::Matrix3d R_world_trunk)
 {
   update_kinematics();
@@ -180,13 +194,19 @@ Eigen::VectorXd HumanoidRobot::get_torques(Eigen::VectorXd acc_a, Eigen::VectorX
   return M * acc + h - J_c * contact_forces;
 }
 
-Eigen::Vector2d HumanoidRobot::dcm(Eigen::Vector2d com_velocity, double omega)
+Eigen::Vector2d HumanoidRobot::dcm(double omega)
+{
+  // DCM = c + (1/omega) c_dot
+  return com_world().head(2) + (1 / omega) * dcom_world().head(2);
+}
+
+Eigen::Vector2d HumanoidRobot::dcm(double omega, Eigen::Vector2d com_velocity)
 {
   // DCM = c + (1/omega) c_dot
   return com_world().head(2) + (1 / omega) * com_velocity;
 }
 
-Eigen::Vector2d HumanoidRobot::zmp(Eigen::Vector2d com_acceleration, double omega)
+Eigen::Vector2d HumanoidRobot::zmp(double omega, Eigen::Vector2d com_acceleration)
 {
   // ZMP = c - (1/omega^2) c_ddot
   return com_world().head(2) - (1 / pow(omega, 2)) * com_acceleration;

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/humanoid_robot.h

@@ -38,6 +38,13 @@ public:
    */
   void ensure_on_floor();
 
+  /**
+   * @brief Place the robot on its support on the floor according 
+   * to the trunk orientation and the kinematic configuration
+   * @param R_world_trunk Orientation of the trunk
+   */
+  void ensure_on_floor_oriented(Eigen::Matrix3d R_world_trunk);
+
   /**
    * @brief Rotate the robot around its support
    * @param R_world_trunk Orientation of the trunk from the IMU
@@ -67,19 +74,26 @@ public:
 
   /**
    * @brief Compute the Divergent Component of Motion (DCM)
-   * @param com_velocity CoM velocity
    * @param omega Natural frequency of the LIP (= sqrt(g/h))
    * @return DCM
    */
-  Eigen::Vector2d dcm(Eigen::Vector2d com_velocity, double omega);
+  Eigen::Vector2d dcm(double omega);
+
+  /**
+   * @brief Compute the Divergent Component of Motion (DCM)
+   * @param omega Natural frequency of the LIP (= sqrt(g/h))
+   * @param com_velocity CoM velocity
+   * @return DCM
+   */
+  Eigen::Vector2d dcm(double omega, Eigen::Vector2d com_velocity);
 
   /**
    * @brief Compute the Zero-tilting Moment Point (ZMP)
-   * @param com_acceleration CoM acceleration
    * @param omega Natural frequency of the LIP (= sqrt(g/h))
+   * @param com_acceleration CoM acceleration
    * @return ZMP
    */
-  Eigen::Vector2d zmp(Eigen::Vector2d com_acceleration, double omega);
+  Eigen::Vector2d zmp(double omega, Eigen::Vector2d com_acceleration);
 
   // We suppose we have one support frame and associated transformation
   RobotWrapper::FrameIndex support_frame();

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -24,13 +24,14 @@ WalkPatternGenerator::Trajectory::Trajectory()
   T.setIdentity();
 }
 
-WalkPatternGenerator::Trajectory::Trajectory(double com_target_z, double t_start, double trunk_pitch) 
+WalkPatternGenerator::Trajectory::Trajectory(double com_target_z, double t_start, double trunk_pitch, double trunk_roll) 
   : left_foot_yaw(true)
   , right_foot_yaw(true)
   , trunk_yaw(true)
   , com_target_z(com_target_z)
   , t_start(t_start)
   , trunk_pitch(trunk_pitch)
+  , trunk_roll(trunk_roll)
 {
   T.setIdentity();
 }
@@ -190,9 +191,10 @@ Eigen::Vector2d WalkPatternGenerator::Trajectory::get_p_world_ZMP(double t, doub
 
 Eigen::Matrix3d WalkPatternGenerator::Trajectory::get_R_world_trunk(double t)
 {
-  return T.linear() * Eigen::AngleAxisd(trunk_yaw.pos(t), Eigen::Vector3d::UnitZ()).matrix() *
-         Eigen::AngleAxisd(trunk_pitch, Eigen::Vector3d::UnitY()).matrix() *
-         Eigen::AngleAxisd(trunk_roll, Eigen::Vector3d::UnitX()).matrix();
+  Eigen::Matrix3d R_world_trunk = Eigen::AngleAxisd(trunk_yaw.pos(t), Eigen::Vector3d::UnitZ()).matrix() *
+                                  Eigen::AngleAxisd(trunk_pitch, Eigen::Vector3d::UnitY()).matrix() *
+                                  Eigen::AngleAxisd(trunk_roll, Eigen::Vector3d::UnitX()).matrix();
+  return T.linear() * R_world_trunk;
 }
 
 HumanoidRobot::Side WalkPatternGenerator::Trajectory::support_side(double t)
@@ -237,7 +239,7 @@ FootstepsPlanner::Support WalkPatternGenerator::Trajectory::get_next_support(dou
   _findPart(parts, t, &index);
   if (index + n >= parts.size())
   {
-    throw std::runtime_error("No next support available for the given time and offset");
+    return T * parts.back().support;
   }
   return T * parts[index + n].support;
 }
@@ -301,7 +303,7 @@ void WalkPatternGenerator::plan_dbl_support(Trajectory& trajectory, int part_ind
   trajectory.trunk_yaw.add_point(part.t_end, frame_yaw(part.support.frame().rotation()), 0);
 }
 
-void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory, double t_replan)
+void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory)
 {
   TrajectoryPart& part = trajectory.parts[part_index];
 
@@ -313,13 +315,13 @@ void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_ind
 
   if (part_index == 0 && old_trajectory != nullptr)
   {
-    Eigen::Vector3d start = old_trajectory->get_T_world_foot(flying_side, t_replan).translation();
-    Eigen::Vector3d start_vel = old_trajectory->get_v_world_foot(flying_side, t_replan);
+    Eigen::Vector3d start = old_trajectory->get_T_world_foot(flying_side, trajectory.t_start).translation();
+    Eigen::Vector3d start_vel = old_trajectory->get_v_world_foot(flying_side, trajectory.t_start);
     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);
+    double replan_yaw = old_trajectory->foot_yaw(flying_side).pos(trajectory.t_start);
+    trajectory.foot_yaw(flying_side).add_point(trajectory.t_start, replan_yaw, 0);
   }
   else
   {
@@ -340,7 +342,7 @@ void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_ind
   trajectory.add_supports(part.t_end, part.support);
 }
 
-void WalkPatternGenerator::plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory, double t_replan)
+void WalkPatternGenerator::plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory)
 {
   // Add the initial position to the trajectory
   trajectory.add_supports(trajectory.t_start, trajectory.parts[0].support);
@@ -359,7 +361,7 @@ void WalkPatternGenerator::plan_feet_trajectories(Trajectory& trajectory, Trajec
     // Single support
     if (trajectory.parts[i].support.footsteps.size() == 1)
     {
-      plan_sgl_support(trajectory, i, old_trajectory, t_replan);
+      plan_sgl_support(trajectory, i, old_trajectory);
     }
     // Double support
     else
@@ -438,9 +440,12 @@ void WalkPatternGenerator::plan_com(Trajectory& trajectory, std::vector<Footstep
 
   // Solving the problem
   problem.solve();
+  
   for (int i = 0; i < trajectory.parts.size(); i++)
   {
-    trajectory.parts[i].com_trajectory = lipms[i].get_trajectory();
+    auto& part = trajectory.parts[i];
+    part.com_trajectory = lipms[i].get_trajectory();
+    part.support.target_world_dcm = part.com_trajectory.dcm(part.t_end, omega);
   }
 }
 
@@ -452,7 +457,7 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::plan(std::vector<Footstep
     throw std::runtime_error("Trying to plan() with 0 supports");
   }
 
-  Trajectory trajectory(parameters.walk_com_height, t_start, parameters.walk_trunk_pitch);
+  Trajectory trajectory(parameters.walk_com_height, t_start, parameters.walk_trunk_pitch, 0.);
 
   plan_com(trajectory, supports, initial_com_world.head(2));
   plan_feet_trajectories(trajectory);
@@ -469,14 +474,21 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::replan(std::vector<Footst
     throw std::runtime_error("Trying to replan() with 0 supports");
   }
 
-  Trajectory trajectory(parameters.walk_com_height, t_replan, parameters.walk_trunk_pitch);
+  // std::cout << "------------------ Old Next support: -----------------" << std::endl;
+  // std::cout << "x: " << old_trajectory.get_next_support(t_replan).frame().translation().x() << std::endl;
+  // std::cout << "y: " << old_trajectory.get_next_support(t_replan).frame().translation().y() << std::endl;
+  // std::cout << "------------------ New Next support: -----------------" << std::endl;
+  // std::cout << "x: " << supports[1].frame().translation().x() << std::endl;
+  // std::cout << "y: " << supports[1].frame().translation().y() << std::endl;
+
+  Trajectory trajectory(parameters.walk_com_height, t_replan, parameters.walk_trunk_pitch, 0.);
 
   Eigen::Vector2d initial_pos = old_trajectory.get_p_world_CoM(t_replan).head(2);
   Eigen::Vector2d initial_vel = old_trajectory.get_v_world_CoM(t_replan).head(2);
   Eigen::Vector2d initial_acc = old_trajectory.get_a_world_CoM(t_replan).head(2);
   plan_com(trajectory, supports, initial_pos, initial_vel, initial_acc);
 
-  plan_feet_trajectories(trajectory, &old_trajectory, t_replan);
+  plan_feet_trajectories(trajectory, &old_trajectory);
 
   return trajectory;
 }
@@ -489,6 +501,17 @@ bool WalkPatternGenerator::can_replan_supports(Trajectory& trajectory, double t_
     return false;
   }
 
+  // XXX: Need to move to a branch
+  // We can't replan if more than 1/2 of the support phase has passed
+  // if (trajectory.get_support(t_replan).elapsed_ratio > 0.8)
+  // {
+  //   return false;
+  // }
+  // if (t_replan - trajectory.get_support(t_replan).t_start < 0.005)
+  // {
+  //   return false;
+  // }
+
   return true;
 }
 
@@ -540,7 +563,7 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::replan_supports(Foo
   double elapsed_duration = t_replan - std::max(t_last_replan, current_support.t_start);
   supports[0].elapsed_ratio = current_support.elapsed_ratio + elapsed_duration / (support_default_duration(current_support) * current_support.time_ratio);
   supports[0].time_ratio = current_support.time_ratio;
-  supports[0].replanned = true;
+  supports[0].target_world_dcm = current_support.target_world_dcm;
   return supports;
 }
 
@@ -578,8 +601,8 @@ void WalkPatternGenerator::Trajectory::print_parts_timings()
   }
 }
 
-std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Support> supports, 
-  Eigen::Vector2d world_measured_dcm, Eigen::Vector2d world_end_dcm)
+std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(double t, 
+  std::vector<FootstepsPlanner::Support> supports, Eigen::Vector2d world_measured_dcm)
 {
   FootstepsPlanner::Support current_support = supports[0];
   FootstepsPlanner::Support next_support = supports[1];
@@ -618,7 +641,10 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(dou
   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_end_dcm - next_support.frame().translation().head(2);
+  Eigen::Vector2d world_target_dcm_offset = current_support.target_world_dcm - next_support.frame().translation().head(2);
+  // Eigen::Vector2d world_target_dcm_offset = world_end_dcm - next_support.frame().translation().head(2);
+  // std::cout << "world_target_dcm_offset: " << world_target_dcm_offset.transpose() << std::endl;
+
   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);
 
@@ -657,7 +683,8 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(dou
   
   // 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);
+  Eigen::Vector2d world_virtual_zmp = get_optimal_zmp(world_measured_dcm, current_support.target_world_dcm, duration, current_support);
+  // Eigen::Vector2d world_virtual_zmp = flatten_on_floor(current_support.frame()).translation().head(2);
   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();

{placo-0.7.6 → placo-0.7.10}/src/placo/humanoid/walk_pattern_generator.h

@@ -36,7 +36,7 @@ public:
   struct Trajectory
   {
     Trajectory();
-    Trajectory(double com_target_z, double t_start = 0., double trunk_pitch = 0.);
+    Trajectory(double com_target_z, double t_start = 0., double trunk_pitch = 0., double trunk_roll = 0.);
 
     // Debug
     void print_parts_timings();
@@ -83,12 +83,14 @@ public:
     FootstepsPlanner::Support get_support(double t);
 
     /**
-     * @brief Returns the nth next support corresponding to the given time in the trajectory
+     * @brief Returns the nth next support corresponding to the given time in the trajectory.
+     * If n is greater than the number of remaining supports, the last support is returned
      */
     FootstepsPlanner::Support get_next_support(double t, int n = 1);
 
     /**
-     * @brief Returns the nth previous support corresponding to the given time in the trajectory
+     * @brief Returns the nth previous support corresponding to the given time in the trajectory.
+     * If n is greater than the number of previous supports, the first support is returned
      */
     FootstepsPlanner::Support get_prev_support(double t, int n = 1);
 
@@ -183,8 +185,8 @@ public:
    * @param world_measured_dcm The measured DCM in world frame
    * @param world_end_dcm The desired DCM at the end of the current support phase
    */
-  std::vector<FootstepsPlanner::Support> update_supports(double t, std::vector<FootstepsPlanner::Support> supports, 
-    Eigen::Vector2d world_measured_dcm, Eigen::Vector2d world_end_dcm);
+  std::vector<FootstepsPlanner::Support> update_supports(double t, 
+    std::vector<FootstepsPlanner::Support> supports, Eigen::Vector2d world_measured_dcm);
 
   /**
    * @brief Computes the best ZMP in the support polygon to move de DCM from 
@@ -196,6 +198,9 @@ public:
    */
   Eigen::Vector2d get_optimal_zmp(Eigen::Vector2d world_dcm_start, Eigen::Vector2d world_dcm_end, 
     double duration, FootstepsPlanner::Support& support);
+
+  int support_default_timesteps(FootstepsPlanner::Support& support);
+  double support_default_duration(FootstepsPlanner::Support& support);
   
 protected:
   // Robot associated to the WPG
@@ -213,10 +218,7 @@ protected:
     Eigen::Vector2d initial_vel = Eigen::Vector2d::Zero(), Eigen::Vector2d initial_acc = Eigen::Vector2d::Zero());
 
   void plan_dbl_support(Trajectory& trajectory, int part_index);
-  void plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory, double t_replan);
-  void plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory = nullptr, double t_replan = 0.);
-
-  int support_default_timesteps(FootstepsPlanner::Support& support);
-  double support_default_duration(FootstepsPlanner::Support& support);
+  void plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory);
+  void plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory = nullptr);
 };
 }  // namespace placo::humanoid