placo 0.8.8__tar.gz → 0.8.10__tar.gz

{placo-0.8.8 → placo-0.8.10}/.github/workflows/wheels.yml

@@ -6,7 +6,7 @@ on:
       - published
   push:
     branches:
-      - '**'               # Sur tout push, toutes branches
+      - 'master'               # Sur tout push, toutes branches
 
 env:
   CIBW_SKIP: "*i686 *musl* *armv7* *ppc64* *s390*"
@@ -28,6 +28,8 @@ jobs:
 
       # Used to host cibuildwheel
       - uses: actions/setup-python@v5
+        with:
+          python-version: "3.12"
 
       - name: Install cibuildwheel
         run: python -m pip install cibuildwheel==3.0.0b1

{placo-0.8.8 → placo-0.8.10}/PKG-INFO

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

{placo-0.8.8 → placo-0.8.10}/bindings/expose-tools.cpp

@@ -55,7 +55,7 @@ void exposeTools()
                                                            set_axises_overloads())
       .def_readwrite("R_local_world", &AxisesMask::R_local_world)
       .def_readwrite("R_custom_world", &AxisesMask::R_custom_world)
-      .def("apply", &AxisesMask::apply);  
+      .def("apply", &AxisesMask::apply);
 
   class__<Prioritized, boost::noncopyable>("Prioritized", no_init)
       .add_property("name", &Prioritized::name)
@@ -140,7 +140,7 @@ void exposeTools()
           +[](HistoryCollection& collection, std::vector<std::string> entries, double start_t, double dt, int length) {
             Eigen::MatrixXd result(length, entries.size());
 
-            for (unsigned int i = 0; i < length; i++)
+            for (int i = 0; i < length; i++)
             {
               double t = start_t + i * dt;
               for (unsigned int j = 0; j < entries.size(); j++)

{placo-0.8.8 → placo-0.8.10}/pyproject.toml

@@ -23,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.8.8"
+version = "0.8.10"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"
@@ -44,7 +44,7 @@ run-tests = false
 
 
 [tool.cibuildwheel.macos]
-before-all = "brew install doxygen"
+before-all = "brew install --formula doxygen"
 
 [tool.cibuildwheel.linux]
 before-all = "yum install -y doxygen"

{placo-0.8.8 → placo-0.8.10}/src/placo/humanoid/footsteps_planner.cpp

@@ -98,12 +98,11 @@ bool FootstepsPlanner::Footstep::operator==(const Footstep& other)
   return side == other.side && frame.isApprox(other.frame);
 }
 
-FootstepsPlanner::Support::Support() 
+FootstepsPlanner::Support::Support()
 {
 }
 
-FootstepsPlanner::Support::Support(std::vector<Footstep> footsteps) 
-  : footsteps(footsteps)
+FootstepsPlanner::Support::Support(std::vector<Footstep> footsteps) : footsteps(footsteps)
 {
 }
 
@@ -216,7 +215,7 @@ bool FootstepsPlanner::Support::operator==(const Support& other)
   {
     return false;
   }
-  for (int k = 0; k < footsteps.size(); k++)
+  for (int k = 0; k < (int)footsteps.size(); k++)
   {
     if (!(footsteps[k] == other.footsteps[k]))
     {
@@ -231,8 +230,8 @@ FootstepsPlanner::FootstepsPlanner(HumanoidParameters& parameters) : parameters(
 {
 }
 
-std::vector<FootstepsPlanner::Support>
-FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footsteps, double t_start, bool start, bool middle, bool end)
+std::vector<FootstepsPlanner::Support> FootstepsPlanner::make_supports(
+    std::vector<FootstepsPlanner::Footstep> footsteps, double t_start, bool start, bool middle, bool end)
 {
   std::vector<Support> supports;
 
@@ -241,36 +240,36 @@ FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footstep
     if (start)
     {
       // Creating the first (double-support) initial state
-      Support support({footsteps[0], footsteps[1]});
+      Support support({ footsteps[0], footsteps[1] });
       support.start = true;
       support.t_start = t_start;
       supports.push_back(support);
     }
     else
     {
-      Support support({footsteps[0]});
+      Support support({ footsteps[0] });
       support.t_start = t_start;
       supports.push_back(support);
 
       if (middle)
       {
-        Support double_support({footsteps[0], footsteps[1]});
+        Support double_support({ footsteps[0], footsteps[1] });
         double_support.t_start = t_start;
         supports.push_back(double_support);
       }
     }
 
     // Adding single/double support phases
-    for (int step = 1; step < footsteps.size() - 1; step++)
+    for (int step = 1; step < (int)footsteps.size() - 1; step++)
     {
-      Support single_support({footsteps[step]});
+      Support single_support({ footsteps[step] });
       supports.push_back(single_support);
 
-      bool is_end = (step == footsteps.size() - 2);
+      bool is_end = (step == (int)footsteps.size() - 2);
 
       if ((!is_end && middle))
       {
-        Support double_support({footsteps[step], footsteps[step + 1]});
+        Support double_support({ footsteps[step], footsteps[step + 1] });
         supports.push_back(double_support);
       }
     }
@@ -279,7 +278,7 @@ FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footstep
   if (end)
   {
     // Creating the first (double-support) initial state
-    Support support({footsteps[footsteps.size() - 2], footsteps[footsteps.size() - 1]});
+    Support support({ footsteps[footsteps.size() - 2], footsteps[footsteps.size() - 1] });
     support.end = true;
     supports.push_back(support);
   }

{placo-0.8.8 → placo-0.8.10}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -10,7 +10,7 @@ using namespace placo::problem;
 using namespace placo::tools;
 
 WalkPatternGenerator::TrajectoryPart::TrajectoryPart(FootstepsPlanner::Support support, double t_start)
-  : support(support), t_start(t_start)
+  : t_start(t_start), support(support)
 {
 }
 
@@ -20,13 +20,13 @@ WalkPatternGenerator::Trajectory::Trajectory() : left_foot_yaw(true), right_foot
 }
 
 WalkPatternGenerator::Trajectory::Trajectory(double com_target_z, double t_start, double trunk_pitch, double trunk_roll)
-  : left_foot_yaw(true)
-  , right_foot_yaw(true)
-  , trunk_yaw(true)
+  : t_start(t_start)
   , com_target_z(com_target_z)
-  , t_start(t_start)
   , trunk_pitch(trunk_pitch)
   , trunk_roll(trunk_roll)
+  , left_foot_yaw(true)
+  , right_foot_yaw(true)
+  , trunk_yaw(true)
 {
   T.setIdentity();
 }
@@ -146,6 +146,26 @@ Eigen::Vector3d WalkPatternGenerator::Trajectory::get_v_world_foot(HumanoidRobot
   return (side == HumanoidRobot::Left) ? get_v_world_left(t) : get_v_world_right(t);
 }
 
+double WalkPatternGenerator::Trajectory::get_yaw_world_left(double t)
+{
+  return frame_yaw(T.rotation()) + left_foot_yaw.pos(t);
+}
+
+double WalkPatternGenerator::Trajectory::get_yaw_world_right(double t)
+{
+  return frame_yaw(T.rotation()) + right_foot_yaw.pos(t);
+}
+
+double WalkPatternGenerator::Trajectory::get_yaw_world_foot(HumanoidRobot::Side side, double t)
+{
+  return (side == HumanoidRobot::Left) ? get_yaw_world_left(t) : get_yaw_world_right(t);
+}
+
+double WalkPatternGenerator::Trajectory::get_yaw_world_trunk(double t)
+{
+  return frame_yaw(T.rotation()) + trunk_yaw.pos(t);
+}
+
 Eigen::Vector3d WalkPatternGenerator::Trajectory::get_p_world_CoM(double t)
 {
   TrajectoryPart& part = _findPart(parts, t);
@@ -232,7 +252,7 @@ FootstepsPlanner::Support WalkPatternGenerator::Trajectory::get_next_support(dou
 {
   int index;
   _findPart(parts, t, &index);
-  if (index + n >= parts.size())
+  if (index + n >= (int)parts.size())
   {
     return T * parts.back().support;
   }
@@ -316,8 +336,10 @@ void WalkPatternGenerator::plan_sgl_support(Trajectory& trajectory, int part_ind
         part.t_start, virt_duration, parameters.walk_foot_height, parameters.walk_foot_rise_ratio, start,
         T_world_end.translation(), part.support.elapsed_ratio, start_vel);
 
-    double replan_yaw = old_trajectory->foot_yaw(flying_side).pos(trajectory.t_start);
-    trajectory.foot_yaw(flying_side).add_point(trajectory.t_start, replan_yaw, 0);
+    // Initiate the flying foot yaw spline with the old trajectory foot yaw position and velocity
+    trajectory.foot_yaw(flying_side)
+        .add_point(trajectory.t_start, old_trajectory->get_yaw_world_foot(flying_side, trajectory.t_start),
+                   old_trajectory->foot_yaw(flying_side).vel(trajectory.t_start));
   }
   else
   {
@@ -345,15 +367,17 @@ void WalkPatternGenerator::plan_feet_trajectories(Trajectory& trajectory, Trajec
   trajectory.add_supports(trajectory.t_start, trajectory.parts[0].support);
   if (old_trajectory == nullptr)
   {
+    // Starting the walk in double support, we set the trunk yaw aligned with the first support
     trajectory.trunk_yaw.add_point(trajectory.t_start, frame_yaw(trajectory.parts[0].support.frame().rotation()), 0);
   }
   else
   {
-    trajectory.trunk_yaw.add_point(trajectory.t_start, old_trajectory->trunk_yaw.pos(trajectory.t_start),
+    // Initiate the trunk_yaw spline with the old trajectory trunk_yaw position and velocity
+    trajectory.trunk_yaw.add_point(trajectory.t_start, old_trajectory->get_yaw_world_trunk(trajectory.t_start),
                                    old_trajectory->trunk_yaw.vel(trajectory.t_start));
   }
 
-  for (int i; i < trajectory.parts.size(); i++)
+  for (int i = 0; i < (int)trajectory.parts.size(); i++)
   {
     // Single support
     if (trajectory.parts[i].support.footsteps.size() == 1)
@@ -454,7 +478,7 @@ void WalkPatternGenerator::plan_com(Trajectory& trajectory, std::vector<Footstep
   // Solving the problem
   problem.solve();
 
-  for (int i = 0; i < trajectory.parts.size(); i++)
+  for (int i = 0; i < (int)trajectory.parts.size(); i++)
   {
     auto& part = trajectory.parts[i];
     part.com_trajectory = lipms[i].get_trajectory();
@@ -621,7 +645,7 @@ double WalkPatternGenerator::support_default_duration(FootstepsPlanner::Support&
 
 void WalkPatternGenerator::Trajectory::print_parts_timings()
 {
-  for (int i = 0; i < parts.size(); i++)
+  for (int i = 0; i < (int)parts.size(); i++)
   {
     std::cout << "Part " << i << " : start at " << parts[i].t_start << ", end at " << parts[i].t_end << std::endl;
   }

{placo-0.8.8 → placo-0.8.10}/src/placo/humanoid/walk_pattern_generator.h

@@ -63,6 +63,11 @@ public:
     Eigen::Vector3d get_v_world_right(double t);
     Eigen::Vector3d get_v_world_foot(HumanoidRobot::Side side, double t);
 
+    double get_yaw_world_left(double t);
+    double get_yaw_world_right(double t);
+    double get_yaw_world_foot(HumanoidRobot::Side side, double t);
+    double get_yaw_world_trunk(double t);
+
     Eigen::Vector3d get_p_world_CoM(double t);
     Eigen::Vector3d get_v_world_CoM(double t);
     Eigen::Vector3d get_a_world_CoM(double t);
@@ -172,36 +177,38 @@ public:
   /**
    * @brief Replans the supports for a given trajectory given a footsteps planner.
    */
-  std::vector<FootstepsPlanner::Support> replan_supports(FootstepsPlanner& planner, Trajectory& trajectory, double t_replan, double t_last_replan);
+  std::vector<FootstepsPlanner::Support> replan_supports(FootstepsPlanner& planner, Trajectory& trajectory,
+                                                         double t_replan, double t_last_replan);
 
   double last_com_planning_duration = 0.;
   double last_feet_planning_duration = 0.;
 
   /**
-   * @brief Updates the supports to ensure DCM viability by adjusting the 
+   * @brief Updates the supports to ensure DCM viability by adjusting the
    * duration and the target of the current swing trajectory.
    * @param t Current time
    * @param supports Planned supports
    * @param world_target_zmp Target ZMP for the flying foot in world frame
    * @param world_measured_dcm Measured DCM in world frame
    */
-  std::vector<FootstepsPlanner::Support> update_supports(double t, std::vector<FootstepsPlanner::Support> supports, 
-    Eigen::Vector2d world_target_zmp, Eigen::Vector2d world_measured_dcm);
+  std::vector<FootstepsPlanner::Support> update_supports(double t, std::vector<FootstepsPlanner::Support> supports,
+                                                         Eigen::Vector2d world_target_zmp,
+                                                         Eigen::Vector2d world_measured_dcm);
 
   /**
-   * @brief Computes the best ZMP in the support polygon to move de DCM from 
+   * @brief Computes the best ZMP in the support polygon to move de DCM from
    * world_dcm_start to world_dcm_end in duration.
    * @param world_dcm_start Initial DCM position in world frame
    * @param world_dcm_end Desired final DCM position in world frame
    * @param duration Duration
    * @param support Support
    */
-  Eigen::Vector2d get_optimal_zmp(Eigen::Vector2d world_dcm_start, Eigen::Vector2d world_dcm_end, 
-    double duration, FootstepsPlanner::Support& support);
+  Eigen::Vector2d get_optimal_zmp(Eigen::Vector2d world_dcm_start, Eigen::Vector2d world_dcm_end, double duration,
+                                  FootstepsPlanner::Support& support);
 
   int support_default_timesteps(FootstepsPlanner::Support& support);
   double support_default_duration(FootstepsPlanner::Support& support);
-  
+
   bool soft = false;
 
 protected:
@@ -214,10 +221,12 @@ protected:
   double omega;
   double omega_2;
 
-  void constrain_lipm(problem::Problem& problem, LIPM& lipm, FootstepsPlanner::Support& support, double omega_2, HumanoidParameters& parameters);
+  void constrain_lipm(problem::Problem& problem, LIPM& lipm, FootstepsPlanner::Support& support, double omega_2,
+                      HumanoidParameters& parameters);
 
-  void plan_com(Trajectory& trajectory, std::vector<FootstepsPlanner::Support>& supports, Eigen::Vector2d initial_pos, 
-    Eigen::Vector2d initial_vel = Eigen::Vector2d::Zero(), Eigen::Vector2d initial_acc = Eigen::Vector2d::Zero());
+  void plan_com(Trajectory& trajectory, std::vector<FootstepsPlanner::Support>& supports, Eigen::Vector2d initial_pos,
+                Eigen::Vector2d initial_vel = Eigen::Vector2d::Zero(),
+                Eigen::Vector2d initial_acc = Eigen::Vector2d::Zero());
 
   void plan_dbl_support(Trajectory& trajectory, int part_index);
   void plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory);

{placo-0.8.8 → placo-0.8.10}/src/placo/kinematics/manipulability_task.cpp

@@ -4,7 +4,7 @@
 namespace placo::kinematics
 {
 ManipulabilityTask::ManipulabilityTask(model::RobotWrapper::FrameIndex frame_index, Type type, double lambda)
-  : frame_index(frame_index), type(type), lambda(lambda)
+  : frame_index(frame_index), lambda(lambda), type(type)
 {
 }
 

{placo-0.8.8 → placo-0.8.10}/src/placo/problem/integrator.cpp

@@ -171,11 +171,6 @@ Expression Integrator::expr_t(double t, int diff)
 {
   t -= t_start;
 
-  if (t < 0 || t > variable->size() * dt)
-  {
-    throw std::runtime_error("expr_t called with a t out of the scope of the integrator");
-  }
-
   int step = std::max<int>(std::min<int>(variable->size() - 1, t / dt), 0);
 
   if (diff == order)
@@ -190,7 +185,7 @@ Expression Integrator::expr_t(double t, int diff)
     Eigen::MatrixXd Ar = AB.first;
     Eigen::MatrixXd Br = AB.second;
 
-    Expression e = (Ar * expr(step)) + Br * variable->expr(step);
+    Expression e = (Ar * expr(step)) + Br * variable->expr(step, 1);
 
     if (diff > -1)
     {