9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/numdiff/cost.hpp" 14 template <
typename Scalar>
15 CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(
const boost::shared_ptr<Base>& model)
16 : Base(model->get_state(), model->get_activation(), model->get_nu()), model_(model) {
17 disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
20 template <
typename Scalar>
21 CostModelNumDiffTpl<Scalar>::~CostModelNumDiffTpl() {}
23 template <
typename Scalar>
24 void CostModelNumDiffTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
26 boost::shared_ptr<CostDataNumDiffTpl<Scalar> > data_nd =
27 boost::static_pointer_cast<CostDataNumDiffTpl<Scalar> >(data);
28 data_nd->data_0->cost = 0.0;
29 model_->calc(data_nd->data_0, x, u);
30 data_nd->cost = data_nd->data_0->cost;
31 data_nd->r = data_nd->data_0->r;
34 template <
typename Scalar>
35 void CostModelNumDiffTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
36 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
37 boost::shared_ptr<CostDataNumDiffTpl<Scalar> > data_nd =
38 boost::static_pointer_cast<CostDataNumDiffTpl<Scalar> >(data);
40 const Scalar& c0 = data_nd->cost;
41 const VectorXs& r0 = data_nd->r;
42 if (get_with_gauss_approx()) {
43 model_->get_activation()->calc(data_nd->data_0->activation, r0);
44 model_->get_activation()->calcDiff(data_nd->data_0->activation, r0);
46 assertStableStateFD(x);
49 data_nd->dx.setZero();
50 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
52 data_nd->dx(ix) = disturbance_;
53 model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
55 for (
size_t i = 0; i < reevals_.size(); ++i) {
56 reevals_[i](data_nd->xp);
59 model_->calc(data_nd->data_x[ix], data_nd->xp, u);
61 data_nd->Lx(ix) = (data_nd->data_x[ix]->cost - c0) / disturbance_;
63 if (get_with_gauss_approx()) {
64 data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - r0) / disturbance_;
66 data_nd->dx(ix) = 0.0;
70 data_nd->du.setZero();
72 for (
size_t i = 0; i < reevals_.size(); ++i) {
75 for (
unsigned iu = 0; iu < model_->get_nu(); ++iu) {
77 data_nd->du(iu) = disturbance_;
78 data_nd->up = u + data_nd->du;
80 model_->calc(data_nd->data_u[iu], x, data_nd->up);
82 data_nd->Lu(iu) = (data_nd->data_u[iu]->cost - c0) / disturbance_;
84 if (get_with_gauss_approx()) {
85 data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - r0) / disturbance_;
87 data_nd->du(iu) = 0.0;
90 if (get_with_gauss_approx()) {
91 const MatrixXs& Arr = data_nd->data_0->activation->Arr;
92 data_nd->Lxx = data_nd->Rx.transpose() * Arr * data_nd->Rx;
93 data_nd->Lxu = data_nd->Rx.transpose() * Arr * data_nd->Ru;
94 data_nd->Luu = data_nd->Ru.transpose() * Arr * data_nd->Ru;
96 data_nd->Lxx.fill(0.0);
97 data_nd->Lxu.fill(0.0);
98 data_nd->Luu.fill(0.0);
102 template <
typename Scalar>
103 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelNumDiffTpl<Scalar>::createData(
104 DataCollectorAbstract*
const data) {
105 return boost::make_shared<CostDataNumDiffTpl<Scalar> >(
this, data);
108 template <
typename Scalar>
109 const boost::shared_ptr<CostModelAbstractTpl<Scalar> >& CostModelNumDiffTpl<Scalar>::get_model()
const {
113 template <
typename Scalar>
114 const Scalar& CostModelNumDiffTpl<Scalar>::get_disturbance()
const {
118 template <
typename Scalar>
119 void CostModelNumDiffTpl<Scalar>::set_disturbance(
const Scalar& disturbance) {
120 disturbance_ = disturbance;
123 template <
typename Scalar>
124 bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
125 return activation_->get_nr() > 0;
128 template <
typename Scalar>
129 void CostModelNumDiffTpl<Scalar>::set_reevals(
const std::vector<ReevaluationFunction>& reevals) {
133 template <
typename Scalar>
134 void CostModelNumDiffTpl<Scalar>::assertStableStateFD(
const Eigen::Ref<const VectorXs>& ) {
Definition: action-base.hxx:11