mpc.hpp

Go to the documentation of this file.

// BSD 3-Clause License
//
// Copyright (C) 2022 LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef SOBEC_MPC_WALK_HPP_
#define SOBEC_MPC_WALK_HPP_
 
#include <crocoddyl/core/fwd.hpp>
#include <crocoddyl/core/solvers/fddp.hpp>
#include <crocoddyl/multibody/fwd.hpp>
// #include <crocoddyl/multibody/data/multibody.hpp>
#include <crocoddyl/multibody/states/multibody.hpp>
 
#include "sobec/fwd.hpp"
 
namespace sobec {
using namespace crocoddyl;
struct MPCWalkParams {
  Eigen::Vector3d vcomRef;
  Eigen::VectorXd x0;
  int Tmpc;
  int Tstart;
  int Tdouble;
  int Tsingle;
  int Tend;
  double DT;
  double solver_th_stop;
  double solver_reg_min;
  int solver_maxiter;
 
  std::string stateRegCostName;
 
  MPCWalkParams();
  virtual ~MPCWalkParams() {}
 
  void readParamsFromYamlString(std::string& StringToParse);
  void readParamsFromYamlFile(const std::string& Filename);
};
 
class MPCWalk {
  typedef typename MathBaseTpl<double>::VectorXs VectorXd;
  typedef typename MathBaseTpl<double>::VectorXs Vector3d;
  typedef std::vector<AMA> ActionList;
  // typedef typename
  // crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double> DAM;
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  explicit MPCWalk(boost::shared_ptr<MPCWalkParams> params,
                   boost::shared_ptr<ShootingProblem> problem);
 
  virtual ~MPCWalk() {}
 
  void initialize(const std::vector<Eigen::VectorXd>& xs,
                  const std::vector<Eigen::VectorXd>& us);
 
  void calc(const Eigen::Ref<const VectorXd>& x, const int t);
 
  void updateTerminalCost(const int t);
  void findTerminalStateResidualModel();
  void findStateModel();
 
 public:
  boost::shared_ptr<MPCWalkParams> params;
 
  boost::shared_ptr<ShootingProblem> storage;
 
  boost::shared_ptr<ShootingProblem> problem;
 
  boost::shared_ptr<SolverFDDP> solver;
 
  boost::shared_ptr<ResidualModelState> terminalStateResidual;
 
  boost::shared_ptr<StateMultibody> state;
 
 protected:
  double reg;
};
 
}  // namespace sobec
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
 
#include "sobec/walk-without-think/mpc.hxx"
 
#endif  // SOBEC_MPC_WALK_HPP_