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> 24 : vcomRef(3), solver_th_stop(1e-9), stateRegCostName(
"stateReg") {}
27 boost::shared_ptr<ShootingProblem> problem)
28 : params(params), storage(problem) {
33 const std::vector<Eigen::VectorXd>& us) {
42 for (
int t = 0; t < p.
Tmpc; ++t) {
45 runmodels.push_back(
storage->get_runningModels()[t]);
48 problem = boost::make_shared<ShootingProblem>(p.
x0, runmodels, termmodel);
66 state = boost::dynamic_pointer_cast<StateMultibody>(
67 problem->get_terminalModel()->get_state());
71 boost::shared_ptr<IntegratedActionModelEuler> iam =
72 boost::dynamic_pointer_cast<IntegratedActionModelEuler>(
77 boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> dam =
78 boost::dynamic_pointer_cast<
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();
113 VectorXd xref = p.
x0;
129 int tlast = Tinit + ((t + p.
Tmpc - Tinit) % Tcycle);
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);
int Tend
Duration of the end phase of the OCP.
Definition: mpc.hpp:40
void findStateModel()
Definition: mpc.hxx:65
void findTerminalStateResidualModel()
Definition: mpc.hxx:70
int Tmpc
Duration of the MPC horizon.
Definition: mpc.hpp:32
double DT
timestep in problem shooting nodes
Definition: mpc.hpp:42
MPC manager, calling iterative subpart of a larger OCP.
Definition: mpc.hpp:26
boost::shared_ptr< ShootingProblem > storage
The reference shooting problem storing all shooting nodes.
Definition: mpc.hpp:94
double reg
Definition: mpc.hpp:109
int Tstart
Duration of start phase of the OCP.
Definition: mpc.hpp:34
boost::shared_ptr< ShootingProblem > problem
the MPC problem used for solving.
Definition: mpc.hpp:97
MPCWalkParams()
Definition: mpc.hxx:23
Eigen::Vector3d vcomRef
reference COM velocity
Definition: mpc.hpp:28
double solver_reg_min
solver param reg_min
Definition: mpc.hpp:46
boost::shared_ptr< StateMultibody > state
Keep a direct reference to the terminal state.
Definition: mpc.hpp:106
int Tsingle
Duration of single-support phases of the OCP.
Definition: mpc.hpp:38
void initialize(const std::vector< Eigen::VectorXd > &xs, const std::vector< Eigen::VectorXd > &us)
once all fields are set, init the mpc manager with guess traj
Definition: mpc.hxx:32
void updateTerminalCost(const int t)
Definition: mpc.hxx:111
Definition: activation-quad-ref.hpp:19
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MPCWalk(boost::shared_ptr< MPCWalkParams > params, boost::shared_ptr< ShootingProblem > problem)
Definition: mpc.hxx:26
int solver_maxiter
Solver max number of iteration.
Definition: mpc.hpp:48
boost::shared_ptr< ResidualModelState > terminalStateResidual
Keep a direct reference to the terminal residual.
Definition: mpc.hpp:103
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
boost::shared_ptr< MPCWalkParams > params
Parameters to tune the algorithm, given at init.
Definition: mpc.hpp:91
boost::shared_ptr< SolverFDDP > solver
Solver for MPC.
Definition: mpc.hpp:100
Eigen::VectorXd x0
reference 0 state
Definition: mpc.hpp:30
void calc(const Eigen::Ref< const VectorXd > &x, const int t)
calc the OCP solution. Init must be called first.
Definition: mpc.hxx:118
double solver_th_stop
stop threshold to configure the solver
Definition: mpc.hpp:44
int Tdouble
Duration of double-support phases of the OCP.
Definition: mpc.hpp:36