Variable duration implemented

Author: MarcDcls

bindings/expose-footsteps.cpp

@@ -29,10 +29,9 @@ void exposeFootsteps()
       .def("support_polygon", &FootstepsPlanner::Footstep::support_polygon)
       .def("overlap", &FootstepsPlanner::Footstep::overlap)
       .def("polygon_contains", &FootstepsPlanner::Footstep::polygon_contains)
-      .staticmethod("polygon_contains")
-      .add_property("kick", &FootstepsPlanner::Footstep::kick, &FootstepsPlanner::Footstep::kick);
+      .staticmethod("polygon_contains");
 
-  class__<FootstepsPlanner::Support>("Support")
+  class__<FootstepsPlanner::Support>("Support", init<std::vector<FootstepsPlanner::Footstep>>())
       .def("support_polygon", &FootstepsPlanner::Support::support_polygon)
       .def("frame", &FootstepsPlanner::Support::frame)
       .def("footstep_frame", &FootstepsPlanner::Support::footstep_frame)
@@ -42,14 +41,16 @@ void exposeFootsteps()
           "set_start", +[](FootstepsPlanner::Support& support, bool b) { support.start = b; })
       .def(
           "set_end", +[](FootstepsPlanner::Support& support, bool b) { support.end = b; })
-      .def("kick", &FootstepsPlanner::Support::kick)
       .add_property("footsteps", &FootstepsPlanner::Support::footsteps)
+      .add_property("t_start", &FootstepsPlanner::Support::t_start, &FootstepsPlanner::Support::t_start)
+      .add_property("elapsed_ratio", &FootstepsPlanner::Support::elapsed_ratio, &FootstepsPlanner::Support::elapsed_ratio)
+      .add_property("time_ratio", &FootstepsPlanner::Support::time_ratio, &FootstepsPlanner::Support::time_ratio)
       .add_property("start", &FootstepsPlanner::Support::start, &FootstepsPlanner::Support::start)
-      .add_property("end", &FootstepsPlanner::Support::end, &FootstepsPlanner::Support::end);
+      .add_property("end", &FootstepsPlanner::Support::end, &FootstepsPlanner::Support::end)
+      .add_property("replanned", &FootstepsPlanner::Support::replanned, &FootstepsPlanner::Support::replanned);
 
   class__<FootstepsPlanner, boost::noncopyable>("FootstepsPlanner", no_init)
       .def("make_supports", &FootstepsPlanner::make_supports)
-      .def("add_first_support", &FootstepsPlanner::add_first_support)
       .def("opposite_footstep", &FootstepsPlanner::opposite_footstep);
 
   class__<FootstepsPlannerNaive, bases<FootstepsPlanner>>("FootstepsPlannerNaive", init<HumanoidParameters&>())

bindings/expose-parameters.cpp

@@ -47,8 +47,8 @@ void exposeParameters()
                     &HumanoidParameters::walk_dtheta_spacing)
       .def("dt", &HumanoidParameters::dt)
       .def("double_support_timesteps", &HumanoidParameters::double_support_timesteps)
-      .def("double_support_duration", &HumanoidParameters::double_support_duration)
       .def("startend_double_support_timesteps", &HumanoidParameters::startend_double_support_timesteps)
+      .def("double_support_duration", &HumanoidParameters::double_support_duration)
       .def("startend_double_support_duration", &HumanoidParameters::startend_double_support_duration)
       .def("has_double_support", &HumanoidParameters::has_double_support)
       .def("ellipsoid_clip", &HumanoidParameters::ellipsoid_clip);

bindings/expose-walk-pattern-generator.cpp

@@ -22,12 +22,22 @@ using namespace placo::humanoid;
 
 void exposeWalkPatternGenerator()
 {
-  class__<WalkPatternGenerator::Trajectory>("WalkTrajectory")
+  class__<WalkPatternGenerator::TrajectoryPart>("WPGTrajectoryPart", init<FootstepsPlanner::Support, double>())
+      .add_property("t_start", &WalkPatternGenerator::TrajectoryPart::t_start)
+      .add_property("t_end", &WalkPatternGenerator::TrajectoryPart::t_end, &WalkPatternGenerator::TrajectoryPart::t_end)
+      .add_property("support", &WalkPatternGenerator::TrajectoryPart::support);
+
+  class__<WalkPatternGenerator::Trajectory>("WPGTrajectory", init<double, double, double>())
       .add_property("t_start", &WalkPatternGenerator::Trajectory::t_start)
       .add_property("t_end", &WalkPatternGenerator::Trajectory::t_end)
+      .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)
       .def("get_T_world_left", &WalkPatternGenerator::Trajectory::get_T_world_left)
-      .def("get_supports", &WalkPatternGenerator::Trajectory::get_supports)
       .def("get_T_world_right", &WalkPatternGenerator::Trajectory::get_T_world_right)
+      .def("get_v_world_right", &WalkPatternGenerator::Trajectory::get_v_world_right)
+      .def("get_v_world_foot", &WalkPatternGenerator::Trajectory::get_v_world_foot)
       .def("get_p_world_CoM", &WalkPatternGenerator::Trajectory::get_p_world_CoM)
       .def("get_v_world_CoM", &WalkPatternGenerator::Trajectory::get_v_world_CoM)
       .def("get_a_world_CoM", &WalkPatternGenerator::Trajectory::get_a_world_CoM)
@@ -37,11 +47,13 @@ void exposeWalkPatternGenerator()
       .def("get_R_world_trunk", &WalkPatternGenerator::Trajectory::get_R_world_trunk)
       .def("support_side", &WalkPatternGenerator::Trajectory::support_side)
       .def("support_is_both", &WalkPatternGenerator::Trajectory::support_is_both)
+      .def("get_supports", &WalkPatternGenerator::Trajectory::get_supports)
       .def("get_support", &WalkPatternGenerator::Trajectory::get_support)
       .def("get_next_support", &WalkPatternGenerator::Trajectory::get_next_support)
       .def("get_prev_support", &WalkPatternGenerator::Trajectory::get_prev_support)
       .def("get_part_t_start", &WalkPatternGenerator::Trajectory::get_part_t_start)
-      .def("apply_transform", &WalkPatternGenerator::Trajectory::apply_transform);
+      .def("apply_transform", &WalkPatternGenerator::Trajectory::apply_transform)
+      .def("print_parts_timings", &WalkPatternGenerator::Trajectory::print_parts_timings);
 
   class__<WalkPatternGenerator>("WalkPatternGenerator", init<HumanoidRobot&, HumanoidParameters&>())
       .def("plan", &WalkPatternGenerator::plan)
@@ -124,16 +136,24 @@ void exposeWalkPatternGenerator()
       .def("dzmp", &LIPM::Trajectory::dzmp)
       .def("dcm", &LIPM::Trajectory::dcm);
 
-  class__<LIPM>("LIPM", init<problem::Problem&, int, double, Eigen::Vector2d, Eigen::Vector2d, Eigen::Vector2d>())
+  class__<LIPM>("LIPM", init<problem::Problem&, double, int, double, Eigen::Vector2d, Eigen::Vector2d, Eigen::Vector2d>())
+      .def("compute_omega", &LIPM::compute_omega)
+      .def("get_trajectory", &LIPM::get_trajectory)
       .def("pos", &LIPM::pos)
       .def("vel", &LIPM::vel)
       .def("acc", &LIPM::acc)
       .def("jerk", &LIPM::jerk)
+      .def("dcm", &LIPM::dcm)
       .def("zmp", &LIPM::zmp)
       .def("dzmp", &LIPM::dzmp)
-      .def("dcm", &LIPM::dcm)
-      .def("compute_omega", &LIPM::compute_omega)
+      .def("build_LIPM_from_previous", &LIPM::build_LIPM_from_previous)
       .def("get_trajectory", &LIPM::get_trajectory)
-      .add_property("x", &LIPM::x)
-      .add_property("y", &LIPM::y);
+      .add_property("dt", &LIPM::dt, &LIPM::dt)
+      .add_property("timesteps", &LIPM::timesteps, &LIPM::timesteps)
+      .add_property("t_start", &LIPM::t_start, &LIPM::t_start)
+      .add_property("t_end", &LIPM::t_end, &LIPM::t_end)
+      .add_property("x_var", &LIPM::x_var, &LIPM::x_var)
+      .add_property("y_var", &LIPM::y_var, &LIPM::y_var)
+      .add_property("x", &LIPM::x, &LIPM::x)
+      .add_property("y", &LIPM::y, &LIPM::y);
 }

src/placo/humanoid/footsteps_planner.cpp

@@ -93,6 +93,16 @@ bool FootstepsPlanner::Footstep::overlap(Footstep& other, double margin)
   return false;
 }
 
+bool FootstepsPlanner::Footstep::operator==(const Footstep& other)
+{
+  return side == other.side && frame.isApprox(other.frame);
+}
+
+FootstepsPlanner::Support::Support(std::vector<Footstep> footsteps) 
+  : footsteps(footsteps)
+{
+}
+
 std::vector<Eigen::Vector2d> FootstepsPlanner::Support::support_polygon()
 {
   if (!computed_polygon)
@@ -122,16 +132,6 @@ std::vector<Eigen::Vector2d> FootstepsPlanner::Support::support_polygon()
   return polygon;
 }
 
-bool FootstepsPlanner::Support::kick()
-{
-  if (is_both())
-  {
-    return false;
-  }
-
-  return footsteps[0].kick;
-}
-
 Eigen::Affine3d FootstepsPlanner::Support::frame()
 {
   Eigen::Affine3d f;
@@ -167,32 +167,6 @@ Eigen::Affine3d FootstepsPlanner::Support::footstep_frame(HumanoidRobot::Side si
   throw std::logic_error("Asked for a frame that doesn't exist");
 }
 
-FootstepsPlanner::FootstepsPlanner(HumanoidParameters& parameters) : parameters(parameters)
-{
-}
-
-bool FootstepsPlanner::Footstep::operator==(const Footstep& other)
-{
-  return side == other.side && frame.isApprox(other.frame);
-}
-
-bool FootstepsPlanner::Support::operator==(const Support& other)
-{
-  if (footsteps.size() != other.footsteps.size())
-  {
-    return false;
-  }
-  for (int k = 0; k < footsteps.size(); k++)
-  {
-    if (!(footsteps[k] == other.footsteps[k]))
-    {
-      return false;
-    }
-  }
-
-  return true;
-}
-
 HumanoidRobot::Side FootstepsPlanner::Support::side()
 {
   if (footsteps.size() > 1)
@@ -222,8 +196,29 @@ FootstepsPlanner::Support operator*(Eigen::Affine3d T, const FootstepsPlanner::S
   return new_support;
 }
 
+bool FootstepsPlanner::Support::operator==(const Support& other)
+{
+  if (footsteps.size() != other.footsteps.size())
+  {
+    return false;
+  }
+  for (int k = 0; k < footsteps.size(); k++)
+  {
+    if (!(footsteps[k] == other.footsteps[k]))
+    {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+FootstepsPlanner::FootstepsPlanner(HumanoidParameters& parameters) : parameters(parameters)
+{
+}
+
 std::vector<FootstepsPlanner::Support>
-FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footsteps, bool start, bool middle, bool end)
+FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footsteps, double t_start, bool start, bool middle, bool end)
 {
   std::vector<Support> supports;
 
@@ -232,40 +227,36 @@ FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footstep
     if (start)
     {
       // Creating the first (double-support) initial state
-      Support support;
+      Support support({footsteps[0], footsteps[1]});
       support.start = true;
-      support.footsteps = { footsteps[0], footsteps[1] };
+      support.t_start = t_start;
       supports.push_back(support);
     }
     else
     {
-      Support support;
-      support.footsteps = { footsteps[0] };
+      Support support({footsteps[0]});
+      support.t_start = t_start;
       supports.push_back(support);
 
       if (middle)
       {
-        Support double_support;
-        double_support.footsteps = { 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++)
     {
-      Support single_support;
-      single_support.footsteps = { footsteps[step] };
+      Support single_support({footsteps[step]});
       supports.push_back(single_support);
 
       bool is_end = (step == footsteps.size() - 2);
 
       if ((!is_end && middle))
       {
-        Support double_support;
-        double_support.footsteps = { footsteps[step], footsteps[step + 1] };
-
+        Support double_support({footsteps[step], footsteps[step + 1]});
         supports.push_back(double_support);
       }
     }
@@ -274,22 +265,14 @@ FootstepsPlanner::make_supports(std::vector<FootstepsPlanner::Footstep> footstep
   if (end)
   {
     // Creating the first (double-support) initial state
-    Support support;
+    Support support({footsteps[footsteps.size() - 2], footsteps[footsteps.size() - 1]});
     support.end = true;
-    support.footsteps = { footsteps[footsteps.size() - 2], footsteps[footsteps.size() - 1] };
     supports.push_back(support);
   }
 
   return supports;
 }
 
-void FootstepsPlanner::add_first_support(std::vector<Support>& supports, Support support)
-{
-  supports[0].start = false;
-  supports.insert(supports.begin(), support);
-  supports[0].start = true;
-}
-
 FootstepsPlanner::Footstep FootstepsPlanner::create_footstep(HumanoidRobot::Side side, Eigen::Affine3d T_world_foot)
 {
   FootstepsPlanner::Footstep footstep(parameters.foot_width, parameters.foot_length);

src/placo/humanoid/footsteps_planner.h

@@ -17,19 +17,21 @@ class FootstepsPlanner
   struct Footstep
   {
     Footstep(double foot_width, double foot_length);
+
     double foot_width;
     double foot_length;
+
     HumanoidRobot::Side side;
     Eigen::Affine3d frame;
+
     std::vector<Eigen::Vector2d> polygon;
     bool computed_polygon = false;
-    bool kick = false;
-
-    bool operator==(const Footstep& other);
 
     std::vector<Eigen::Vector2d> support_polygon();
     std::vector<Eigen::Vector2d> compute_polygon(double margin = 0.);
 
+    bool operator==(const Footstep& other);
+
     bool overlap(Footstep& other, double margin = 0.);
 
     static bool polygon_contains(std::vector<Eigen::Vector2d>& polygon, Eigen::Vector2d point);
@@ -41,13 +43,21 @@ class FootstepsPlanner
    */
   struct Support
   {
+    Support(std::vector<Footstep> footsteps);
+
     std::vector<Footstep> footsteps;
-    std::vector<Eigen::Vector2d> polygon;
-    bool computed_polygon = false;
+
+    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
+    double time_ratio = 1.; // Time ratio for the remaining part of the support phase
+
     bool start = false;
     bool end = false;
     bool replanned = false;
-    bool kick();
+
+    std::vector<Eigen::Vector2d> polygon;
+    bool computed_polygon = false;
+
     std::vector<Eigen::Vector2d> support_polygon();
 
     /**
@@ -105,16 +115,13 @@ class FootstepsPlanner
    * @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, 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
    */
   virtual std::string name() = 0;
 
-  static void add_first_support(std::vector<Support>& supports, Support support);
-
   /**
    * @brief Return the opposite footstep in a neutral position (i.e. at a
    * distance parameters.feet_spacing from the given footstep)

src/placo/humanoid/humanoid_parameters.cpp

@@ -13,14 +13,14 @@ int HumanoidParameters::double_support_timesteps()
   return std::round(double_support_ratio * single_support_timesteps);
 }
 
-double HumanoidParameters::double_support_duration()
+int HumanoidParameters::startend_double_support_timesteps()
 {
-  return double_support_timesteps() * dt();
+  return std::round(startend_double_support_ratio * single_support_timesteps);
 }
 
-int HumanoidParameters::startend_double_support_timesteps()
+double HumanoidParameters::double_support_duration()
 {
-  return std::round(startend_double_support_ratio * single_support_timesteps);
+  return double_support_timesteps() * dt();
 }
 
 double HumanoidParameters::startend_double_support_duration()

src/placo/humanoid/humanoid_parameters.h

@@ -40,22 +40,22 @@ class HumanoidParameters
   double startend_double_support_ratio = 1.;
 
   /**
-   * @brief Duration [s] of a double support
+   * @brief Default duration [s] of a double support
    */
   double double_support_duration();
 
   /**
-   * @brief Duration [s] of a start/end double support
+   * @brief Default duration [s] of a start/end double support
    */
   double startend_double_support_duration();
 
   /**
-   * @brief Number of timesteps for one double support
+   * @brief Default number of timesteps for one double support
    */
   int double_support_timesteps();
 
   /**
-   * @brief Number of timesteps for one start/end double support
+   * @brief Default number of timesteps for one start/end double support
    */
   int startend_double_support_timesteps();