placo 0.6.2__tar.gz → 0.6.3__tar.gz

{placo-0.6.2 → placo-0.6.3}/CMakeLists.txt

@@ -51,6 +51,7 @@ add_library(libplaco SHARED
     src/placo/tools/prioritized.cpp
     src/placo/tools/cubic_spline.cpp
     src/placo/tools/cubic_spline_3d.cpp
+    src/placo/tools/directions.cpp
 
     # Problem formulation
     src/placo/problem/problem.cpp

{placo-0.6.2 → placo-0.6.3}/PKG-INFO

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

{placo-0.6.2 → placo-0.6.3}/bindings/expose-tools.cpp

@@ -9,6 +9,7 @@
 #include "placo/tools/cubic_spline_3d.h"
 #include "placo/tools/axises_mask.h"
 #include "placo/tools/prioritized.h"
+#include "placo/tools/directions.h"
 #include "expose-utils.hpp"
 #ifdef HAVE_RHOBAN_UTILS
 #include "rhoban_utils/history/history.h"
@@ -24,6 +25,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(loadReplays_overloads, loadReplays, 1, 2)
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_axises_overloads, set_axises, 1, 2);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(configure_overloads, configure, 2, 3);
+BOOST_PYTHON_FUNCTION_OVERLOADS(directions_3d_overloads, directions_3d, 1, 2);
 
 void exposeTools()
 {
@@ -33,6 +35,8 @@ void exposeTools()
   def("frame_yaw", &frame_yaw);
   def("flatten_on_floor", &flatten_on_floor);
   def("optimal_transformation", &optimal_transformation);
+  def("directions_2d", &directions_2d);
+  def("directions_3d", &directions_3d, directions_3d_overloads());
 
   exposeStdVector<int>("vector_int");
   exposeStdVector<double>("vector_double");

{placo-0.6.2 → placo-0.6.3}/doxygen_parse.py

@@ -1,5 +1,6 @@
 import glob
 import re
+from typing import Union
 import xml.etree.ElementTree as ET
 
 # Mapping member definitions (id) to all informations
@@ -148,7 +149,7 @@ def parse_xml(xml_file: str):
             parse_compound(compounddef_node)
 
 
-def resolve_doxygen_id(id: list|str):
+def resolve_doxygen_id(id: Union[list,str]):
     if type(id) == list:
         tpl = resolve_doxygen_id(id[0])
         typ = resolve_doxygen_id(id[1])

{placo-0.6.2 → placo-0.6.3}/placo.pyi

@@ -7236,6 +7236,35 @@ class WheelTask:
     ...
 
 
+def directions_2d(
+  n: int, # int
+
+) -> numpy.ndarray:
+  """Generates a set of directions in 2D. 
+        
+
+  :param n: the number of directions 
+
+  :return: matrix of size n x 2"""
+  ...
+
+
+def directions_3d(
+  n: int, # int
+  epsilon: float = 0.5, # double
+
+) -> numpy.ndarray:
+  """Generates a set of directions in 3D, using Fibonacci lattice. 
+        
+
+  :param n: the number of directions 
+
+  :param epsilon: the epsilon parameter for the Fibonacci lattice 
+
+  :return: matrix of size n x 3"""
+  ...
+
+
 def flatten_on_floor(
   transformation: numpy.ndarray, # const Eigen::Affine3d &
 

{placo-0.6.2 → placo-0.6.3}/pyproject.toml

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

{placo-0.6.2 → placo-0.6.3}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -342,8 +342,7 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
     for (int timestep = 1; timestep < kept_timesteps; timestep++)
     {
       Eigen::Vector2d jerk = old_trajectory->get_j_world_CoM(trajectory.t_start + timestep * parameters.dt()).head(2);
-      problem.add_constraint(lipm.jerk(timestep) == jerk)
-          .configure(ProblemConstraint::Soft, 1e-4);
+      problem.add_constraint(lipm.jerk(timestep) == jerk).configure(ProblemConstraint::Soft, 1e-4);
     }
   }
 
@@ -355,7 +354,8 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
     current_support = trajectory.supports[i];
     int step_timesteps = support_timesteps(current_support);
 
-    for (int timestep = constrained_timesteps; timestep < fmin(timesteps, constrained_timesteps + step_timesteps); timestep++)
+    for (int timestep = constrained_timesteps; timestep < fmin(timesteps, constrained_timesteps + step_timesteps);
+         timestep++)
     {
       // Ensuring ZMP remains in the support polygon
       if (timestep > kept_timesteps)
@@ -363,7 +363,7 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
         problem.add_constraint(PolygonConstraint::in_polygon_xy(
             lipm.zmp(timestep, omega_2), current_support.support_polygon(), parameters.zmp_margin));
       }
-      
+
       // ZMP reference trajectory : aiming for the center of single supports
       if (!current_support.is_both() || current_support.start || current_support.end)
       {
@@ -509,7 +509,10 @@ void WalkPatternGenerator::planSingleSupportTrajectory(TrajectoryPart& part, Tra
   trajectory.yaw(flying_side).add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
 
   // The trunk orientation follow the steps orientation
-  trajectory.trunk_yaw.add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
+  if (!parameters.has_double_support())
+  {
+    trajectory.trunk_yaw.add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
+  }
 
   // Support foot remaining steady
   trajectory.add_supports(t, part.support);
@@ -522,7 +525,14 @@ void WalkPatternGenerator::planFeetTrajectories(Trajectory& trajectory, Trajecto
   // Add the initial position to the trajectory
   trajectory.add_supports(t, trajectory.supports[0]);
 
-  trajectory.trunk_yaw.add_point(t, frame_yaw(trajectory.supports[0].frame().rotation()), 0);
+  if (old_trajectory == nullptr)
+  {
+    trajectory.trunk_yaw.add_point(t, frame_yaw(trajectory.supports[0].frame().rotation()), 0);
+  }
+  else
+  {
+    trajectory.trunk_yaw.add_point(t, old_trajectory->trunk_yaw.pos(t), old_trajectory->trunk_yaw.vel(t));
+  }
 
   if (!trajectory.supports[0].is_both())
   {
@@ -556,7 +566,6 @@ void WalkPatternGenerator::planFeetTrajectories(Trajectory& trajectory, Trajecto
         planSingleSupportTrajectory(part, trajectory, step, t, old_trajectory, t_replan);
       }
     }
-
     // Double support
     else
     {
@@ -625,7 +634,7 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::replan(std::vector<Footst
 bool WalkPatternGenerator::can_replan_supports(Trajectory& trajectory, double t_replan)
 {
   // We can't replan from an "end", a "start" or a "kick"
-  if (trajectory.get_support(t_replan).end || trajectory.get_support(t_replan).start || 
+  if (trajectory.get_support(t_replan).end || trajectory.get_support(t_replan).start ||
       trajectory.get_next_support(t_replan).end || trajectory.get_support(t_replan).kick())
   {
     return false;
@@ -673,7 +682,7 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::replan_supports(Foo
 
   std::vector<FootstepsPlanner::Support> supports;
   supports = FootstepsPlanner::make_supports(footsteps, false, parameters.has_double_support(), true);
-  
+
   if (current_support.is_both())
   {
     supports.erase(supports.begin());

{placo-0.6.2 → placo-0.6.3}/src/placo/humanoid/walk_pattern_generator.h

@@ -65,18 +65,18 @@ public:
 
     /**
      * @brief Returns the support corresponding to the given time in the trajectory
-    */
+     */
     FootstepsPlanner::Support get_support(double t);
 
     /**
      * @brief Returns the nth next support corresponding to the given time in the trajectory
      */
-    FootstepsPlanner::Support get_next_support(double t, int n=1);
+    FootstepsPlanner::Support get_next_support(double t, int n = 1);
 
     /**
      * @brief Returns the nth previous support corresponding to the given time in the trajectory
      */
-    FootstepsPlanner::Support get_prev_support(double t, int n=1);
+    FootstepsPlanner::Support get_prev_support(double t, int n = 1);
 
     std::vector<FootstepsPlanner::Support> get_supports();
     int remaining_supports(double t);

placo-0.6.3/src/placo/tools/directions.cpp

@@ -0,0 +1,42 @@
+#include "directions.h"
+
+namespace placo::tools
+{
+Eigen::MatrixXd directions_2d(int n)
+{
+  Eigen::MatrixXd directions(n, 2);
+
+  for (int i = 0; i < n; i++)
+  {
+    double angle = 2 * M_PI * i / n;
+    directions(i, 0) = cos(angle);
+    directions(i, 1) = sin(angle);
+  }
+
+  return directions;
+}
+
+Eigen::MatrixXd directions_3d(int n, double epsilon)
+{
+  Eigen::MatrixXd directions(n, 3);
+
+  // Golden ratio
+  double phi = (1 + sqrt(5)) / 2;
+
+  for (int i = 0; i < n; i++)
+  {
+    double x = fmod(i / phi, 1);
+    double y = (i + epsilon) / (n - 1 + 2 * epsilon);
+
+    double alpha = 2 * M_PI * x;
+    double beta = acos(1 - 2 * y);
+
+    directions(i, 0) = sin(beta) * cos(alpha);
+    directions(i, 1) = sin(beta) * sin(alpha);
+    directions(i, 2) = cos(beta);
+  }
+
+  return directions;
+}
+
+}  // namespace placo::tools

placo-0.6.3/src/placo/tools/directions.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+
+namespace placo::tools
+{
+/**
+ * @brief Generates a set of directions in 2D
+ * @param n the number of directions
+ * @return matrix of size n x 2
+ */
+Eigen::MatrixXd directions_2d(int n);
+
+/**
+ * @brief Generates a set of directions in 3D, using Fibonacci lattice
+ * @param n the number of directions
+ * @param epsilon the epsilon parameter for the Fibonacci lattice
+ * @return matrix of size n x 3
+ */
+Eigen::MatrixXd directions_3d(int n, double epsilon = 0.5);
+}  // namespace placo::tools