ExternalPlanner.h

Go to the documentation of this file.

#pragma once
#include <mc_control/MCController.h>
 
#include <ExternalFootstepPlanner/Plan.h>
#include <ExternalFootstepPlanner/Request.h>
#include <lipm_walking/FootstepPlan.h>
#include <lipm_walking/utils/SE2d.h>
 
namespace mc_control
{
struct MCController;
} // namespace mc_control
 
namespace lipm_walking
{
 
struct ExternalPlanner
{
  using SE2d = lipm_walking::utils::SE2d;
  using Request = mc_plugin::ExternalFootstepPlanner::Request;
  using Foot = mc_plugin::ExternalFootstepPlanner::Foot;
 
  enum State
  {
    Standing,
    DoubleSupport,
    SingleSupport
  }; 
 
  ExternalPlanner(mc_control::MCController & ctl);
 
  void configure(const mc_rtc::Configuration & config);
 
  bool planningRequested() const
  {
    return ctl_.datastore().call<bool>("ExternalFootstepPlanner::PlanningRequested");
  }
 
  // clang-format off
  // clang-format on
  void requestPlan(const State state,
                   const Foot supportFoot,
                   const utils::SE2d & start_lf,
                   const utils::SE2d & start_rf,
                   double allowed_time);
 
  void cancelRequest()
  {
    // FIXME should handle cancelling the request on the planner side
    requested_ = false;
  }
 
  bool planRequested(State state) const noexcept
  {
    return state_ == state && requested_;
  }
 
  bool hasPlan(State state) const
  {
    return state_ == state && ctl_.datastore().call<bool>("ExternalFootstepPlanner::HasPlan");
  }
 
  std::vector<lipm_walking::Contact> plan();
 
  State state() const
  {
    return state_;
  }
 
  void leftFootLandingOffset(const SE2d & offset)
  {
    leftFootLandingOffset_ = offset;
  }
 
  void rightFootLandingOffset(const SE2d & offset)
  {
    rightFootLandingOffset_ = offset;
  }
 
  void allowedTimeSingleSupport(const double time)
  {
    allowedTimeSingleSupport_ = time;
  }
 
  double allowedTimeSingleSupport() const noexcept
  {
    return allowedTimeSingleSupport_;
  }
 
  void allowedTimeStanding(const double time)
  {
    allowedTimeStanding_ = time;
  }
 
  double allowedTimeStanding() const noexcept
  {
    return allowedTimeStanding_;
  }
 
  void activate()
  {
    ctl_.datastore().call<void>("ExternalFootstepPlanner::Activate");
  }
 
  void deactivate()
  {
    ctl_.datastore().call<void>("ExternalFootstepPlanner::Deactivate");
  }
 
protected:
  void requestPlanWorldPositionTarget(const State state,
                                      const Foot supportFoot,
                                      const utils::SE2d & start_lf,
                                      const utils::SE2d & start_rf,
                                      const utils::SE2d & targetWorld,
                                      double allowed_time);
 
  void requestPlanLocalPositionTarget(const State state,
                                      const Foot supportFoot,
                                      const utils::SE2d & start_lf,
                                      const utils::SE2d & start_rf,
                                      const utils::SE2d & localTarget,
                                      double allowed_time);
 
protected:
  mc_control::MCController & ctl_;
  State state_ = State::Standing;
  bool requested_ = false;
  double allowedTimeSingleSupport_ = 0.5;
  double allowedTimeStanding_ = 2.0;
 
  SE2d leftFootLandingOffset_{0., 0.15, 0.};
  SE2d rightFootLandingOffset_{0, -0.15, 0.};
};
 
} // namespace lipm_walking