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> 18 #include "sobec/mpc-walk.hpp" 23 MPCWalk::MPCWalk(boost::shared_ptr<ShootingProblem> problem)
26 stateRegCostName(
"stateReg")
33 void MPCWalk::initialize(
const std::vector<Eigen::VectorXd>& xs,
34 const std::vector<Eigen::VectorXd>& us) {
36 assert(Tstart + Tend + 2 * (Tdouble + Tsingle) <= storage->get_T());
38 if (x0.size() == 0) x0 = storage->get_x0();
42 for (
int t = 0; t < Tmpc; ++t) {
45 runmodels.push_back(storage->get_runningModels()[t]);
47 ActionPtr termmodel = storage->get_terminalModel();
48 problem = boost::make_shared<ShootingProblem>(x0, runmodels, termmodel);
50 findTerminalStateResidualModel();
52 updateTerminalCost(0);
55 solver = boost::make_shared<SolverFDDP>(problem);
56 solver->set_th_stop(solver_th_stop);
57 solver->set_reg_min(solver_reg_min);
62 solver->solve(xs, us);
65 void MPCWalk::findStateModel() {
66 state = boost::dynamic_pointer_cast<StateMultibody>(
67 problem->get_terminalModel()->get_state());
70 void MPCWalk::findTerminalStateResidualModel() {
71 boost::shared_ptr<IntegratedActionModelEuler> iam =
72 boost::dynamic_pointer_cast<IntegratedActionModelEuler>(
73 problem->get_terminalModel());
77 boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> dam =
78 boost::dynamic_pointer_cast<
79 crocoddyl::DifferentialActionModelContactFwdDynamics>(
80 iam->get_differential());
84 boost::shared_ptr<CostModelSum> costsum = dam->get_costs();
87 const CostModelSum::CostModelContainer& costs = costsum->get_costs();
89 assert(costs.find(stateRegCostName) != costs.end());
91 boost::shared_ptr<CostModelAbstract> costa =
92 boost::const_pointer_cast<CostModelAbstract>(
93 costs.at(stateRegCostName)->cost);
96 boost::shared_ptr<CostModelResidual> cost =
97 boost::dynamic_pointer_cast<CostModelResidual>(costa);
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();
108 this->terminalStateResidual = residual;
111 void MPCWalk::updateTerminalCost(
const int t) {
113 xref.head<3>() += vcomRef * (t + Tmpc) * DT;
114 terminalStateResidual->set_reference(xref);
117 void MPCWalk::calc(
const Eigen::Ref<const VectorXd>& x,
const int t) {
121 updateTerminalCost(t);
124 int Tcycle = 2 * (Tsingle + Tdouble);
125 int tlast = Tstart + 1 + ((t + Tmpc - Tstart - 1) % Tcycle);
127 problem->circularAppend(storage->get_runningModels()[tlast],
128 storage->get_runningDatas()[tlast]);
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());
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());
141 solver->setCandidate(xs_guess, us_guess);
147 solver->solve(xs_guess, us_guess, solver_maxiter,
false, reg);
148 reg = solver->get_xreg();
Definition: contact-force.hxx:11