placo 0.9.12__tar.gz → 0.9.13__tar.gz

{placo-0.9.12 → placo-0.9.13}/PKG-INFO

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

{placo-0.9.12 → placo-0.9.13}/bindings/expose-parameters.cpp

@@ -58,5 +58,7 @@ void exposeParameters()
       .def("startend_double_support_duration", &HumanoidParameters::startend_double_support_duration)
       .def("has_double_support", &HumanoidParameters::has_double_support)
       .def("ellipsoid_clip", &HumanoidParameters::ellipsoid_clip)
-      .def("ellipsoid_overlap_clip", &HumanoidParameters::ellipsoid_overlap_clip);
+      .def("box_clip", &HumanoidParameters::box_clip)
+      .def("ellipsoid_overlap_clip", &HumanoidParameters::ellipsoid_overlap_clip)
+      .def("box_overlap_clip", &HumanoidParameters::box_overlap_clip);
 }

{placo-0.9.12 → placo-0.9.13}/pyproject.toml

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

{placo-0.9.12 → placo-0.9.13}/src/placo/humanoid/footsteps_planner.cpp

@@ -319,7 +319,14 @@ FootstepsPlanner::Footstep FootstepsPlanner::clipped_opposite_footstep(Footstep
     step.y() += parameters.walk_dtheta_spacing * fabs(step.z());
   }
 
-  step = parameters.ellipsoid_clip(step);
+  if (use_ellipsoid_clipping)
+  {
+    step = parameters.ellipsoid_clip(step);
+  }
+  else
+  {
+    step = parameters.box_clip(step);
+  }
 
   for (int k = 0; k < 32; k++)
   {

{placo-0.9.12 → placo-0.9.13}/src/placo/humanoid/footsteps_planner.h

@@ -149,6 +149,8 @@ public:
   // Humanoid parameters for planning and control
   HumanoidParameters& parameters;
 
+  bool use_ellipsoid_clipping = false;
+
 protected:
   virtual void plan_impl(std::vector<Footstep>&, HumanoidRobot::Side flying_side, Eigen::Affine3d T_world_left,
                          Eigen::Affine3d T_world_right) = 0;

{placo-0.9.12 → placo-0.9.13}/src/placo/humanoid/humanoid_parameters.cpp

@@ -55,10 +55,41 @@ Eigen::Vector3d HumanoidParameters::ellipsoid_clip(Eigen::Vector3d step)
   return step;
 }
 
+Eigen::Vector3d HumanoidParameters::box_clip(Eigen::Vector3d step)
+{
+  Eigen::Vector3d factor((step.x() >= 0) ? walk_max_dx_forward : walk_max_dx_backward, walk_max_dy, walk_max_dtheta);
+  step.x() /= factor.x();
+  step.y() /= factor.y();
+  step.z() /= factor.z();
+
+  double norm = fabs(step.x()) + fabs(step.y()) + fabs(step.z());
+  if (norm > 1)
+  {
+    step /= norm;
+  }
+
+  step.x() *= factor.x();
+  step.y() *= factor.y();
+  step.z() *= factor.z();
+
+  return step;
+}
+
 Eigen::Vector3d HumanoidParameters::ellipsoid_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step)
+{
+  return overlap_clip(support_side, step, true);
+}
+
+Eigen::Vector3d HumanoidParameters::box_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step)
+{
+  return overlap_clip(support_side, step, false);
+}
+
+Eigen::Vector3d HumanoidParameters::overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step, bool ellipsoid)
 {
   FootstepsPlannerRepetitive planner(*this);
   planner.configure(step.x(), step.y(), step.z(), 3);
+  planner.use_ellipsoid_clipping = ellipsoid;
 
   // Creatting footsteps
   Eigen::Affine3d T_world_left = Eigen::Affine3d::Identity();

{placo-0.9.12 → placo-0.9.13}/src/placo/humanoid/humanoid_parameters.h

@@ -162,15 +162,25 @@ public:
   std::vector<Eigen::Vector2d> dcm_offset_polygon;
 
   /**
-   * @brief Applies the ellipsoid clipping to a given step size (dx, dy, dtheta)
+   * @brief Applies the ellipsoid (L2) clipping to a given step size (dx, dy, dtheta)
    */
   Eigen::Vector3d ellipsoid_clip(Eigen::Vector3d step);
 
+  /**
+   * @brief Applies the box clipping (L1) to a given step size (dx, dy, dtheta)
+   */
+  Eigen::Vector3d box_clip(Eigen::Vector3d step);
+
   /**
    * @brief Clips a step using ellipsoid and overlap avoidance
    */
   Eigen::Vector3d ellipsoid_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step);
 
+  /**
+   * @brief Clips a step using ellipsoid and overlap avoidance
+   */
+  Eigen::Vector3d box_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step);
+
   /**
    * @brief Frames for opposite and neutral positions
    */
@@ -178,5 +188,12 @@ public:
                                  double d_y = 0., double d_theta = 0.);
   Eigen::Affine3d neutral_frame(HumanoidRobot::Side side, Eigen::Affine3d T_world_foot, double d_x = 0.,
                                 double d_y = 0., double d_theta = 0.);
+
+protected:
+
+/**
+   * @brief Clips a step using ellipsoid and overlap avoidance
+   */
+  Eigen::Vector3d overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step, bool ellipsoid = false);
 };
 }  // namespace placo::humanoid