11 #include "crocoddyl/core/utils/exception.hpp" 12 #include "crocoddyl/core/integrator/euler.hpp" 16 template <
typename Scalar>
17 IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
18 boost::shared_ptr<DifferentialActionModelAbstract> model,
const Scalar& time_step,
const bool& with_cost_residual)
19 : Base(model->get_state(), model->get_nu(), model->get_nr()),
21 time_step_(time_step),
22 time_step2_(time_step * time_step),
23 with_cost_residual_(with_cost_residual),
24 enable_integration_(true) {
25 Base::set_u_lb(differential_->get_u_lb());
26 Base::set_u_ub(differential_->get_u_ub());
27 if (time_step_ < 0.) {
29 time_step2_ = time_step_ * time_step_;
30 std::cerr <<
"Warning: dt has positive value, set to 1e-3" << std::endl;
32 if (time_step == 0.) {
33 enable_integration_ =
false;
37 template <
typename Scalar>
38 IntegratedActionModelEulerTpl<Scalar>::~IntegratedActionModelEulerTpl() {}
40 template <
typename Scalar>
41 void IntegratedActionModelEulerTpl<Scalar>::calc(
const boost::shared_ptr<ActionDataAbstract>& data,
42 const Eigen::Ref<const VectorXs>& x,
43 const Eigen::Ref<const VectorXs>& u) {
44 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
45 throw_pretty(
"Invalid argument: " 46 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
48 if (static_cast<std::size_t>(u.size()) != nu_) {
49 throw_pretty(
"Invalid argument: " 50 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
54 boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
57 differential_->calc(d->differential, x, u);
60 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
61 x.tail(differential_->get_state()->get_nv());
62 const VectorXs& a = d->differential->xout;
63 if (enable_integration_) {
64 d->dx << v * time_step_ + a * time_step2_, a * time_step_;
65 differential_->get_state()->integrate(x, d->dx, d->xnext);
66 d->cost = time_step_ * d->differential->cost;
70 d->cost = d->differential->cost;
74 if (with_cost_residual_) {
75 d->r = d->differential->r;
79 template <
typename Scalar>
80 void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x,
82 const Eigen::Ref<const VectorXs>& u) {
83 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
84 throw_pretty(
"Invalid argument: " 85 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
87 if (static_cast<std::size_t>(u.size()) != nu_) {
88 throw_pretty(
"Invalid argument: " 89 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
92 const std::size_t& nv = differential_->get_state()->get_nv();
95 boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
98 differential_->calcDiff(d->differential, x, u);
99 differential_->get_state()->Jintegrate(x, d->dx, d->dxnext_dx, d->dxnext_ddx);
101 d->Fx = d->dxnext_dx;
102 if (enable_integration_) {
103 const MatrixXs& da_dx = d->differential->Fx;
104 const MatrixXs& da_du = d->differential->Fu;
105 d->ddx_dx << da_dx * time_step_, da_dx;
106 d->ddx_du << da_du * time_step_, da_du;
107 for (std::size_t i = 0; i < nv; ++i) {
108 d->ddx_dx(i, i + nv) += 1.;
110 d->Fx.noalias() += time_step_ * (d->dxnext_ddx * d->ddx_dx);
111 d->Fu.noalias() = time_step_ * (d->dxnext_ddx * d->ddx_du);
112 d->Lx = time_step_ * d->differential->Lx;
113 d->Lu = time_step_ * d->differential->Lu;
114 d->Lxx = time_step_ * d->differential->Lxx;
115 d->Lxu = time_step_ * d->differential->Lxu;
116 d->Luu = time_step_ * d->differential->Luu;
119 d->Lx = d->differential->Lx;
120 d->Lu = d->differential->Lu;
121 d->Lxx = d->differential->Lxx;
122 d->Lxu = d->differential->Lxu;
123 d->Luu = d->differential->Luu;
127 template <
typename Scalar>
128 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > IntegratedActionModelEulerTpl<Scalar>::createData() {
129 return boost::make_shared<IntegratedActionDataEuler>(
this);
132 template <
typename Scalar>
133 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
134 IntegratedActionModelEulerTpl<Scalar>::get_differential()
const {
135 return differential_;
138 template <
typename Scalar>
139 const Scalar& IntegratedActionModelEulerTpl<Scalar>::get_dt()
const {
143 template <
typename Scalar>
144 void IntegratedActionModelEulerTpl<Scalar>::set_dt(
const Scalar& dt) {
146 throw_pretty(
"Invalid argument: " 147 <<
"dt has positive value");
150 time_step2_ = dt * dt;
153 template <
typename Scalar>
154 void IntegratedActionModelEulerTpl<Scalar>::set_differential(
155 boost::shared_ptr<DifferentialActionModelAbstract> model) {
156 const std::size_t& nu = model->get_nu();
159 unone_ = VectorXs::Zero(nu_);
161 nr_ = model->get_nr();
162 state_ = model->get_state();
163 differential_ = model;
164 Base::set_u_lb(differential_->get_u_lb());
165 Base::set_u_ub(differential_->get_u_ub());
Definition: action-base.hxx:11