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 <mc_rtc/GUIState.h>

#include <lipm_walking/FootstepPlan.h>
#include <lipm_walking/utils/SE2d.h>
#include <lipm_walking/utils/polynomials.h>

namespace lipm_walking
{

struct PlanInterpolator
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  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
  {
    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)
  {
    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;
  }

private:
  void restoreDefaults();

  void runShuffling_();

  void runTurning_();

  void runWalking_();

  void updateLocalTarget_(const SE2d & target);

  void updateWorldTarget_(const Eigen::Vector3d & desired);

public:
  bool isShown = false; 
  unsigned nbIter = 0; 
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;
};

} // namespace lipm_walking