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 MPCWalk::MPCWalk(boost::shared_ptr<ShootingProblem> problem)
24  : vcomRef(3),
25  solver_th_stop(1e-9),
26  stateRegCostName("stateReg")
27 
28  ,
29  storage(problem) {
30  // std::cout << "Constructor" << std::endl;
31 }
32 
33 void MPCWalk::initialize(const std::vector<Eigen::VectorXd>& xs,
34  const std::vector<Eigen::VectorXd>& us) {
35  assert(Tmpc > 0);
36  assert(Tstart + Tend + 2 * (Tdouble + Tsingle) <= storage->get_T());
37 
38  if (x0.size() == 0) x0 = storage->get_x0();
39 
40  // Init shooting problem for mpc solver
41  ActionList runmodels;
42  for (int t = 0; t < Tmpc; ++t) {
43  // std::cout << storage->get_runningModels().size() << " " << t <<
44  // std::endl;
45  runmodels.push_back(storage->get_runningModels()[t]);
46  }
47  ActionPtr termmodel = storage->get_terminalModel();
48  problem = boost::make_shared<ShootingProblem>(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(solver_th_stop);
57  solver->set_reg_min(solver_reg_min);
58 
59  reg = 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(stateRegCostName) != costs.end());
90 
91  boost::shared_ptr<CostModelAbstract> costa =
92  boost::const_pointer_cast<CostModelAbstract>(
93  costs.at(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  VectorXd xref = x0;
113  xref.head<3>() += vcomRef * (t + Tmpc) * DT;
114  terminalStateResidual->set_reference(xref);
115 }
116 
117 void MPCWalk::calc(const Eigen::Ref<const VectorXd>& x, const int t) {
118  // std::cout << "calc Tmpc=" << Tmpc << std::endl;
119 
121  updateTerminalCost(t);
122 
124  int Tcycle = 2 * (Tsingle + Tdouble);
125  int tlast = Tstart + 1 + ((t + Tmpc - Tstart - 1) % Tcycle);
126  // std::cout << "tlast = " << tlast << std::endl;
127  problem->circularAppend(storage->get_runningModels()[tlast],
128  storage->get_runningDatas()[tlast]);
129 
131  std::vector<Eigen::VectorXd>& xs_opt =
132  const_cast<std::vector<Eigen::VectorXd>&>(solver->get_xs());
133  std::vector<Eigen::VectorXd> xs_guess(xs_opt.begin() + 1, xs_opt.end());
134  xs_guess.push_back(xs_guess.back());
135 
136  std::vector<Eigen::VectorXd>& us_opt =
137  const_cast<std::vector<Eigen::VectorXd>&>(solver->get_us());
138  std::vector<Eigen::VectorXd> us_guess(us_opt.begin() + 1, us_opt.end());
139  us_guess.push_back(us_guess.back());
140 
141  solver->setCandidate(xs_guess, us_guess);
142 
144  problem->set_x0(x);
145 
147  solver->solve(xs_guess, us_guess, solver_maxiter, false, reg);
148  reg = solver->get_xreg();
149 }
150 
151 } // namespace sobec
Definition: contact-force.hxx:11