placo 0.8.10__tar.gz → 0.9.1__tar.gz

{placo-0.8.10 → placo-0.9.1}/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.10 → placo-0.9.1}/PKG-INFO

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

{placo-0.8.10 → placo-0.9.1}/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.10 → placo-0.9.1}/bindings/expose-robot-wrapper.cpp

@@ -43,10 +43,12 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("set_velocity_limits", &RobotType::set_velocity_limits)
       .def("set_torque_limit", &RobotType::set_torque_limit)
       .def("set_joint_limits", &RobotType::set_joint_limits)
-      .def("get_joint_limits", +[](RobotType& robot, const std::string& joint) {
-        auto limits = robot.get_joint_limits(joint);
-        return Eigen::Vector2d(limits.first, limits.second);
-      })
+      .def(
+          "get_joint_limits",
+          +[](RobotType& robot, const std::string& joint) {
+            auto limits = robot.get_joint_limits(joint);
+            return Eigen::Vector2d(limits.first, limits.second);
+          })
       .def("update_kinematics", &RobotType::update_kinematics)
       .def("compute_hessians", &RobotType::compute_hessians)
       .def(
@@ -57,7 +59,6 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("get_T_world_fbase", &RobotType::get_T_world_fbase)
       .def("set_T_world_fbase", &RobotType::set_T_world_fbase)
       .def("com_world", &RobotType::com_world)
-      .def("dcom_world", &RobotType::dcom_world)
       .def("centroidal_map", &RobotType::centroidal_map)
       .def("joint_names", &RobotType::joint_names, joint_names_overloads())
       .def("frame_names", &RobotType::frame_names)
@@ -180,14 +181,12 @@ void exposeRobotWrapper()
       .def<void (HumanoidRobot::*)(const std::string&)>("update_support_side", &HumanoidRobot::update_support_side)
       .def("ensure_on_floor", &HumanoidRobot::ensure_on_floor)
       .def("ensure_on_floor_oriented", &HumanoidRobot::ensure_on_floor_oriented)
+      .def("update_from_imu", &HumanoidRobot::update_from_imu)
       .def("get_T_world_left", &HumanoidRobot::get_T_world_left)
       .def("get_T_world_right", &HumanoidRobot::get_T_world_right)
       .def("get_T_world_trunk", &HumanoidRobot::get_T_world_trunk)
       .def("get_com_velocity", &HumanoidRobot::get_com_velocity)
-      .def("dcm", +[](HumanoidRobot& robot, double omega) { return robot.dcm(omega); })
-      .def("dcm_from_com_vel", +[](HumanoidRobot& robot, double omega, Eigen::Vector2d com_velocity) {
-        return robot.dcm(omega, com_velocity);
-      })
+      .def("dcm", &HumanoidRobot::dcm)
       .def("zmp", &HumanoidRobot::zmp)
       .def("other_side", &HumanoidRobot::other_side)
       .def(
@@ -213,10 +212,12 @@ void exposeRobotWrapper()
           "get_support_side", +[](const HumanoidRobot& robot) { return robot.support_side; })
       .add_property("support_is_both", &HumanoidRobot::support_is_both, &HumanoidRobot::support_is_both)
       .add_property("support_side", &HumanoidRobot::support_side)
-      .def("get_T_world_support", +[](const HumanoidRobot& robot) { return robot.T_world_support; })
-      .def("set_T_world_support", +[](HumanoidRobot& robot, const Eigen::Affine3d& T_world_support) {
-        robot.T_world_support = T_world_support;
-      });
+      .def(
+          "get_T_world_support", +[](const HumanoidRobot& robot) { return robot.T_world_support; })
+      .def(
+          "set_T_world_support", +[](HumanoidRobot& robot, const Eigen::Affine3d& T_world_support) {
+            robot.T_world_support = T_world_support;
+          });
 
   exposeStdVector<RobotWrapper::Collision>("vector_Collision");
   exposeStdVector<RobotWrapper::Distance>("vector_Distance");

{placo-0.8.10 → placo-0.9.1}/bindings/expose-walk-pattern-generator.cpp

@@ -23,9 +23,11 @@ using namespace placo::humanoid;
 void exposeWalkPatternGenerator()
 {
   class__<WalkPatternGenerator::TrajectoryPart>("WPGTrajectoryPart", init<FootstepsPlanner::Support, double>())
-      .add_property("t_start", &WalkPatternGenerator::TrajectoryPart::t_start, &WalkPatternGenerator::TrajectoryPart::t_start)
+      .add_property("t_start", &WalkPatternGenerator::TrajectoryPart::t_start,
+                    &WalkPatternGenerator::TrajectoryPart::t_start)
       .add_property("t_end", &WalkPatternGenerator::TrajectoryPart::t_end, &WalkPatternGenerator::TrajectoryPart::t_end)
-      .add_property("support", &WalkPatternGenerator::TrajectoryPart::support, &WalkPatternGenerator::TrajectoryPart::support);
+      .add_property("support", &WalkPatternGenerator::TrajectoryPart::support,
+                    &WalkPatternGenerator::TrajectoryPart::support);
 
   class__<WalkPatternGenerator::Trajectory>("WPGTrajectory", init<double, double, double, double>())
       .add_property("t_start", &WalkPatternGenerator::Trajectory::t_start)
@@ -46,6 +48,9 @@ void exposeWalkPatternGenerator()
       .def("get_p_world_ZMP", &WalkPatternGenerator::Trajectory::get_p_world_ZMP)
       .def("get_p_world_DCM", &WalkPatternGenerator::Trajectory::get_p_world_DCM)
       .def("get_R_world_trunk", &WalkPatternGenerator::Trajectory::get_R_world_trunk)
+      .def("get_p_support_CoM", &WalkPatternGenerator::Trajectory::get_p_support_CoM)
+      .def("get_v_support_CoM", &WalkPatternGenerator::Trajectory::get_v_support_CoM)
+      .def("get_p_support_DCM", &WalkPatternGenerator::Trajectory::get_p_support_DCM)
       .def("support_side", &WalkPatternGenerator::Trajectory::support_side)
       .def("support_is_both", &WalkPatternGenerator::Trajectory::support_is_both)
       .def("get_supports", &WalkPatternGenerator::Trajectory::get_supports)
@@ -67,7 +72,11 @@ void exposeWalkPatternGenerator()
       .def("get_optimal_zmp", &WalkPatternGenerator::get_optimal_zmp)
       .def("support_default_timesteps", &WalkPatternGenerator::support_default_timesteps)
       .def("support_default_duration", &WalkPatternGenerator::support_default_duration)
-      .add_property("soft", &WalkPatternGenerator::soft, &WalkPatternGenerator::soft);
+      .add_property("soft", &WalkPatternGenerator::soft, &WalkPatternGenerator::soft)
+      .add_property("zmp_in_support_weight", &WalkPatternGenerator::zmp_in_support_weight,
+                    &WalkPatternGenerator::zmp_in_support_weight)
+      .add_property("stop_end_support_weight", &WalkPatternGenerator::stop_end_support_weight,
+                    &WalkPatternGenerator::stop_end_support_weight);
 
   class__<SwingFoot>("SwingFoot", init<>())
       .def("make_trajectory", &SwingFoot::make_trajectory)
@@ -134,14 +143,12 @@ void exposeWalkPatternGenerator()
                     make_function(
                         +[](WalkTasks& tasks) -> OrientationTask& { return *tasks.trunk_orientation_task; },
                         return_value_policy<reference_existing_object>()))
-      .add_property("com_task",
-                    make_function(
-                        +[](WalkTasks& tasks) -> CoMTask& { return *tasks.com_task; },
-                        return_value_policy<reference_existing_object>()))
-      .add_property("trunk_task",
-                    make_function(
-                        +[](WalkTasks& tasks) -> PositionTask& { return *tasks.trunk_task; },
-                        return_value_policy<reference_existing_object>()));
+      .add_property("com_task", make_function(
+                                    +[](WalkTasks& tasks) -> CoMTask& { return *tasks.com_task; },
+                                    return_value_policy<reference_existing_object>()))
+      .add_property("trunk_task", make_function(
+                                      +[](WalkTasks& tasks) -> PositionTask& { return *tasks.trunk_task; },
+                                      return_value_policy<reference_existing_object>()));
 
   class__<LIPM::Trajectory>("LIPMTrajectory", init<>())
       .def("pos", &LIPM::Trajectory::pos)

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

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

{placo-0.8.10 → placo-0.9.1}/src/placo/humanoid/footsteps_planner.h

@@ -48,11 +48,17 @@ public:
 
     std::vector<Footstep> footsteps;
 
-    double t_start = -1.; // Time at which the support starts. Is set to -1 if not initialized
-    double elapsed_ratio = 0.; // Elapsed ratio of the support phase, ranging from 0 to 1
+    // Time at which the support starts. Is set to -1 if not initialized
+    double t_start = -1.;
 
-    double time_ratio = 1.; // Time ratio for the remaining part of the support phase
-    Eigen::Vector2d target_world_dcm = Eigen::Vector2d::Zero(); // Target DCM in the world frame at the end of the support phase
+    // Elapsed ratio of the support phase, ranging from 0 to 1
+    double elapsed_ratio = 0.;
+
+    // Time ratio for the remaining part of the support phase
+    double time_ratio = 1.;
+
+    // Target DCM in the world frame at the end of the support phase
+    Eigen::Vector2d target_world_dcm = Eigen::Vector2d::Zero();
 
     bool start = false;
     bool end = false;
@@ -119,7 +125,8 @@ public:
    * @return vector of supports to use. It starts with initial double supports,
    * and add double support phases between footsteps.
    */
-  static std::vector<Support> make_supports(std::vector<Footstep> footsteps, double t_start, bool start = true, bool middle = false, bool end = true);
+  static std::vector<Support> make_supports(std::vector<Footstep> footsteps, double t_start, bool start = true,
+                                            bool middle = false, bool end = true);
 
   /**
    * @brief Return the type of footsteps planner

{placo-0.8.10 → placo-0.9.1}/src/placo/humanoid/humanoid_robot.cpp

@@ -194,12 +194,6 @@ Eigen::VectorXd HumanoidRobot::get_torques(Eigen::VectorXd acc_a, Eigen::VectorX
   return M * acc + h - J_c * contact_forces;
 }
 
-Eigen::Vector2d HumanoidRobot::dcm(double omega)
-{
-  // DCM = c + (1/omega) c_dot
-  return com_world().head(2) + (1 / omega) * dcom_world().head(2);
-}
-
 Eigen::Vector2d HumanoidRobot::dcm(double omega, Eigen::Vector2d com_velocity)
 {
   // DCM = c + (1/omega) c_dot

{placo-0.8.10 → placo-0.9.1}/src/placo/humanoid/humanoid_robot.h

@@ -39,7 +39,7 @@ public:
   void ensure_on_floor();
 
   /**
-   * @brief Place the robot on its support on the floor according 
+   * @brief Place the robot on its support on the floor according
    * to the trunk orientation and the kinematic configuration
    * @param R_world_trunk Orientation of the trunk
    */
@@ -70,14 +70,8 @@ public:
    * @param use_non_linear_effects If true, non linear effects are taken into account (state.qd necessary)
    * @return Torques of the motors
    */
-  Eigen::VectorXd get_torques(Eigen::VectorXd acc_a, Eigen::VectorXd contact_forces, bool use_non_linear_effects = false);
-
-  /**
-   * @brief Compute the Divergent Component of Motion (DCM)
-   * @param omega Natural frequency of the LIP (= sqrt(g/h))
-   * @return DCM
-   */
-  Eigen::Vector2d dcm(double omega);
+  Eigen::VectorXd get_torques(Eigen::VectorXd acc_a, Eigen::VectorXd contact_forces,
+                              bool use_non_linear_effects = false);
 
   /**
    * @brief Compute the Divergent Component of Motion (DCM)
@@ -145,4 +139,4 @@ public:
   double dist_z_pan_camera;  // Distance along z between the pan DoF and the camera in the head
   double dist_y_trunk_foot;  // Distance along y between the trunk and the left_foot frame in model
 };
-}  // namespace placo::humanoid 
+}  // namespace placo::humanoid

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

@@ -212,6 +212,25 @@ Eigen::Matrix3d WalkPatternGenerator::Trajectory::get_R_world_trunk(double t)
   return T.linear() * R_world_trunk;
 }
 
+Eigen::Vector3d WalkPatternGenerator::Trajectory::get_p_support_CoM(double t)
+{
+  TrajectoryPart& part = _findPart(parts, t);
+  Eigen::Affine3d T_support_world = placo::tools::flatten_on_floor(get_T_world_foot(part.support.side(), t)).inverse();
+  return T_support_world * get_p_world_CoM(t);
+}
+
+Eigen::Vector3d WalkPatternGenerator::Trajectory::get_v_support_CoM(double t)
+{
+  TrajectoryPart& part = _findPart(parts, t);
+  Eigen::Affine3d T_support_world = placo::tools::flatten_on_floor(get_T_world_foot(part.support.side(), t)).inverse();
+  return T_support_world.linear() * get_v_world_CoM(t);
+}
+
+Eigen::Vector2d WalkPatternGenerator::Trajectory::get_p_support_DCM(double t, double omega)
+{
+  return get_p_support_CoM(t).head(2) + (1 / omega) * get_v_support_CoM(t).head(2);
+}
+
 HumanoidRobot::Side WalkPatternGenerator::Trajectory::support_side(double t)
 {
   return _findPart(parts, t).support.side();
@@ -402,7 +421,7 @@ void WalkPatternGenerator::constrain_lipm(problem::Problem& problem, LIPM& lipm,
         lipm.zmp(timestep, omega_2), support.support_polygon(), parameters.zmp_margin));
     if (soft)
     {
-      zmp_constraint.configure(ProblemConstraint::Soft, 1e5);
+      zmp_constraint.configure(ProblemConstraint::Soft, zmp_in_support_weight);
     }
 
     // Optional offset for single supports
@@ -428,15 +447,15 @@ void WalkPatternGenerator::constrain_lipm(problem::Problem& problem, LIPM& lipm,
 
       if (soft)
       {
-        pos_constraint.configure(ProblemConstraint::Soft, 1e3);
-        vel_constraint.configure(ProblemConstraint::Soft, 1e3);
-        acc_constraint.configure(ProblemConstraint::Soft, 1e3);
+        pos_constraint.configure(ProblemConstraint::Soft, stop_end_support_weight);
+        vel_constraint.configure(ProblemConstraint::Soft, stop_end_support_weight);
+        acc_constraint.configure(ProblemConstraint::Soft, stop_end_support_weight);
       }
     }
   }
 }
 
-void WalkPatternGenerator::plan_com(Trajectory& trajectory, std::vector<FootstepsPlanner::Support>& supports,
+bool WalkPatternGenerator::plan_com(Trajectory& trajectory, std::vector<FootstepsPlanner::Support>& supports,
                                     Eigen::Vector2d initial_pos, Eigen::Vector2d initial_vel,
                                     Eigen::Vector2d initial_acc)
 {
@@ -476,14 +495,24 @@ void WalkPatternGenerator::plan_com(Trajectory& trajectory, std::vector<Footstep
   trajectory.t_end = t;
 
   // Solving the problem
-  problem.solve();
+  try
+  {
+    problem.solve();
+  }
+  catch (const std::exception& e)
+  {
+    return false;
+  }
 
+  // Attributing the LIPM trajectories to the trajectory parts
   for (int i = 0; i < (int)trajectory.parts.size(); i++)
   {
     auto& part = trajectory.parts[i];
     part.com_trajectory = lipms[i].get_trajectory();
     part.support.target_world_dcm = part.com_trajectory.dcm(part.t_end, omega);
   }
+
+  return true;
 }
 
 WalkPatternGenerator::Trajectory WalkPatternGenerator::plan(std::vector<FootstepsPlanner::Support>& supports,
@@ -523,7 +552,12 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::replan(std::vector<Footst
   Eigen::Vector2d initial_pos = old_trajectory.get_p_world_CoM(t_replan).head(2);
   Eigen::Vector2d initial_vel = old_trajectory.get_v_world_CoM(t_replan).head(2);
   Eigen::Vector2d initial_acc = old_trajectory.get_a_world_CoM(t_replan).head(2);
-  plan_com(trajectory, supports, initial_pos, initial_vel, initial_acc);
+  bool planned_com = plan_com(trajectory, supports, initial_pos, initial_vel, initial_acc);
+
+  if (!planned_com)
+  {
+    return old_trajectory;
+  }
 
   plan_feet_trajectories(trajectory, &old_trajectory);
 
@@ -539,9 +573,9 @@ bool WalkPatternGenerator::can_replan_supports(Trajectory& trajectory, double t_
     return false;
   }
 
+  // We can't replan if the current support is a double support
   if (trajectory.get_support(t_replan).is_both())
   {
-    // We can't replan if the current support is a double support
     return false;
   }
 
@@ -613,7 +647,6 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::replan_supports(Foo
       current_support.elapsed_ratio +
       elapsed_duration / (support_default_duration(current_support) * current_support.time_ratio);
   supports[0].time_ratio = current_support.time_ratio;
-  supports[0].target_world_dcm = current_support.target_world_dcm;
   return supports;
 }
 
@@ -651,9 +684,8 @@ void WalkPatternGenerator::Trajectory::print_parts_timings()
   }
 }
 
-std::vector<FootstepsPlanner::Support>
-WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Support> supports,
-                                      Eigen::Vector2d world_target_zmp, Eigen::Vector2d world_measured_dcm)
+std::vector<FootstepsPlanner::Support> WalkPatternGenerator::update_supports(
+    double t, std::vector<FootstepsPlanner::Support> supports, Eigen::Vector2d world_measured_dcm)
 {
   FootstepsPlanner::Support current_support = supports[0];
   if (current_support.is_both())
@@ -671,12 +703,15 @@ WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Su
   Eigen::Matrix2d R_world_support = current_support.frame().rotation().topLeftCorner(2, 2);
   Eigen::Vector2d p_world_support = current_support.frame().translation().head(2);
 
-  // Decision variables
-  placo::problem::Variable* support_next_zmp =
-      &problem.add_variable(2);  // ZMP of the next support expressed in the current support frame
-  placo::problem::Variable* tau = &problem.add_variable(1);  // exp(omega * T) where T is the end of the current support
-  placo::problem::Variable* support_dcm_offset =
-      &problem.add_variable(2);  // Offset of the DCM from the ZMP in the current support frame
+  // ----------------- Decision variables: ------------------
+  // ZMP of the next support expressed in the current support frame
+  placo::problem::Variable* support_next_zmp = &problem.add_variable(2);
+
+  // exp(omega * T) where T is the end of the current support
+  placo::problem::Variable* tau = &problem.add_variable(1);
+
+  // Offset of the DCM from the ZMP in the current support frame
+  placo::problem::Variable* support_dcm_offset = &problem.add_variable(2);
 
   // ----------------- Objective functions: -----------------
   // Time reference
@@ -685,14 +720,11 @@ WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Su
 
   // ZMP Reference (expressed in the world frame)
   Expression world_next_zmp_expr = p_world_support + R_world_support * support_next_zmp->expr();
+  Eigen::Vector2d world_target_zmp = supports[1].frame().translation().head(2);
   problem.add_constraint(world_next_zmp_expr == world_target_zmp).configure(ProblemConstraint::Soft, w2);
 
   // DCM offset reference (expressed in the world frame)
-  // XXX : l'offset doit être calculé par rapport à la cible mise à jour non ?
   Eigen::Vector2d world_target_dcm_offset = current_support.target_world_dcm - world_target_zmp;
-  // Eigen::Vector2d world_target_dcm_offset = world_end_dcm - next_support.frame().translation().head(2);
-  // std::cout << "world_target_dcm_offset: " << world_target_dcm_offset.transpose() << std::endl;
-
   Expression world_dcm_offset_expr = R_world_support * support_dcm_offset->expr();
   problem.add_constraint(world_dcm_offset_expr == world_target_dcm_offset).configure(ProblemConstraint::Soft, w3);
 
@@ -747,6 +779,7 @@ WalkPatternGenerator::update_supports(double t, std::vector<FootstepsPlanner::Su
   problem.solve();
 
   // Updating next support position
+  // XXX: Problème de transform T qui s'applique ?
   Eigen::Vector2d world_next_zmp_val = p_world_support + R_world_support * support_next_zmp->value;
   supports[1].footsteps[0].frame.translation().x() = world_next_zmp_val(0);
   supports[1].footsteps[0].frame.translation().y() = world_next_zmp_val(1);

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

@@ -78,6 +78,10 @@ public:
 
     Eigen::Matrix3d get_R_world_trunk(double t);
 
+    Eigen::Vector3d get_p_support_CoM(double t);
+    Eigen::Vector3d get_v_support_CoM(double t);
+    Eigen::Vector2d get_p_support_DCM(double t, double omega);
+
     HumanoidRobot::Side support_side(double t);
     bool support_is_both(double t);
     bool is_flying(HumanoidRobot::Side side, double t);
@@ -180,19 +184,14 @@ public:
   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
    * 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);
 
   /**
@@ -210,6 +209,8 @@ public:
   double support_default_duration(FootstepsPlanner::Support& support);
 
   bool soft = false;
+  double zmp_in_support_weight = 1e3;
+  double stop_end_support_weight = 1e3;
 
 protected:
   // Robot associated to the WPG
@@ -218,13 +219,33 @@ protected:
   // The parameters to use for planning. The values are forwarded to the relevant solvers when needed.
   HumanoidParameters& parameters;
 
+  // Natural frequency of the LIPM (omega = sqrt(g/h))
   double omega;
+
+  // Squared natural frequency of the LIPM (omega^2 = g/h)
   double omega_2;
 
+  /**
+   * @brief Constrains the LIPM to ensure that the ZMP stays in the support polygon and that the CoM stops at
+   * the end of an end support.
+   * @param problem Problem to add the constraints to
+   * @param lipm LIPM to constrain
+   * @param support Support to constrain
+   * @param omega_2 Squared natural frequency of the LIPM (omega^2 = g/h)
+   * @param parameters Humanoid parameters to use for the constraints
+   */
   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,
+  /**
+   * @brief Plans the CoM trajectory for a given support. Returns false if the QP solver failed to solve the problem.
+   * @param trajectory Trajectory to fill
+   * @param support Support to plan
+   * @param initial_pos Initial position of the CoM in the world frame
+   * @param initial_vel Initial velocity of the CoM in the world frame
+   * @param initial_acc Initial acceleration of the CoM in the world frame
+   */
+  bool 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());
 

placo-0.9.1/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.1/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.10 → placo-0.9.1}/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));