42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 typedef _Scalar Scalar;
58 typedef typename MathBase::VectorXs VectorXs;
59 typedef typename MathBase::MatrixXs MatrixXs;
71 std::shared_ptr<DifferentialActionModelAbstract> model,
72 std::shared_ptr<ControlParametrizationModelAbstract> control,
73 const RKType rktype,
const Scalar time_step = Scalar(1e-3),
74 const bool with_cost_residual =
true);
88 std::shared_ptr<DifferentialActionModelAbstract> model,
89 const RKType rktype,
const Scalar time_step = Scalar(1e-3),
90 const bool with_cost_residual =
true);
100 virtual void calc(
const std::shared_ptr<ActionDataAbstract>& data,
101 const Eigen::Ref<const VectorXs>& x,
102 const Eigen::Ref<const VectorXs>& u)
override;
114 virtual void calc(
const std::shared_ptr<ActionDataAbstract>& data,
115 const Eigen::Ref<const VectorXs>& x)
override;
124 virtual void calcDiff(
const std::shared_ptr<ActionDataAbstract>& data,
125 const Eigen::Ref<const VectorXs>& x,
126 const Eigen::Ref<const VectorXs>& u)
override;
138 virtual void calcDiff(
const std::shared_ptr<ActionDataAbstract>& data,
139 const Eigen::Ref<const VectorXs>& x)
override;
146 virtual std::shared_ptr<ActionDataAbstract>
createData()
override;
157 template <
typename NewScalar>
164 const std::shared_ptr<ActionDataAbstract>& data)
override;
179 virtual void quasiStatic(
const std::shared_ptr<ActionDataAbstract>& data,
180 Eigen::Ref<VectorXs> u,
181 const Eigen::Ref<const VectorXs>& x,
182 const std::size_t maxiter = 100,
183 const Scalar tol = Scalar(1e-9))
override;
195 virtual void print(std::ostream& os)
const override;
213 void set_rk_type(
const RKType rktype);
216 std::vector<Scalar> rk_c_;
223 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
225 typedef _Scalar Scalar;
232 typedef typename MathBase::VectorXs VectorXs;
233 typedef typename MathBase::MatrixXs MatrixXs;
235 template <
template <
typename Scalar>
class Model>
238 integral(model->get_ni(), Scalar(0.)),
239 dx(model->get_state()->get_ndx()),
240 ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
241 y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
242 ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
243 dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
244 dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
245 model->get_state()->get_ndx())),
247 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
248 dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
249 model->get_state()->get_ndx())),
251 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
252 dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
253 dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
255 MatrixXs::Zero(model->get_state()->get_ndx(),
256 model->get_state()->get_ndx())),
258 MatrixXs::Zero(model->get_control()->get_nw(),
259 model->get_control()->get_nw())),
261 MatrixXs::Zero(model->get_nu(), model->get_nu())),
263 MatrixXs::Zero(model->get_state()->get_ndx(),
264 model->get_control()->get_nw())),
265 ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
269 MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
270 Luu_partialx(model->get_ni(),
271 MatrixXs::Zero(model->get_nu(), model->get_nu())),
272 Lxu_i(model->get_ni(),
273 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
274 Lxx_partialx(model->get_ni(),
275 MatrixXs::Zero(model->get_state()->get_ndx(),
276 model->get_state()->get_ndx())),
279 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
282 for (std::size_t i = 0; i < model->get_ni(); ++i) {
283 differential.push_back(std::shared_ptr<DifferentialActionDataAbstract>(
284 model->get_differential()->createData()));
285 control.push_back(std::shared_ptr<ControlParametrizationDataAbstract>(
286 model->get_control()->createData()));
289 const std::size_t nv = model->get_state()->get_nv();
290 dyi_dx[0].diagonal().setOnes();
291 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
295 std::vector<std::shared_ptr<DifferentialActionDataAbstract> >
297 std::vector<std::shared_ptr<ControlParametrizationDataAbstract> >
299 std::vector<Scalar> integral;
301 std::vector<VectorXs>
ki;
302 std::vector<VectorXs>
304 std::vector<VectorXs>
ws;
305 std::vector<VectorXs> dx_rk;
307 std::vector<MatrixXs>
310 std::vector<MatrixXs>
314 std::vector<MatrixXs>
317 std::vector<MatrixXs>
321 std::vector<VectorXs>
324 std::vector<VectorXs>
328 std::vector<MatrixXs>
331 std::vector<MatrixXs>
338 std::vector<MatrixXs>
342 std::vector<MatrixXs>
346 std::vector<MatrixXs>
351 std::vector<MatrixXs> Luu_partialx;
352 std::vector<MatrixXs> Lxu_i;
353 std::vector<MatrixXs> Lxx_partialx;
354 std::vector<MatrixXs> Lxx_partialu;