placo 0.9.13__tar.gz → 0.9.14__tar.gz

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

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

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

@@ -59,6 +59,8 @@ void exposeParameters()
       .def("has_double_support", &HumanoidParameters::has_double_support)
       .def("ellipsoid_clip", &HumanoidParameters::ellipsoid_clip)
       .def("box_clip", &HumanoidParameters::box_clip)
+      .def("conic_clip", &HumanoidParameters::conic_clip)
       .def("ellipsoid_overlap_clip", &HumanoidParameters::ellipsoid_overlap_clip)
-      .def("box_overlap_clip", &HumanoidParameters::box_overlap_clip);
+      .def("box_overlap_clip", &HumanoidParameters::box_overlap_clip)
+      .def("conic_overlap_clip", &HumanoidParameters::conic_overlap_clip);
 }

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

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

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

@@ -319,11 +319,15 @@ FootstepsPlanner::Footstep FootstepsPlanner::clipped_opposite_footstep(Footstep
     step.y() += parameters.walk_dtheta_spacing * fabs(step.z());
   }
 
-  if (use_ellipsoid_clipping)
+  if (footstep_clipping == HumanoidParameters::FootstepClipping::Conic)
+  {
+    step = parameters.conic_clip(step);
+  }
+  else if (footstep_clipping == HumanoidParameters::FootstepClipping::Ellipsoid)
   {
     step = parameters.ellipsoid_clip(step);
   }
-  else
+  else if (footstep_clipping == HumanoidParameters::FootstepClipping::Box)
   {
     step = parameters.box_clip(step);
   }

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

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

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

@@ -75,21 +75,47 @@ Eigen::Vector3d HumanoidParameters::box_clip(Eigen::Vector3d step)
   return step;
 }
 
+Eigen::Vector3d HumanoidParameters::conic_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 = sqrt(step.x() * step.x() + step.y() * 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);
+  return overlap_clip(support_side, step, FootstepClipping::Ellipsoid);
 }
 
 Eigen::Vector3d HumanoidParameters::box_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step)
 {
-  return overlap_clip(support_side, step, false);
+  return overlap_clip(support_side, step, FootstepClipping::Box);
+}
+
+Eigen::Vector3d HumanoidParameters::conic_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step)
+{
+  return overlap_clip(support_side, step, FootstepClipping::Conic);
 }
 
-Eigen::Vector3d HumanoidParameters::overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step, bool ellipsoid)
+Eigen::Vector3d HumanoidParameters::overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step,
+                                                 HumanoidParameters::FootstepClipping clipping)
 {
   FootstepsPlannerRepetitive planner(*this);
   planner.configure(step.x(), step.y(), step.z(), 3);
-  planner.use_ellipsoid_clipping = ellipsoid;
+  planner.footstep_clipping = clipping;
 
   // Creatting footsteps
   Eigen::Affine3d T_world_left = Eigen::Affine3d::Identity();

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

@@ -161,6 +161,13 @@ public:
   // DCM offset bounds (polygon)
   std::vector<Eigen::Vector2d> dcm_offset_polygon;
 
+  enum FootstepClipping
+  {
+    Ellipsoid = 0,
+    Box = 1,
+    Conic = 2
+  };
+
   /**
    * @brief Applies the ellipsoid (L2) clipping to a given step size (dx, dy, dtheta)
    */
@@ -171,6 +178,11 @@ public:
    */
   Eigen::Vector3d box_clip(Eigen::Vector3d step);
 
+    /**
+   * @brief Applies the conic clipping to a given step size (dx, dy, dtheta)
+   */
+  Eigen::Vector3d conic_clip(Eigen::Vector3d step);
+
   /**
    * @brief Clips a step using ellipsoid and overlap avoidance
    */
@@ -181,6 +193,11 @@ public:
    */
   Eigen::Vector3d box_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step);
 
+  /**
+   * @brief Clips a step using ellipsoid and overlap avoidance
+   */
+  Eigen::Vector3d conic_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step);
+
   /**
    * @brief Frames for opposite and neutral positions
    */
@@ -194,6 +211,6 @@ protected:
 /**
    * @brief Clips a step using ellipsoid and overlap avoidance
    */
-  Eigen::Vector3d overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step, bool ellipsoid = false);
+  Eigen::Vector3d overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step, FootstepClipping clipping);
 };
 }  // namespace placo::humanoid