placo 0.9.2__tar.gz → 0.9.3__tar.gz

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

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

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

{placo-0.9.2 → 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.2 → 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.2 → 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.2 → 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();
@@ -756,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);
@@ -769,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));
 

{placo-0.9.2 → 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.2 → 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.;