10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/numdiff/diff-action.hpp" 15 template <
typename Scalar>
16 DifferentialActionModelNumDiffTpl<Scalar>::DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model,
17 bool with_gauss_approx)
18 : Base(model->get_state(), model->get_nu(), model->get_nr()), model_(model) {
19 with_gauss_approx_ = with_gauss_approx;
20 disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
21 assert_pretty((!with_gauss_approx_ || nr_ > 1),
"No Gauss approximation possible with nr = 1");
24 template <
typename Scalar>
25 DifferentialActionModelNumDiffTpl<Scalar>::~DifferentialActionModelNumDiffTpl() {}
27 template <
typename Scalar>
28 void DifferentialActionModelNumDiffTpl<Scalar>::calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
29 const Eigen::Ref<const VectorXs>& x,
30 const Eigen::Ref<const VectorXs>& u) {
31 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
32 throw_pretty(
"Invalid argument: " 33 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
35 if (static_cast<std::size_t>(u.size()) != nu_) {
36 throw_pretty(
"Invalid argument: " 37 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
39 DifferentialActionDataNumDiff* data_nd =
static_cast<DifferentialActionDataNumDiff*
>(data.get());
40 model_->calc(data_nd->data_0, x, u);
41 data->cost = data_nd->data_0->cost;
42 data->xout = data_nd->data_0->xout;
45 template <
typename Scalar>
46 void DifferentialActionModelNumDiffTpl<Scalar>::calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
47 const Eigen::Ref<const VectorXs>& x,
48 const Eigen::Ref<const VectorXs>& u) {
49 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
50 throw_pretty(
"Invalid argument: " 51 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
53 if (static_cast<std::size_t>(u.size()) != nu_) {
54 throw_pretty(
"Invalid argument: " 55 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
57 boost::shared_ptr<DifferentialActionDataNumDiff> data_nd =
58 boost::static_pointer_cast<DifferentialActionDataNumDiff>(data);
60 const VectorXs& xn0 = data_nd->data_0->xout;
61 const Scalar& c0 = data_nd->data_0->cost;
62 data->xout = data_nd->data_0->xout;
63 data->cost = data_nd->data_0->cost;
65 assertStableStateFD(x);
68 data_nd->dx.setZero();
69 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
70 data_nd->dx(ix) = disturbance_;
71 model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
72 model_->calc(data_nd->data_x[ix], data_nd->xp, u);
74 const VectorXs& xn = data_nd->data_x[ix]->xout;
75 const Scalar& c = data_nd->data_x[ix]->cost;
76 data->Fx.col(ix) = (xn - xn0) / disturbance_;
78 data->Lx(ix) = (c - c0) / disturbance_;
79 data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
80 data_nd->dx(ix) = 0.0;
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]->xout;
90 const Scalar& c = data_nd->data_u[iu]->cost;
91 data->Fu.col(iu) = (xn - xn0) / disturbance_;
93 data->Lu(iu) = (c - c0) / disturbance_;
94 data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
95 data_nd->du(iu) = 0.0;
98 if (with_gauss_approx_) {
99 data->Lxx = data_nd->Rx.transpose() * data_nd->Rx;
100 data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
101 data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
105 template <
typename Scalar>
106 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > DifferentialActionModelNumDiffTpl<Scalar>::createData() {
107 return boost::make_shared<DifferentialActionDataNumDiff>(
this);
110 template <
typename Scalar>
111 const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
112 DifferentialActionModelNumDiffTpl<Scalar>::get_model()
const {
116 template <
typename Scalar>
117 const Scalar& DifferentialActionModelNumDiffTpl<Scalar>::get_disturbance()
const {
121 template <
typename Scalar>
122 void DifferentialActionModelNumDiffTpl<Scalar>::set_disturbance(
const Scalar& disturbance) {
123 if (disturbance < 0.) {
124 throw_pretty(
"Invalid argument: " 125 <<
"Disturbance value is positive");
127 disturbance_ = disturbance;
130 template <
typename Scalar>
131 bool DifferentialActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
132 return with_gauss_approx_;
135 template <
typename Scalar>
136 void DifferentialActionModelNumDiffTpl<Scalar>::assertStableStateFD(
const Eigen::Ref<const VectorXs>& ) {
Definition: action-base.hxx:11