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 MPCWalkParams::MPCWalkParams()
24 : vcomRef(3), solver_th_stop(1e-9), stateRegCostName(
"stateReg") {}
26 MPCWalk::MPCWalk(boost::shared_ptr<MPCWalkParams> params,
27 boost::shared_ptr<ShootingProblem> problem)
28 : params(params), storage(problem) {
32 void MPCWalk::initialize(
const std::vector<Eigen::VectorXd>& xs,
33 const std::vector<Eigen::VectorXd>& us) {
34 MPCWalkParams& p = *params;
36 assert(p.Tstart + p.Tend + 2 * (p.Tdouble + p.Tsingle) <= storage->get_T());
38 if (p.x0.size() == 0) p.x0 = storage->get_x0();
42 for (
int t = 0; t < p.Tmpc; ++t) {
45 runmodels.push_back(storage->get_runningModels()[t]);
47 AMA termmodel = storage->get_terminalModel();
48 problem = boost::make_shared<ShootingProblem>(p.x0, runmodels, termmodel);
50 findTerminalStateResidualModel();
52 updateTerminalCost(0);
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);
59 reg = p.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(params->stateRegCostName) != costs.end());
91 boost::shared_ptr<CostModelAbstract> costa =
92 boost::const_pointer_cast<CostModelAbstract>(
93 costs.at(params->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) {
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);
118 void MPCWalk::calc(
const Eigen::Ref<const VectorXd>& x,
const int t) {
121 const MPCWalkParams& p = *params;
124 updateTerminalCost(t);
127 int Tcycle = 2 * (p.Tsingle + p.Tdouble), Tinit = p.Tstart + p.Tdouble / 2;
129 int tlast = Tinit + ((t + p.Tmpc - Tinit) % Tcycle);
131 problem->circularAppend(storage->get_runningModels()[tlast],
132 storage->get_runningDatas()[tlast]);
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());
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());
145 solver->setCandidate(xs_guess, us_guess);
151 solver->solve(xs_guess, us_guess, p.solver_maxiter,
false, reg);
152 reg = solver->get_xreg();
Definition: contact-force.hxx:11