placo 0.9.1__tar.gz → 0.9.3__tar.gz

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

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

{placo-0.9.1 → placo-0.9.3}/bindings/expose-footsteps.cpp

@@ -25,10 +25,12 @@ void exposeFootsteps()
       .add_property("foot_length", &FootstepsPlanner::Footstep::foot_length, &FootstepsPlanner::Footstep::foot_length)
       .add_property("foot_width", &FootstepsPlanner::Footstep::foot_width, &FootstepsPlanner::Footstep::foot_width)
       .add_property("frame", &FootstepsPlanner::Footstep::frame, &FootstepsPlanner::Footstep::frame)
-      .def("set_frame_xy", +[](FootstepsPlanner::Footstep& footstep, double x, double y) {
-        footstep.frame.translation().x() = x;
-        footstep.frame.translation().y() = y;
-      })
+      .def(
+          "set_frame_xy",
+          +[](FootstepsPlanner::Footstep& footstep, double x, double y) {
+            footstep.frame.translation().x() = x;
+            footstep.frame.translation().y() = y;
+          })
       .def("support_polygon", &FootstepsPlanner::Footstep::support_polygon)
       .def("overlap", &FootstepsPlanner::Footstep::overlap)
       .def("polygon_contains", &FootstepsPlanner::Footstep::polygon_contains)
@@ -52,12 +54,21 @@ 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("target_world_dcm", &FootstepsPlanner::Support::target_world_dcm, 
+      .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)
-      .def("opposite_footstep", &FootstepsPlanner::opposite_footstep);
+      .def("opposite_footstep", &FootstepsPlanner::opposite_footstep)
+      .def(
+          "supports_head", +[](const std::vector<FootstepsPlanner::Support> supports, int index) {
+            std::vector<FootstepsPlanner::Support> head;
+            for (int i = 0; i < index; ++i)
+            {
+              head.push_back(supports[i]);
+            }
+            return head;
+          });
 
   class__<FootstepsPlannerNaive, bases<FootstepsPlanner>>("FootstepsPlannerNaive", init<HumanoidParameters&>())
       .def("plan", &FootstepsPlannerNaive::plan)

{placo-0.9.1 → placo-0.9.3}/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.3}/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.3"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.9.1 → placo-0.9.3}/src/placo/humanoid/footsteps_planner.h

@@ -103,7 +103,7 @@ public:
   };
 
   /**
-   * @brief Initializes the solver
+   * @brief Initializes the planner
    * @param parameters Parameters of the walk
    */
   FootstepsPlanner(HumanoidParameters& parameters);

{placo-0.9.1 → placo-0.9.3}/src/placo/humanoid/footsteps_planner_naive.cpp

@@ -18,6 +18,9 @@ namespace placo::humanoid
 {
 FootstepsPlannerNaive::FootstepsPlannerNaive(HumanoidParameters& parameters) : FootstepsPlanner(parameters)
 {
+  accessibility_length = parameters.walk_max_dx_forward;
+  accessibility_width = parameters.walk_max_dy;
+  accessibility_yaw = parameters.walk_max_dtheta;
 }
 
 std::string FootstepsPlannerNaive::name()
@@ -125,10 +128,14 @@ void FootstepsPlannerNaive::plan_impl(std::vector<FootstepsPlanner::Footstep>& f
       error_yaw = accessibility_yaw;
     }
 
+    // Ellipsoid clipping
+    Eigen::Vector3d step(error.x(), error.y(), error_yaw);
+    step = parameters.ellipsoid_clip(step);
+
     // Computing new frame
     Eigen::Affine3d new_step;
-    new_step.translation() = T_support_floatingIdle.translation() + error;
-    new_step.linear() = Eigen::AngleAxisd(error_yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    new_step.translation() = T_support_floatingIdle.translation() + Eigen::Vector3d(step.x(), step.y(), 0.);
+    new_step.linear() = Eigen::AngleAxisd(step[2], Eigen::Vector3d::UnitZ()).toRotationMatrix();
 
     // Going to next step
     Footstep footstep = create_footstep(HumanoidRobot::other_side(current_support_side), T_world_support * new_step);

{placo-0.9.1 → placo-0.9.3}/src/placo/humanoid/swing_foot_cubic.cpp

@@ -15,7 +15,9 @@ Eigen::Vector3d SwingFootCubic::Trajectory::vel(double t)
 }
 
 SwingFootCubic::Trajectory SwingFootCubic::make_trajectory(double t_start, double virt_duration, double height,
-  double rise_ratio, Eigen::Vector3d start, Eigen::Vector3d target, double elapsed_ratio, Eigen::Vector3d start_vel)
+                                                           double rise_ratio, Eigen::Vector3d start,
+                                                           Eigen::Vector3d target, double elapsed_ratio,
+                                                           Eigen::Vector3d start_vel)
 {
   SwingFootCubic::Trajectory trajectory;
 

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

@@ -346,7 +346,6 @@ void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_ind
   Eigen::Affine3d T_world_end = trajectory.parts[part_index + 1].support.footstep_frame(flying_side);
 
   double virt_duration = support_default_duration(part.support) * part.support.time_ratio;
-
   if (part_index == 0 && old_trajectory != nullptr)
   {
     Eigen::Vector3d start = old_trajectory->get_T_world_foot(flying_side, trajectory.t_start).translation();
@@ -554,9 +553,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 +579,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;
 }
 
@@ -766,7 +755,7 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(
   }
 
   // LIPM Dynamics (expressed in the world frame)
-  double duration = std::max(0., T - elapsed_time);
+  double duration = std::max(1e-5, T - elapsed_time);
   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);
@@ -779,13 +768,12 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(
   problem.solve();
 
   // Updating next support position
-  // XXX: Problème de transform T qui s'applique ?
   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;
+  double support_remaining_time = std::max(1e-3, log(tau->value(0)) / omega - elapsed_time);
   supports[0].time_ratio =
       support_remaining_time / (support_default_duration(current_support) * (1 - current_support.elapsed_ratio));
 
@@ -817,4 +805,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.3}/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

{placo-0.9.1 → placo-0.9.3}/src/placo/humanoid/walk_tasks.cpp

@@ -58,7 +58,8 @@ void WalkTasks::reach_initial_pose(Eigen::Affine3d T_world_left, double feet_spa
                                    double trunk_pitch)
 {
   Eigen::Affine3d T_world_right = T_world_left;
-  T_world_right.translation() = T_world_left.translation() + T_world_left.rotation() * Eigen::Vector3d(0, -feet_spacing, 0);
+  T_world_right.translation() =
+      T_world_left.translation() + T_world_left.rotation() * Eigen::Vector3d(0, -feet_spacing, 0);
 
   Eigen::Vector3d com_world = interpolate_frames(T_world_left, T_world_right, .5).translation();
   com_world.z() = com_height;
@@ -68,7 +69,7 @@ void WalkTasks::reach_initial_pose(Eigen::Affine3d T_world_left, double feet_spa
   trunk_orientation_task->R_world_frame = R_world_trunk;
 
   update_tasks(T_world_left, T_world_right, com_world, R_world_trunk);
-  
+
   for (int i = 0; i < 100; i++)
   {
     if (i <= 10)
@@ -129,7 +130,8 @@ void WalkTasks::remove_tasks()
   }
 }
 
-void WalkTasks::update_tasks_and_pid(WalkPatternGenerator::Trajectory& trajectory, double t, Eigen::Vector2d dcm, double omega, double elapsed)
+void WalkTasks::update_tasks_and_pid(WalkPatternGenerator::Trajectory& trajectory, double t, Eigen::Vector2d dcm,
+                                     double omega, double elapsed)
 {
   Eigen::Vector2d error = dcm - trajectory.get_p_world_DCM(t + com_delay, omega);
   integral = (1.0 - lambda) * integral + error * elapsed;
@@ -139,7 +141,7 @@ void WalkTasks::update_tasks_and_pid(WalkPatternGenerator::Trajectory& trajector
   Eigen::Vector2d com_offset = K_p * error + K_i * integral + K_d * derivative;
 
   update_tasks(trajectory.get_T_world_left(t), trajectory.get_T_world_right(t),
-               trajectory.get_p_world_CoM(t + com_delay) + Eigen::Vector3d(com_offset[0], com_offset[1], 0.), 
+               trajectory.get_p_world_CoM(t + com_delay) + Eigen::Vector3d(com_offset[0], com_offset[1], 0.),
                trajectory.get_R_world_trunk(t));
 }
 

{placo-0.9.1 → placo-0.9.3}/src/placo/humanoid/walk_tasks.h

@@ -40,7 +40,8 @@ public:
   double com_y = 0.;
 
   // DCM error PID
-  void update_tasks_and_pid(WalkPatternGenerator::Trajectory& trajectory, double t, Eigen::Vector2d dcm, double omega, double elapsed);
+  void update_tasks_and_pid(WalkPatternGenerator::Trajectory& trajectory, double t, Eigen::Vector2d dcm, double omega,
+                            double elapsed);
 
   double K_p = 0.;
   double K_i = 0.;