placo 0.9.1__tar.gz → 0.9.2__tar.gz

{placo-0.9.1 → placo-0.9.2}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.9.1
+Version: 0.9.2
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.9
 License-Expression: MIT

{placo-0.9.1 → placo-0.9.2}/bindings/expose-walk-pattern-generator.cpp

@@ -35,8 +35,8 @@ void exposeWalkPatternGenerator()
       .add_property("com_target_z", &WalkPatternGenerator::Trajectory::com_target_z)
       .add_property("trunk_pitch", &WalkPatternGenerator::Trajectory::trunk_pitch)
       .add_property("trunk_roll", &WalkPatternGenerator::Trajectory::trunk_roll)
-      .add_property("kept_ts", &WalkPatternGenerator::Trajectory::kept_ts)
       .add_property("parts", &WalkPatternGenerator::Trajectory::parts)
+      .add_property("replan_success", &WalkPatternGenerator::Trajectory::replan_success)
       .def("get_T_world_left", &WalkPatternGenerator::Trajectory::get_T_world_left)
       .def("get_T_world_right", &WalkPatternGenerator::Trajectory::get_T_world_right)
       .def("get_v_world_right", &WalkPatternGenerator::Trajectory::get_v_world_right)

{placo-0.9.1 → placo-0.9.2}/pyproject.toml

@@ -23,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.9.1"
+version = "0.9.2"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.9.1 → placo-0.9.2}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -554,9 +554,10 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::replan(std::vector<Footst
   Eigen::Vector2d initial_acc = old_trajectory.get_a_world_CoM(t_replan).head(2);
   bool planned_com = plan_com(trajectory, supports, initial_pos, initial_vel, initial_acc);
 
+  // If we can't plan the CoM, we return the trimmed old trajectory
   if (!planned_com)
   {
-    return old_trajectory;
+    return trim_old_trajectory(old_trajectory, t_replan);
   }
 
   plan_feet_trajectories(trajectory, &old_trajectory);
@@ -579,17 +580,6 @@ 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;
 }
 
@@ -817,4 +807,40 @@ Eigen::Vector2d WalkPatternGenerator::get_optimal_zmp(Eigen::Vector2d world_dcm_
   problem.solve();
   return world_zmp->value;
 }
+
+WalkPatternGenerator::Trajectory WalkPatternGenerator::trim_old_trajectory(Trajectory& old_trajectory, double t_replan)
+{
+  Trajectory trajectory = old_trajectory;
+  trajectory.t_start = t_replan;
+  trajectory.replan_success = false;
+
+  // Remove the parts before the replan time
+  std::vector<WalkPatternGenerator::TrajectoryPart> new_parts;
+  for (auto part : old_trajectory.parts)
+  {
+    if (t_replan >= part.t_start)
+    {
+      if (t_replan < part.t_end)
+      {
+        TrajectoryPart new_part = part;
+        new_part.t_start = t_replan;
+
+        // Updating the elapsed ratio of the support
+        double elapsed_duration = t_replan - std::max(old_trajectory.t_start, new_part.support.t_start);
+        new_part.support.elapsed_ratio =
+            new_part.support.elapsed_ratio +
+            elapsed_duration / (support_default_duration(new_part.support) * new_part.support.time_ratio);
+
+        new_parts.push_back(new_part);
+      }
+    }
+    else
+    {
+      new_parts.push_back(part);
+    }
+  }
+  trajectory.parts = new_parts;
+
+  return trajectory;
+}
 }  // namespace placo::humanoid

{placo-0.9.1 → placo-0.9.2}/src/placo/humanoid/walk_pattern_generator.h

@@ -53,7 +53,8 @@ public:
     double trunk_pitch;
     double trunk_roll;
 
-    int kept_ts = 0;
+    // Replan succeeded?
+    bool replan_success = true;
 
     Eigen::Affine3d get_T_world_left(double t);
     Eigen::Affine3d get_T_world_right(double t);
@@ -252,5 +253,13 @@ protected:
   void plan_dbl_support(Trajectory& trajectory, int part_index);
   void plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory);
   void plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory = nullptr);
+
+  /**
+   * @brief Trims the old trajectory to the new time horizon without modifying the CoM and feet trajectories.
+   * @param old_trajectory The old trajectory to trim
+   * @param t_replan The time at which the replan happens
+   * @return The trimmed trajectory
+   */
+  Trajectory trim_old_trajectory(Trajectory& old_trajectory, double t_replan);
 };
 }  // namespace placo::humanoid