placo 0.9.11__tar.gz → 0.9.13__tar.gz

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

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

{placo-0.9.11 → placo-0.9.13}/bindings/expose-footsteps.cpp

@@ -24,7 +24,9 @@ void exposeFootsteps()
       .add_property("side", &FootstepsPlanner::Footstep::side, &FootstepsPlanner::Footstep::side)
       .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)
+      .add_property(
+          "frame", +[](FootstepsPlanner::Footstep& footstep) { return footstep.frame; },
+          &FootstepsPlanner::Footstep::frame)
       .def(
           "set_frame_xy",
           +[](FootstepsPlanner::Footstep& footstep, double x, double y) {

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

@@ -49,12 +49,16 @@ void exposeParameters()
       .add_property("walk_dtheta_spacing", &HumanoidParameters::walk_dtheta_spacing,
                     &HumanoidParameters::walk_dtheta_spacing)
       .add_property("op_space_polygon", &HumanoidParameters::op_space_polygon, &HumanoidParameters::op_space_polygon)
-      .add_property("dcm_offset_polygon", &HumanoidParameters::dcm_offset_polygon, &HumanoidParameters::dcm_offset_polygon)
+      .add_property("dcm_offset_polygon", &HumanoidParameters::dcm_offset_polygon,
+                    &HumanoidParameters::dcm_offset_polygon)
       .def("dt", &HumanoidParameters::dt)
       .def("double_support_timesteps", &HumanoidParameters::double_support_timesteps)
       .def("startend_double_support_timesteps", &HumanoidParameters::startend_double_support_timesteps)
       .def("double_support_duration", &HumanoidParameters::double_support_duration)
       .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_clip", &HumanoidParameters::ellipsoid_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.11 → 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.11"
+version = "0.9.13"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.9.11 → 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.11 → 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.11 → placo-0.9.13}/src/placo/humanoid/humanoid_parameters.cpp

@@ -1,5 +1,7 @@
 #include <cmath>
 #include "placo/humanoid/humanoid_parameters.h"
+#include "placo/humanoid/footsteps_planner_repetitive.h"
+#include "placo/tools/utils.h"
 
 namespace placo::humanoid
 {
@@ -53,6 +55,62 @@ 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();
+  T_world_left.translate(Eigen::Vector3d(0, feet_spacing / 2., 0));
+  Eigen::Affine3d T_world_right = Eigen::Affine3d::Identity();
+  T_world_right.translate(Eigen::Vector3d(0, -feet_spacing / 2, 0));
+
+  auto footsteps =
+      planner.plan(support_side == HumanoidRobot::Side::Left ? HumanoidRobot::Side::Right : HumanoidRobot::Side::Left,
+                   T_world_left, T_world_right);
+
+  Eigen::Affine3d T_world_support = footsteps[1].frame;
+  Eigen::Affine3d T_world_target = footsteps[2].frame;
+  Eigen::Affine3d T_support_target = T_world_support.inverse() * T_world_target;
+
+  double offset = (support_side == HumanoidRobot::Side::Left) ? -feet_spacing : feet_spacing;
+
+  return Eigen::Vector3d(T_support_target.translation().x(), T_support_target.translation().y() - offset,
+                         tools::frame_yaw(T_support_target.rotation()));
+}
+
 Eigen::Affine3d HumanoidParameters::opposite_frame(HumanoidRobot::Side side, Eigen::Affine3d T_world_foot, double d_x,
                                                    double d_y, double d_theta)
 {

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

@@ -162,10 +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
    */
@@ -173,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