FootstepPlan.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 <lipm_walking/Contact.h>
#include <lipm_walking/Sole.h>
#include <lipm_walking/utils/clamp.h>
#include <string>

namespace lipm_walking
{

struct FootstepPlan
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  void complete(const Sole & sole);

  sva::PTransformd computeInitialTransform(const mc_rbdyn::Robot & robot) const;

  void goToNextFootstep();

  void goToNextFootstep(const sva::PTransformd & actualTargetPose);

  void load(const mc_rtc::Configuration & config);

  void reset(unsigned startIndex);

  void restorePreviousFootstep();

  void save(mc_rtc::Configuration & config) const;

  void updateInitialTransform(const sva::PTransformd & X_0_lf, const sva::PTransformd & X_0_rf, double initHeight);

  void appendContact(Contact step)
  {
    contacts_.push_back(step);
  }

  double comHeight() const
  {
    return comHeight_;
  }

  void comHeight(double height)
  {
    comHeight_ = clamp(height, 0., 2.);
  }

  const std::vector<Contact> & contacts() const
  {
    return contacts_;
  }

  double doubleSupportDuration() const
  {
    return doubleSupportDuration_;
  }

  void doubleSupportDuration(double duration)
  {
    doubleSupportDuration_ = clamp(duration, 0., 1.);
  }

  double finalDSPDuration() const
  {
    return finalDSPDuration_;
  }

  bool hasTorsoPitch() const
  {
    return (torsoPitch_ > -10.);
  }

  void finalDSPDuration(double duration)
  {
    finalDSPDuration_ = clamp(duration, 0.1, 1.6);
  }

  double initDSPDuration() const
  {
    return initDSPDuration_;
  }

  void initDSPDuration(double duration)
  {
    initDSPDuration_ = clamp(duration, 0.1, 1.6);
  }

  const sva::PTransformd & initPose()
  {
    return X_0_init_;
  }

  double landingDuration() const
  {
    if(supportContact_.swingConfig.has("landing_duration"))
    {
      return supportContact_.swingConfig("landing_duration");
    }
    return landingDuration_;
  }

  void landingDuration(double duration)
  {
    landingDuration_ = clamp(duration, 0., 0.5);
  }

  double landingPitch() const
  {
    if(prevContact_.swingConfig.has("landing_pitch"))
    {
      return prevContact_.swingConfig("landing_pitch");
    }
    return landingPitch_;
  }

  void landingPitch(double pitch)
  {
    constexpr double MIN_LANDING_PITCH = -1.;
    constexpr double MAX_LANDING_PITCH = 1.;
    landingPitch_ = clamp(pitch, MIN_LANDING_PITCH, MAX_LANDING_PITCH);
  }

  const Contact & nextContact() const
  {
    return nextContact_;
  }

  const Contact & prevContact() const
  {
    return prevContact_;
  }

  void resetContacts(const std::vector<Contact> & contacts)
  {
    contacts_ = contacts;
  }

  double singleSupportDuration() const
  {
    return singleSupportDuration_;
  }

  void singleSupportDuration(double duration)
  {
    singleSupportDuration_ = clamp(duration, 0., 2.);
  }

  const Contact & supportContact() const
  {
    return supportContact_;
  }

  void swapFirstTwoContacts()
  {
    std::swap(contacts_[0], contacts_[1]);
  }

  double swingHeight() const
  {
    if(prevContact_.swingConfig.has("height"))
    {
      return prevContact_.swingConfig("height");
    }
    return swingHeight_;
  }

  void swingHeight(double height)
  {
    constexpr double MIN_SWING_FOOT_HEIGHT = 0.;
    constexpr double MAX_SWING_FOOT_HEIGHT = 0.25;
    swingHeight_ = clamp(height, MIN_SWING_FOOT_HEIGHT, MAX_SWING_FOOT_HEIGHT);
  }

  double takeoffDuration() const
  {
    if(supportContact_.swingConfig.has("takeoff_duration"))
    {
      return supportContact_.swingConfig("takeoff_duration");
    }
    return takeoffDuration_;
  }

  void takeoffDuration(double duration)
  {
    takeoffDuration_ = clamp(duration, 0., 0.5);
  }

  Eigen::Vector3d takeoffOffset() const
  {
    if(prevContact_.swingConfig.has("takeoff_offset"))
    {
      return prevContact_.swingConfig("takeoff_offset");
    }
    return takeoffOffset_;
  }

  void takeoffOffset(const Eigen::Vector3d & offset)
  {
    takeoffOffset_ = offset;
  }

  double takeoffPitch() const
  {
    if(prevContact_.swingConfig.has("takeoff_pitch"))
    {
      return prevContact_.swingConfig("takeoff_pitch");
    }
    return takeoffPitch_;
  }

  void takeoffPitch(double pitch)
  {
    constexpr double MIN_TAKEOFF_PITCH = -1.;
    constexpr double MAX_TAKEOFF_PITCH = 1.;
    takeoffPitch_ = clamp(pitch, MIN_TAKEOFF_PITCH, MAX_TAKEOFF_PITCH);
  }

  const Contact & targetContact() const
  {
    return targetContact_;
  }

  double torsoPitch() const
  {
    return torsoPitch_;
  }

  void rewind()
  {
    reset(0);
  }

public:
  mc_rtc::Configuration mpcConfig;
  std::string name = "";

private:
  Contact nextContact_;
  Contact prevContact_;
  Contact supportContact_;
  Contact targetContact_;
  Eigen::Vector3d takeoffOffset_ = Eigen::Vector3d::Zero(); // [m]
  double comHeight_ = 0.8; // [m]
  double doubleSupportDuration_ = 0.2; // [s]
  double finalDSPDuration_ = 0.6; // [s]
  double initDSPDuration_ = 0.6; // [s]
  double landingDuration_ = 0.15; // [s]
  double landingPitch_ = 0.;
  double singleSupportDuration_ = 0.8; // [s]
  double swingHeight_ = 0.04; // [m]
  double takeoffDuration_ = 0.05; // [s]
  double takeoffPitch_ = 0.;
  double torsoPitch_ = -100.;
  std::vector<Contact> contacts_;
  sva::PTransformd X_0_init_;
  unsigned nextFootstep_ = 0;
};
} // namespace lipm_walking

namespace mc_rtc
{
template<>
struct ConfigurationLoader<lipm_walking::FootstepPlan>
{
  static lipm_walking::FootstepPlan load(const mc_rtc::Configuration & config)
  {
    lipm_walking::FootstepPlan plan;
    plan.load(config);
    return plan;
  }

  static mc_rtc::Configuration save(const lipm_walking::FootstepPlan & plan)
  {
    mc_rtc::Configuration config;
    plan.save(config);
    return config;
  }
};

} // namespace mc_rtc