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

#include <copra/LMPC.h>
#include <copra/PreviewSystem.h>
#include <copra/constraints.h>
#include <copra/costFunctions.h>
#include <lipm_walking/Contact.h>
#include <lipm_walking/Pendulum.h>
#include <lipm_walking/Preview.h>
#include <lipm_walking/Sole.h>
#include <lipm_walking/utils/world.h>

namespace lipm_walking
{

struct ModelPredictiveControl
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static constexpr double SAMPLING_PERIOD = 0.1; 
  static constexpr unsigned INPUT_SIZE = 2; 
  static constexpr unsigned NB_STEPS = 16; 
  static constexpr unsigned STATE_SIZE = 6; 
  ModelPredictiveControl();

  void addGUIElements(std::shared_ptr<mc_rtc::gui::StateBuilder> gui);

  void addLogEntries(mc_rtc::Logger & logger);

  void configure(const mc_rtc::Configuration &);

  void phaseDurations(double initSupportDuration, double doubleSupportDuration, double targetSupportDuration);

  bool buildAndSolve();

  void comHeight(double height)
  {
    double zeta = height / world::GRAVITY;
    double omegaInv = std::sqrt(zeta);
    // clang-format off
    dcmFromState_ <<
      1, 0, omegaInv, 0, 0, 0,
      0, 1, 0, omegaInv, 0, 0;
    zmpFromState_ <<
      1, 0, 0, 0, -zeta, 0,
      0, 1, 0, 0, 0, -zeta;
    // clang-format on
  }

  void contacts(Contact initContact, Contact targetContact, Contact nextContact)
  {
    initContact_ = initContact;
    nextContact_ = nextContact;
    targetContact_ = targetContact;
  }

  void initState(const Pendulum & pendulum)
  {
    initState_ = Eigen::VectorXd(STATE_SIZE);
    initState_ << pendulum.com().head<2>(), pendulum.comd().head<2>(), pendulum.comdd().head<2>();
  }

  unsigned indexToHrep(unsigned i) const
  {
    return indexToHrep_[i];
  }

  const Contact & initContact() const
  {
    return initContact_;
  }

  unsigned nbInitSupportSteps() const
  {
    return nbInitSupportSteps_;
  }

  unsigned nbDoubleSupportSteps() const
  {
    return nbDoubleSupportSteps_;
  }

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

  void sole(const Sole & sole)
  {
    sole_ = sole;
  }

  const std::shared_ptr<Preview> solution() const
  {
    return solution_;
  }

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

private:
  void computeZMPRef();

  void updateTerminalConstraint();

  void updateZMPConstraint();

  void updateJerkCost();

  void updateVelCost();

  void updateZMPCost();

public:
  Eigen::Vector2d velWeights = {10., 10.}; 
  double jerkWeight = 1.; 
  double zmpWeight = 1000.; 
private:
  using RefVector = Eigen::Matrix<double, 2 * (NB_STEPS + 1), 1>;
  using StateMatrix = Eigen::Matrix<double, 2, STATE_SIZE>;

  Contact initContact_; 
  Contact nextContact_; 
  Contact targetContact_; 
  RefVector velRef_ = RefVector::Zero(); 
  RefVector zmpRef_ = RefVector::Zero(); 
  Eigen::Matrix<double, 2 * (NB_STEPS + 1), STATE_SIZE *(NB_STEPS + 1)> velCostMat_;
  StateMatrix dcmFromState_ = StateMatrix::Zero(); 
  StateMatrix zmpFromState_ = StateMatrix::Zero(); 
  Eigen::VectorXd initState_; 
  HrepXd hreps_[4]; 
  copra::SolverFlag solver_ = copra::SolverFlag::QLD; 
  double buildAndSolveTime_ = 0.; 
  double solveTime_ = 0.; 
  std::shared_ptr<Preview> solution_ = nullptr; 
  std::shared_ptr<copra::ControlCost> jerkCost_ = nullptr; 
  std::shared_ptr<copra::PreviewSystem> previewSystem_ = nullptr; 
  std::shared_ptr<copra::TrajectoryConstraint> termDCMCons_ = nullptr; 
  std::shared_ptr<copra::TrajectoryConstraint> termZMPCons_ = nullptr; 
  std::shared_ptr<copra::TrajectoryConstraint> zmpCons_ = nullptr; 
  std::shared_ptr<copra::TrajectoryCost> velCost_ = nullptr; 
  std::shared_ptr<copra::TrajectoryCost> zmpCost_ = nullptr; 
  unsigned indexToHrep_[NB_STEPS + 1]; 
  unsigned nbDoubleSupportSteps_ = 0; 
  Sole sole_; 
  unsigned nbInitSupportSteps_ = 0; 
  unsigned nbNextDoubleSupportSteps_ = 0; 
  unsigned nbTargetSupportSteps_ = 0; 
};

} // namespace lipm_walking