placo 0.9.12__tar.gz → 0.9.14__tar.gz

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

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

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

@@ -58,5 +58,9 @@ 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("conic_clip", &HumanoidParameters::conic_clip)
+      .def("ellipsoid_overlap_clip", &HumanoidParameters::ellipsoid_overlap_clip)
+      .def("box_overlap_clip", &HumanoidParameters::box_overlap_clip)
+      .def("conic_overlap_clip", &HumanoidParameters::conic_overlap_clip);
 }

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

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

@@ -319,7 +319,18 @@ FootstepsPlanner::Footstep FootstepsPlanner::clipped_opposite_footstep(Footstep
     step.y() += parameters.walk_dtheta_spacing * fabs(step.z());
   }
 
-  step = parameters.ellipsoid_clip(step);
+  if (footstep_clipping == HumanoidParameters::FootstepClipping::Conic)
+  {
+    step = parameters.conic_clip(step);
+  }
+  else if (footstep_clipping == HumanoidParameters::FootstepClipping::Ellipsoid)
+  {
+    step = parameters.ellipsoid_clip(step);
+  }
+  else if (footstep_clipping == HumanoidParameters::FootstepClipping::Box)
+  {
+    step = parameters.box_clip(step);
+  }
 
   for (int k = 0; k < 32; k++)
   {

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

@@ -149,6 +149,8 @@ public:
   // Humanoid parameters for planning and control
   HumanoidParameters& parameters;
 
+  HumanoidParameters::FootstepClipping footstep_clipping = HumanoidParameters::FootstepClipping::Conic;
+
 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.14}/src/placo/humanoid/humanoid_parameters.cpp

@@ -55,10 +55,67 @@ 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::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, FootstepClipping::Ellipsoid);
+}
+
+Eigen::Vector3d HumanoidParameters::box_overlap_clip(HumanoidRobot::Side support_side, Eigen::Vector3d step)
+{
+  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,
+                                                 HumanoidParameters::FootstepClipping clipping)
 {
   FootstepsPlannerRepetitive planner(*this);
   planner.configure(step.x(), step.y(), step.z(), 3);
+  planner.footstep_clipping = clipping;
 
   // Creatting footsteps
   Eigen::Affine3d T_world_left = Eigen::Affine3d::Identity();

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

@@ -161,16 +161,43 @@ public:
   // DCM offset bounds (polygon)
   std::vector<Eigen::Vector2d> dcm_offset_polygon;
 
+  enum FootstepClipping
+  {
+    Ellipsoid = 0,
+    Box = 1,
+    Conic = 2
+  };
+
   /**
-   * @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 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
    */
   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 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
    */
@@ -178,5 +205,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, FootstepClipping clipping);
 };
 }  // namespace placo::humanoid