mpc-walk.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <crocoddyl/core/costs/cost-sum.hpp>
10 #include <crocoddyl/core/costs/residual.hpp>
11 #include <crocoddyl/core/integrator/euler.hpp>
12 #include <crocoddyl/core/optctrl/shooting.hpp>
13 #include <crocoddyl/core/utils/exception.hpp>
14 #include <crocoddyl/multibody/actions/contact-fwddyn.hpp>
15 #include <crocoddyl/multibody/residuals/state.hpp>
16 #include <crocoddyl/multibody/states/multibody.hpp>
17 
18 #include "sobec/mpc-walk.hpp"
19 
20 namespace sobec {
21 using namespace crocoddyl;
22 
23 MPCWalkParams::MPCWalkParams()
24  : vcomRef(3), solver_th_stop(1e-9), stateRegCostName("stateReg") {}
25 
26 MPCWalk::MPCWalk(boost::shared_ptr<MPCWalkParams> params,
27  boost::shared_ptr<ShootingProblem> problem)
28  : params(params), storage(problem) {
29  // std::cout << "Constructor" << std::endl;
30 }
31 
32 void MPCWalk::initialize(const std::vector<Eigen::VectorXd>& xs,
33  const std::vector<Eigen::VectorXd>& us) {
34  MPCWalkParams& p = *params;
35  assert(p.Tmpc > 0);
36  assert(p.Tstart + p.Tend + 2 * (p.Tdouble + p.Tsingle) <= storage->get_T());
37 
38  if (p.x0.size() == 0) p.x0 = storage->get_x0();
39 
40  // Init shooting problem for mpc solver
41  ActionList runmodels;
42  for (int t = 0; t < p.Tmpc; ++t) {
43  // std::cout << storage->get_runningModels().size() << " " << t <<
44  // std::endl;
45  runmodels.push_back(storage->get_runningModels()[t]);
46  }
47  AMA termmodel = storage->get_terminalModel();
48  problem = boost::make_shared<ShootingProblem>(p.x0, runmodels, termmodel);
49 
50  findTerminalStateResidualModel();
51  findStateModel();
52  updateTerminalCost(0);
53 
54  // Init solverc
55  solver = boost::make_shared<SolverFDDP>(problem);
56  solver->set_th_stop(p.solver_th_stop);
57  solver->set_reg_min(p.solver_reg_min);
58 
59  reg = p.solver_reg_min;
60 
61  // Run first solve
62  solver->solve(xs, us);
63 }
64 
65 void MPCWalk::findStateModel() {
66  state = boost::dynamic_pointer_cast<StateMultibody>(
67  problem->get_terminalModel()->get_state());
68 }
69 
70 void MPCWalk::findTerminalStateResidualModel() {
71  boost::shared_ptr<IntegratedActionModelEuler> iam =
72  boost::dynamic_pointer_cast<IntegratedActionModelEuler>(
73  problem->get_terminalModel());
74  assert(iam != 0);
75  // std::cout << "IAM" << std::endl;
76 
77  boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> dam =
78  boost::dynamic_pointer_cast<
79  crocoddyl::DifferentialActionModelContactFwdDynamics>(
80  iam->get_differential());
81  assert(dam != 0);
82  // std::cout<<"DAM0" << *dam << std::endl;
83 
84  boost::shared_ptr<CostModelSum> costsum = dam->get_costs();
85  // std::cout << "Cost sum" << std::endl;
86 
87  const CostModelSum::CostModelContainer& costs = costsum->get_costs();
88  // std::cout << "Costs" << std::endl;
89  assert(costs.find(params->stateRegCostName) != costs.end());
90 
91  boost::shared_ptr<CostModelAbstract> costa =
92  boost::const_pointer_cast<CostModelAbstract>(
93  costs.at(params->stateRegCostName)->cost);
94  // std::cout << "IAM" << std::endl;
95 
96  boost::shared_ptr<CostModelResidual> cost =
97  boost::dynamic_pointer_cast<CostModelResidual>(costa);
98  assert(cost != 0);
99  // std::cout << "Cost" << std::endl;
100 
101  cost->get_residual();
102  boost::shared_ptr<ResidualModelState> residual =
103  boost::dynamic_pointer_cast<ResidualModelState>(cost->get_residual());
104  assert(residual != 0);
105  residual->get_reference();
106  // std::cout << "Res" << std::endl;
107 
108  this->terminalStateResidual = residual;
109 }
110 
111 void MPCWalk::updateTerminalCost(const int t) {
112  const MPCWalkParams& p = *params;
113  VectorXd xref = p.x0;
114  xref.head<3>() += p.vcomRef * (t + p.Tmpc) * p.DT;
115  terminalStateResidual->set_reference(xref);
116 }
117 
118 void MPCWalk::calc(const Eigen::Ref<const VectorXd>& x, const int t) {
119  // std::cout << "calc Tmpc=" << Tmpc << std::endl;
120 
121  const MPCWalkParams& p = *params;
122 
124  updateTerminalCost(t);
125 
127  int Tcycle = 2 * (p.Tsingle + p.Tdouble), Tinit = p.Tstart + p.Tdouble / 2;
128 
129  int tlast = Tinit + ((t + p.Tmpc - Tinit) % Tcycle);
130  // std::cout << "tlast = " << tlast << std::endl;
131  problem->circularAppend(storage->get_runningModels()[tlast],
132  storage->get_runningDatas()[tlast]);
133 
135  std::vector<Eigen::VectorXd>& xs_opt =
136  const_cast<std::vector<Eigen::VectorXd>&>(solver->get_xs());
137  std::vector<Eigen::VectorXd> xs_guess(xs_opt.begin() + 1, xs_opt.end());
138  xs_guess.push_back(xs_guess.back());
139 
140  std::vector<Eigen::VectorXd>& us_opt =
141  const_cast<std::vector<Eigen::VectorXd>&>(solver->get_us());
142  std::vector<Eigen::VectorXd> us_guess(us_opt.begin() + 1, us_opt.end());
143  us_guess.push_back(us_guess.back());
144 
145  solver->setCandidate(xs_guess, us_guess);
146 
148  problem->set_x0(x);
149 
151  solver->solve(xs_guess, us_guess, p.solver_maxiter, false, reg);
152  reg = solver->get_xreg();
153 }
154 
155 } // namespace sobec
sobec
Definition: contact-force.hxx:11