PlanInterpolator.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018-2019, CNRS-UM LIRMM
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#pragma once
 
#include <mc_rbdyn/rpy_utils.h>
 
#include <lipm_walking/FootstepPlan.h>
#include <lipm_walking/utils/SE2d.h>
#include <lipm_walking/utils/polynomials.h>
 
namespace mc_rtc
{
namespace gui
{
struct StateBuilder;
} // namespace gui
} // namespace mc_rtc
 
namespace lipm_walking
{
 
struct PlanInterpolator
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  using SE2d = lipm_walking::utils::SE2d;
 
  static constexpr double DEFAULT_EXTRA_STEP_WIDTH = 0.; // [m]
  static constexpr double IN_PLACE_EXTRA_STEP_WIDTH = 0.02; // [m]
  static constexpr double IN_PLACE_MAX_STEP_ANGLE = 45.; // [deg]
  static constexpr double MAX_EXTRA_STEP_WIDTH = 0.05; // [m]
  static constexpr double MAX_LATERAL_STEP_LENGTH = 0.2; // [m]
  static constexpr double MAX_SAGITTAL_STEP_LENGTH = 0.4; // [m]
  static constexpr double MIN_STEP_LENGTH = 0.001; // [m]
 
  enum class Gait
  {
    Walk,
    Shuffle,
    Turn
  };
 
  PlanInterpolator(std::shared_ptr<mc_rtc::gui::StateBuilder> gui) : gui_(gui) {}
 
  void addGUIElements();
 
  void configure(const mc_rtc::Configuration & config);
 
  void removeGUIElements();
 
  void run();
 
  void suggestGait();
 
  void updateSupportPath(const sva::PTransformd & X_0_lf, const sva::PTransformd & X_0_rf);
 
  std::vector<std::string> availablePlans() const
  {
    return plans_.keys();
  }
 
  const std::string & customPlanName() const
  {
    return customPlan_.name;
  }
 
  std::string gait() const noexcept
  {
    if(gait_ == Gait::Shuffle)
    {
      return "Shuffle";
    }
    else if(gait_ == Gait::Turn)
    {
      return "Turn";
    }
    else // (gait_ == Gait::Walk)
    {
      return "Walk";
    }
  }
 
  void gait(const std::string & dir)
  {
    if(dir == "Shuffle")
    {
      extraStepWidth_ = DEFAULT_EXTRA_STEP_WIDTH;
      gait_ = Gait::Shuffle;
    }
    else if(dir == "Turn")
    {
      extraStepWidth_ = IN_PLACE_EXTRA_STEP_WIDTH;
      gait_ = Gait::Turn;
      targetPose_.x = 0.;
      targetPose_.y = 0.;
    }
    else // (dir == "Walk")
    {
      extraStepWidth_ = DEFAULT_EXTRA_STEP_WIDTH;
      gait_ = Gait::Walk;
    }
    run();
  }
 
  FootstepPlan getPlan(std::string name)
  {
    // Always compute custom plans
    // This will generate a plan based on targetPose_
    if(name.rfind("custom_", 0) == 0)
    {
      run();
      return customPlan_;
    }
 
    if(name == customPlan_.name)
    {
      return customPlan_;
    }
    else // pre-defined plans
    {
      return plans_(name);
    }
  }
 
  void restoreBackwardTarget()
  {
    gait_ = Gait::Walk;
    targetPose_ = lastBackwardTarget_;
    run();
  }
 
  void restoreForwardTarget()
  {
    gait_ = Gait::Walk;
    targetPose_ = lastForwardTarget_;
    run();
  }
 
  void restoreLateralTarget()
  {
    gait_ = Gait::Shuffle;
    targetPose_ = lastLateralTarget_;
    run();
  }
 
  double stepWidth() const
  {
    return stepWidth_;
  }
 
  void stepWidth(double stepWidth)
  {
    stepWidth_ = stepWidth;
  }
 
  const sva::PTransformd & worldReference() const
  {
    return worldReference_;
  }
 
  void worldReference(const sva::PTransformd & worldReference)
  {
    worldReference_ = worldReference;
  }
 
  bool checkPlanUpdated()
  {
    if(hasRun_)
    {
      hasRun_ = false;
      return true;
    }
    return false;
  }
 
  void updateLocalTarget_(const SE2d & target);
 
  void updateWorldTarget_(const Eigen::Vector3d & desired);
 
private:
  void restoreDefaults();
 
  void runShuffling_();
 
  void runTurning_();
 
  void runWalking_();
 
private:
  FootstepPlan customPlan_;
  Gait gait_ = Gait::Walk;
  HoubaPolynomial<Eigen::Vector2d> supportPath_;
  SE2d initPose_ = {0., 0., 0.};
  SE2d lastBackwardTarget_ = {-0.5, 0., 0.}; 
  SE2d lastForwardTarget_ = {0.5, 0., 0.}; 
  SE2d lastLateralTarget_ = {0.0, 0.3, 0.}; 
  SE2d targetPose_ = {0.5, 0., 0.}; 
  bool startWithRightFootstep_ = true;
  double desiredStepAngle_ = 10. * M_PI / 180.; // [rad]
  double desiredStepLength_ = 0.2; // [m]
  double extraStepWidth_ = 0.; // [m]
  double outputVel_ = 0.25; // [m] / [s]
  double stepAngle_ = 0.0; // [deg]
  double stepLength_ = 0.2; 
  double stepWidth_ = 0.18; // [m], default value is for HRP-4
  mc_rtc::Configuration plans_; 
  std::shared_ptr<mc_rtc::gui::StateBuilder> gui_;
  std::vector<Eigen::Vector3d> supportPathDisplay_;
  sva::PTransformd worldReference_;
  unsigned nbFootsteps_ = 0;
  bool hasRun_ = false; 
};
 
} // namespace lipm_walking