placo 0.9.0__tar.gz → 0.9.2__tar.gz

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

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

{placo-0.9.0 → placo-0.9.2}/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.9.0 → placo-0.9.2}/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)
@@ -33,8 +35,8 @@ void exposeWalkPatternGenerator()
       .add_property("com_target_z", &WalkPatternGenerator::Trajectory::com_target_z)
       .add_property("trunk_pitch", &WalkPatternGenerator::Trajectory::trunk_pitch)
       .add_property("trunk_roll", &WalkPatternGenerator::Trajectory::trunk_roll)
-      .add_property("kept_ts", &WalkPatternGenerator::Trajectory::kept_ts)
       .add_property("parts", &WalkPatternGenerator::Trajectory::parts)
+      .add_property("replan_success", &WalkPatternGenerator::Trajectory::replan_success)
       .def("get_T_world_left", &WalkPatternGenerator::Trajectory::get_T_world_left)
       .def("get_T_world_right", &WalkPatternGenerator::Trajectory::get_T_world_right)
       .def("get_v_world_right", &WalkPatternGenerator::Trajectory::get_v_world_right)
@@ -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.9.0 → placo-0.9.2}/pyproject.toml

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

{placo-0.9.0 → placo-0.9.2}/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.9.0 → placo-0.9.2}/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.9.0 → placo-0.9.2}/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.9.0 → placo-0.9.2}/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,13 @@ 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 we can't plan the CoM, we return the trimmed old trajectory
+  if (!planned_com)
+  {
+    return trim_old_trajectory(old_trajectory, t_replan);
+  }
 
   plan_feet_trajectories(trajectory, &old_trajectory);
 
@@ -539,23 +574,12 @@ 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;
   }
 
-  // XXX: Need to move to a branch
-  // We can't replan if more than 1/2 of the support phase has passed
-  // if (trajectory.get_support(t_replan).elapsed_ratio > 0.8)
-  // {
-  //   return false;
-  // }
-  // if (t_replan - trajectory.get_support(t_replan).t_start < 0.005)
-  // {
-  //   return false;
-  // }
-
   return true;
 }
 
@@ -613,7 +637,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 +674,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 +693,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 +710,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 +769,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);
@@ -784,4 +807,40 @@ Eigen::Vector2d WalkPatternGenerator::get_optimal_zmp(Eigen::Vector2d world_dcm_
   problem.solve();
   return world_zmp->value;
 }
+
+WalkPatternGenerator::Trajectory WalkPatternGenerator::trim_old_trajectory(Trajectory& old_trajectory, double t_replan)
+{
+  Trajectory trajectory = old_trajectory;
+  trajectory.t_start = t_replan;
+  trajectory.replan_success = false;
+
+  // Remove the parts before the replan time
+  std::vector<WalkPatternGenerator::TrajectoryPart> new_parts;
+  for (auto part : old_trajectory.parts)
+  {
+    if (t_replan >= part.t_start)
+    {
+      if (t_replan < part.t_end)
+      {
+        TrajectoryPart new_part = part;
+        new_part.t_start = t_replan;
+
+        // Updating the elapsed ratio of the support
+        double elapsed_duration = t_replan - std::max(old_trajectory.t_start, new_part.support.t_start);
+        new_part.support.elapsed_ratio =
+            new_part.support.elapsed_ratio +
+            elapsed_duration / (support_default_duration(new_part.support) * new_part.support.time_ratio);
+
+        new_parts.push_back(new_part);
+      }
+    }
+    else
+    {
+      new_parts.push_back(part);
+    }
+  }
+  trajectory.parts = new_parts;
+
+  return trajectory;
+}
 }  // namespace placo::humanoid

{placo-0.9.0 → placo-0.9.2}/src/placo/humanoid/walk_pattern_generator.h

@@ -53,7 +53,8 @@ public:
     double trunk_pitch;
     double trunk_roll;
 
-    int kept_ts = 0;
+    // Replan succeeded?
+    bool replan_success = true;
 
     Eigen::Affine3d get_T_world_left(double t);
     Eigen::Affine3d get_T_world_right(double t);
@@ -78,6 +79,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 +185,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 +210,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,18 +220,46 @@ 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());
 
   void plan_dbl_support(Trajectory& trajectory, int part_index);
   void plan_sgl_support(Trajectory& trajectory, int part_index, Trajectory* old_trajectory);
   void plan_feet_trajectories(Trajectory& trajectory, Trajectory* old_trajectory = nullptr);
+
+  /**
+   * @brief Trims the old trajectory to the new time horizon without modifying the CoM and feet trajectories.
+   * @param old_trajectory The old trajectory to trim
+   * @param t_replan The time at which the replan happens
+   * @return The trimmed trajectory
+   */
+  Trajectory trim_old_trajectory(Trajectory& old_trajectory, double t_replan);
 };
 }  // namespace placo::humanoid

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

@@ -176,6 +176,7 @@ public:
    * @brief Adds joints task
    * @param joints value for the joints
    * @return joints task
+   * @pyignore
    */
   JointsTask& add_joints_task(std::map<std::string, double>& joints);
 

{placo-0.9.0 → placo-0.9.2}/src/placo/model/robot_wrapper.cpp

@@ -252,11 +252,6 @@ Eigen::Vector3d RobotWrapper::com_world()
   return data->com[0];
 }
 
-Eigen::Vector3d RobotWrapper::dcom_world()
-{
-  return com_jacobian() * state.qd;
-}
-
 void RobotWrapper::update_kinematics()
 {
   pinocchio::framesForwardKinematics(model, *data, state.q);

{placo-0.9.0 → placo-0.9.2}/src/placo/model/robot_wrapper.h

@@ -239,11 +239,6 @@ public:
    */
   Eigen::Vector3d com_world();
 
-  /**
-   * @brief Gets the CoM velocity in the world
-   */
-  Eigen::Vector3d dcom_world();
-
   /**
    * @brief Gets the frame to world transformation matrix for a given frame
    *
@@ -480,7 +475,7 @@ public:
   Eigen::MatrixXd joint_jacobian(pinocchio::JointIndex joint,
                                  pinocchio::ReferenceFrame ref = pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED);
 
-                                 /**
+  /**
    * @brief Joint jacobian, default reference is LOCAL_WORLD_ALIGNED
    *
    * Be sure you called \ref update_kinematics before calling this method if your state has changed