10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/numdiff/action.hpp" 15 template <
typename Scalar>
16 ActionModelNumDiffTpl<Scalar>::ActionModelNumDiffTpl(boost::shared_ptr<Base> model)
17 : Base(model->get_state(), model->get_nu(), model->get_nr()), model_(model) {
18 disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
21 template <
typename Scalar>
22 ActionModelNumDiffTpl<Scalar>::~ActionModelNumDiffTpl() {}
24 template <
typename Scalar>
25 void ActionModelNumDiffTpl<Scalar>::calc(
const boost::shared_ptr<ActionDataAbstract>& data,
26 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
27 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
28 throw_pretty(
"Invalid argument: " 29 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
31 if (static_cast<std::size_t>(u.size()) != nu_) {
32 throw_pretty(
"Invalid argument: " 33 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
35 boost::shared_ptr<ActionDataNumDiffTpl<Scalar> > data_nd =
36 boost::static_pointer_cast<ActionDataNumDiffTpl<Scalar> >(data);
37 model_->calc(data_nd->data_0, x, u);
38 data->cost = data_nd->data_0->cost;
39 data->xnext = data_nd->data_0->xnext;
42 template <
typename Scalar>
43 void ActionModelNumDiffTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
44 const Eigen::Ref<const VectorXs>& x,
45 const Eigen::Ref<const VectorXs>& u) {
46 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
47 throw_pretty(
"Invalid argument: " 48 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
50 if (static_cast<std::size_t>(u.size()) != nu_) {
51 throw_pretty(
"Invalid argument: " 52 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
54 boost::shared_ptr<ActionDataNumDiffTpl<Scalar> > data_nd =
55 boost::static_pointer_cast<ActionDataNumDiffTpl<Scalar> >(data);
57 const VectorXs& xn0 = data_nd->data_0->xnext;
58 const Scalar& c0 = data_nd->data_0->cost;
59 data->xnext = data_nd->data_0->xnext;
60 data->cost = data_nd->data_0->cost;
62 assertStableStateFD(x);
65 data_nd->dx.setZero();
66 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
67 data_nd->dx(ix) = disturbance_;
68 model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
69 model_->calc(data_nd->data_x[ix], data_nd->xp, u);
71 const VectorXs& xn = data_nd->data_x[ix]->xnext;
72 const Scalar& c = data_nd->data_x[ix]->cost;
73 model_->get_state()->diff(xn0, xn, data_nd->Fx.col(ix));
75 data->Lx(ix) = (c - c0) / disturbance_;
76 if (get_with_gauss_approx() > 0) {
77 data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
79 data_nd->dx(ix) = 0.0;
81 data->Fx /= disturbance_;
84 data_nd->du.setZero();
85 for (
unsigned iu = 0; iu < model_->get_nu(); ++iu) {
86 data_nd->du(iu) = disturbance_;
87 model_->calc(data_nd->data_u[iu], x, u + data_nd->du);
89 const VectorXs& xn = data_nd->data_u[iu]->xnext;
90 const Scalar& c = data_nd->data_u[iu]->cost;
91 model_->get_state()->diff(xn0, xn, data_nd->Fu.col(iu));
93 data->Lu(iu) = (c - c0) / disturbance_;
94 if (get_with_gauss_approx() > 0) {
95 data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
97 data_nd->du(iu) = 0.0;
99 data->Fu /= disturbance_;
101 if (get_with_gauss_approx() > 0) {
102 data->Lxx = data_nd->Rx.transpose() * data_nd->Rx;
103 data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
104 data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
112 template <
typename Scalar>
113 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelNumDiffTpl<Scalar>::createData() {
114 return boost::make_shared<ActionDataNumDiffTpl<Scalar> >(
this);
117 template <
typename Scalar>
118 const boost::shared_ptr<ActionModelAbstractTpl<Scalar> >& ActionModelNumDiffTpl<Scalar>::get_model()
const {
122 template <
typename Scalar>
123 const Scalar& ActionModelNumDiffTpl<Scalar>::get_disturbance()
const {
127 template <
typename Scalar>
128 void ActionModelNumDiffTpl<Scalar>::set_disturbance(
const Scalar& disturbance) {
129 if (disturbance < 0.) {
130 throw_pretty(
"Invalid argument: " 131 <<
"Disturbance value is positive");
133 disturbance_ = disturbance;
136 template <
typename Scalar>
137 bool ActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
138 return model_->get_nr() > 0;
141 template <
typename Scalar>
142 void ActionModelNumDiffTpl<Scalar>::assertStableStateFD(
const Eigen::Ref<const VectorXs>& ) {
Definition: action-base.hxx:11