placo 0.8.9__tar.gz → 0.9.0__tar.gz

{placo-0.8.9 → placo-0.9.0}/.github/workflows/wheels.yml

@@ -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.9 → placo-0.9.0}/CMakeLists.txt

@@ -89,6 +89,8 @@ add_library(libplaco SHARED
     src/placo/kinematics/com_polygon_constraint.cpp
     src/placo/kinematics/joint_space_half_spaces_constraint.cpp
     src/placo/kinematics/cone_constraint.cpp
+    src/placo/kinematics/yaw_constraint.cpp
+    src/placo/kinematics/distance_constraint.cpp
     src/placo/kinematics/axis_align_task.cpp
 
     # Dynamics QP solver

{placo-0.8.9 → placo-0.9.0}/PKG-INFO

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

{placo-0.8.9 → placo-0.9.0}/bindings/expose-kinematics.cpp

@@ -101,6 +101,14 @@ void exposeKinematics()
           .def<ConeConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
               "add_cone_constraint", &KinematicsSolver::add_cone_constraint, return_internal_reference<>())
 
+          // Yaw constraint
+          .def<YawConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
+              "add_yaw_constraint", &KinematicsSolver::add_yaw_constraint, return_internal_reference<>())
+
+          // Distance constraint
+          .def<DistanceConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
+              "add_distance_constraint", &KinematicsSolver::add_distance_constraint, return_internal_reference<>())
+
           // Masking/unmasking DoFs
           .def("mask_dof", &KinematicsSolver::mask_dof)
           .def("unmask_dof", &KinematicsSolver::unmask_dof)
@@ -246,4 +254,12 @@ void exposeKinematics()
       .def_readwrite("angle_max", &ConeConstraint::angle_max)
       .def_readwrite("N", &ConeConstraint::N)
       .def_readwrite("range", &ConeConstraint::range);
+
+  class__<YawConstraint, bases<Constraint>>(
+      "YawConstraint", init<model::RobotWrapper::FrameIndex, model::RobotWrapper::FrameIndex, double>())
+      .def_readwrite("angle_max", &YawConstraint::angle_max);
+
+  class__<DistanceConstraint, bases<Constraint>>(
+      "DistanceConstraint", init<model::RobotWrapper::FrameIndex, model::RobotWrapper::FrameIndex, double>())
+      .def_readwrite("distance_max", &DistanceConstraint::distance_max);
 }

{placo-0.8.9 → placo-0.9.0}/pyproject.toml

@@ -23,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.8.9"
+version = "0.9.0"
 
 [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.9 → placo-0.9.0}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -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);
@@ -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,11 +367,13 @@ 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));
   }
 

{placo-0.8.9 → placo-0.9.0}/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.9.0/src/placo/kinematics/distance_constraint.cpp

@@ -0,0 +1,41 @@
+#include "placo/kinematics/distance_constraint.h"
+#include "placo/kinematics/kinematics_solver.h"
+#include "placo/problem/polygon_constraint.h"
+
+namespace placo::kinematics
+{
+DistanceConstraint::DistanceConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                                       double distance_max)
+  : frame_a(frame_a), frame_b(frame_b), distance_max(distance_max)
+{
+}
+
+void DistanceConstraint::add_constraint(placo::problem::Problem& problem)
+{
+  auto T_world_a = solver->robot.get_T_world_frame(frame_a);
+  auto T_world_b = solver->robot.get_T_world_frame(frame_b);
+
+  Eigen::Vector3d ab_world = T_world_b.translation() - T_world_a.translation();
+
+  double distance = ab_world.norm();
+  Eigen::Vector3d direction = ab_world.normalized();
+
+  Eigen::MatrixXd J_a = solver->robot.frame_jacobian(frame_a, pinocchio::LOCAL_WORLD_ALIGNED);
+  Eigen::MatrixXd J_b = solver->robot.frame_jacobian(frame_b, pinocchio::LOCAL_WORLD_ALIGNED);
+  Eigen::MatrixXd J_distance = direction.transpose() * (J_b - J_a).block(0, 0, 3, solver->N);
+
+  // Expression for the angle
+  problem::Expression e;
+  e.A.resize(1, solver->N);
+  e.b.resize(1);
+
+  e.A.row(0) = J_distance;
+  e.b(0) = distance;
+
+  problem.add_constraint(e <= distance_max)
+      .configure(priority == Prioritized::Priority::Hard ? problem::ProblemConstraint::Hard :
+                                                           problem::ProblemConstraint::Soft,
+                 weight);
+}
+
+}  // namespace placo::kinematics

placo-0.9.0/src/placo/kinematics/distance_constraint.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <vector>
+#include "placo/problem/problem.h"
+#include "placo/kinematics/constraint.h"
+
+namespace placo::kinematics
+{
+class KinematicsSolver;
+
+/**
+ * @brief Constraints the distance betweek two points in the robot
+ */
+class DistanceConstraint : public Constraint
+{
+public:
+  /**
+   * @brief Constraints the distance betweek two points in the robot
+   * @param frame_a
+   * @param frame_b
+   * @param distance_max
+   */
+  DistanceConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                     double distance_max);
+
+  /**
+   * @brief Frame A
+   */
+  model::RobotWrapper::FrameIndex frame_a;
+
+  /**
+   * @brief Frame B
+   */
+  model::RobotWrapper::FrameIndex frame_b;
+
+  /**
+   * @brief Maximum distance allowed between frame A and frame B
+   */
+  double distance_max;
+
+  virtual void add_constraint(placo::problem::Problem& problem) override;
+};
+}  // namespace placo::kinematics

{placo-0.8.9 → placo-0.9.0}/src/placo/kinematics/kinematics_solver.cpp

@@ -226,6 +226,30 @@ ConeConstraint& KinematicsSolver::add_cone_constraint(std::string frame_a, std::
   return add_cone_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), angle_max);
 }
 
+YawConstraint& KinematicsSolver::add_yaw_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                                    model::RobotWrapper::FrameIndex frame_b, double angle_max)
+{
+  return add_constraint(new YawConstraint(frame_a, frame_b, angle_max));
+}
+
+YawConstraint& KinematicsSolver::add_yaw_constraint(std::string frame_a, std::string frame_b, double angle_max)
+{
+  return add_yaw_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), angle_max);
+}
+
+DistanceConstraint& KinematicsSolver::add_distance_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                                              model::RobotWrapper::FrameIndex frame_b,
+                                                              double distance_max)
+{
+  return add_constraint(new DistanceConstraint(frame_a, frame_b, distance_max));
+}
+
+DistanceConstraint& KinematicsSolver::add_distance_constraint(std::string frame_a, std::string frame_b,
+                                                              double distance_max)
+{
+  return add_distance_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), distance_max);
+}
+
 void KinematicsSolver::mask_dof(std::string dof)
 {
   masked_dof.insert(robot.get_joint_v_offset(dof));

{placo-0.8.9 → placo-0.9.0}/src/placo/kinematics/kinematics_solver.h

@@ -30,6 +30,8 @@
 #include "placo/kinematics/com_polygon_constraint.h"
 #include "placo/kinematics/joint_space_half_spaces_constraint.h"
 #include "placo/kinematics/cone_constraint.h"
+#include "placo/kinematics/yaw_constraint.h"
+#include "placo/kinematics/distance_constraint.h"
 
 // Problem formulation
 #include "placo/problem/problem.h"
@@ -300,7 +302,6 @@ public:
    * @param frame_a frame A
    * @param frame_b frame B
    * @param alpha_max alpha max (in radians) between the frame z-axis and the cone frame zt-axis
-   * @param T_world_cone cone frame
    * @return constraint
    * @pyignore
    */
@@ -312,11 +313,50 @@ public:
    * @param frame_a frame A
    * @param frame_b frame B
    * @param alpha_max alpha max (in radians) between the frame z-axis and the cone frame zt-axis
-   * @param T_world_cone cone frame
    * @return constraint
    */
   ConeConstraint& add_cone_constraint(std::string frame_a, std::string frame_b, double alpha_max);
 
+  /**
+   * @brief Adds a yaw constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param alpha_max angle max for yaw of x-axis in frame b in a
+   * @return constraint
+   * @pyignore
+   */
+  YawConstraint& add_yaw_constraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                                    double alpha_max);
+
+  /**
+   * @brief Adds a yaw constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param alpha_max angle max for yaw of x-axis in frame b in a
+   * @return constraint
+   */
+  YawConstraint& add_yaw_constraint(std::string frame_a, std::string frame_b, double alpha_max);
+
+  /**
+   * @brief Adds a distance constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param distance_max maximum distance between the two frames
+   * @return constraint
+   * @pyignore
+   */
+  DistanceConstraint& add_distance_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                              model::RobotWrapper::FrameIndex frame_b, double distance_max);
+
+  /**
+   * @brief Adds a distance constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param distance_max maximum distance between the two frames
+   * @return constraint
+   */
+  DistanceConstraint& add_distance_constraint(std::string frame_a, std::string frame_b, double distance_max);
+
   /**
    * @brief Constructs the QP problem and solves it
    * @param apply apply the solution to the robot model

placo-0.9.0/src/placo/kinematics/yaw_constraint.cpp

@@ -0,0 +1,52 @@
+#include "placo/kinematics/yaw_constraint.h"
+#include "placo/kinematics/kinematics_solver.h"
+#include "placo/problem/polygon_constraint.h"
+
+namespace placo::kinematics
+{
+YawConstraint::YawConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                             double angle_max)
+  : frame_a(frame_a), frame_b(frame_b), angle_max(angle_max)
+{
+}
+
+void YawConstraint::add_constraint(placo::problem::Problem& problem)
+{
+  Eigen::Affine3d T_a_b = solver->robot.get_T_a_b(frame_a, frame_b);
+
+  // Jacobian of the rotational velocity expressed in a
+  Eigen::MatrixXd J_relative =
+      T_a_b.linear() *
+          solver->robot.frame_jacobian(frame_b, pinocchio::ReferenceFrame::LOCAL).block(3, 0, 3, solver->N) -
+      solver->robot.frame_jacobian(frame_a, pinocchio::ReferenceFrame::LOCAL).block(3, 0, 3, solver->N);
+  ;
+
+  // B's x axis in A
+  Eigen::Vector3d x_axis = T_a_b.linear().block(0, 0, 3, 1);
+
+  // Current angle
+  double alpha = atan2(x_axis.y(), x_axis.x());
+
+  Eigen::Vector3d perp_axis = Eigen::Vector3d::UnitZ().cross(x_axis).normalized();
+  Eigen::Vector3d yaw_axis = x_axis.cross(perp_axis).normalized();
+
+  Eigen::MatrixXd J_angle = yaw_axis.transpose() * J_relative;
+
+  // Expression for the angle
+  problem::Expression e;
+  e.A.resize(2, solver->N);
+  e.b.resize(2);
+  // First row is angle + J_angle*qd
+  e.A.block(0, 0, 1, solver->N) = J_angle;
+  e.b(0, 0) = alpha;
+  // Second row is -(angle -J_angle*qd)
+  e.A.block(1, 0, 1, solver->N) = -J_angle;
+  e.b(1, 0) = -alpha;
+
+  problem.add_constraint(e <= angle_max)
+      .configure(priority == Prioritized::Priority::Hard ? problem::ProblemConstraint::Hard :
+                                                           problem::ProblemConstraint::Soft,
+                 weight);
+}
+
+}  // namespace placo::kinematics

placo-0.9.0/src/placo/kinematics/yaw_constraint.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <vector>
+#include "placo/problem/problem.h"
+#include "placo/kinematics/constraint.h"
+
+namespace placo::kinematics
+{
+class KinematicsSolver;
+
+/**
+ * @brief A yaw constraint is a constraint where the x-axis of frame b should have a yaw withing +- angle_max in frame a
+ */
+class YawConstraint : public Constraint
+{
+public:
+  /**
+   * @brief Constrains the yaw of frame b in frame a, such that the x-axis of frame b should remain with +- angle_max
+   * @param frame_a
+   * @param frame_b
+   * @param angle_max
+   */
+  YawConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b, double angle_max);
+
+  /**
+   * @brief Frame A
+   */
+  model::RobotWrapper::FrameIndex frame_a;
+
+  /**
+   * @brief Frame B
+   */
+  model::RobotWrapper::FrameIndex frame_b;
+
+  /**
+   * @brief Maximum angle allowable by the yaw constraint (yaw is taken for x-axis of b around z-axis in a)
+   */
+  double angle_max;
+
+  virtual void add_constraint(placo::problem::Problem& problem) override;
+};
+}  // namespace placo::kinematics