Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/action.hxx	Lines:	113	136	83.1 %
Date:	2024-02-13 11:12:33	Branches:	61	260	23.5 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          New York University, Max Planck Gesellschaft,
//                          University of Oxford, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/numdiff/action.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
ActionModelNumDiffTpl<Scalar>::ActionModelNumDiffTpl(
    boost::shared_ptr<Base> model, bool with_gauss_approx)
    : Base(model->get_state(), model->get_nu(), model->get_nr(),
           model->get_ng(), model->get_nh()),
      model_(model),
      e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())),
      with_gauss_approx_(with_gauss_approx) {
  e_hess_ = std::sqrt(2.0 * e_jac_);
  this->set_u_lb(model_->get_u_lb());
  this->set_u_ub(model_->get_u_ub());
}

template <typename Scalar>
ActionModelNumDiffTpl<Scalar>::~ActionModelNumDiffTpl() {}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  Data* d = static_cast<Data*>(data.get());
  model_->calc(d->data_0, x, u);
  data->xnext = d->data_0->xnext;
  data->cost = d->data_0->cost;
  d->g = d->data_0->g;
  d->h = d->data_0->h;
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());
  model_->calc(d->data_0, x);
  data->xnext = d->data_0->xnext;
  data->cost = d->data_0->cost;
  d->g = d->data_0->g;
  d->h = d->data_0->h;
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  const VectorXs& x0 = d->data_0->xnext;
  const Scalar c0 = d->data_0->cost;
  data->xnext = d->data_0->xnext;
  data->cost = d->data_0->cost;
  const VectorXs& g0 = d->g;
  const VectorXs& h0 = d->h;
  const std::size_t ndx = model_->get_state()->get_ndx();
  const std::size_t nu = model_->get_nu();
  const std::size_t ng = model_->get_ng();
  const std::size_t nh = model_->get_nh();
  d->Gx.resize(ng, ndx);
  d->Gu.resize(ng, nu);
  d->Hx.resize(nh, ndx);
  d->Hu.resize(nh, nu);
  d->du.setZero();

  assertStableStateFD(x);

  // Computing the d action(x,u) / dx
  model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
  d->x_norm = d->dx.norm();
  d->dx.setZero();
  d->xh_jac = e_jac_ * std::max(1., d->x_norm);
  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
    d->dx(ix) = d->xh_jac;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp, u);
    // dynamics
    model_->get_state()->diff(x0, d->data_x[ix]->xnext, d->Fx.col(ix));
    // cost
    data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
    if (get_with_gauss_approx()) {
      d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
    }
    // constraint
    data->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
    data->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
    d->dx(ix) = 0.;
  }
  data->Fx /= d->xh_jac;

  // Computing the d action(x,u) / du
  d->uh_jac = e_jac_ * std::max(1., u.norm());
  for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
    d->du(iu) = d->uh_jac;
    model_->calc(d->data_u[iu], x, u + d->du);
    // dynamics
    model_->get_state()->diff(x0, d->data_u[iu]->xnext, d->Fu.col(iu));
    // cost
    data->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
    if (get_with_gauss_approx()) {
      d->Ru.col(iu) = (d->data_u[iu]->r - d->data_0->r) / d->uh_jac;
    }
    // constraint
    d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
    d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
    d->du(iu) = 0.0;
  }
  data->Fu /= d->uh_jac;

#ifdef NDEBUG
  // Computing the d^2 cost(x,u) / dx^2
  d->xh_hess = e_hess_ * std::max(1., d->x_norm);
  d->xh_hess_pow2 = d->xh_hess * d->xh_hess;
  for (std::size_t ix = 0; ix < ndx; ++ix) {
    d->dx(ix) = d->xh_hess;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp, u);
    const Scalar cp = d->data_x[ix]->cost;
    model_->get_state()->integrate(x, -d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp, u);
    const Scalar cm = d->data_x[ix]->cost;
    data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2;
    for (std::size_t jx = ix + 1; jx < ndx; ++jx) {
      d->dx(jx) = d->xh_hess;
      model_->get_state()->integrate(x, d->dx, d->xp);
      model_->calc(d->data_x[ix], d->xp, u);
      const Scalar cpp =
          d->data_x[ix]
              ->cost;  // cost due to positive disturbance in both directions
      d->dx(ix) = 0.;
      model_->get_state()->integrate(x, d->dx, d->xp);
      model_->calc(d->data_x[ix], d->xp, u);
      const Scalar czp =
          d->data_x[ix]->cost;  // cost due to zero disturance in 'i' and
                                // positive disturbance in 'j' direction
      data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2;
      data->Lxx(jx, ix) = data->Lxx(ix, jx);
      d->dx(ix) = d->xh_hess;
      d->dx(jx) = 0.;
    }
    d->dx(ix) = 0.;
  }

  // Computing the d^2 cost(x,u) / du^2
  d->uh_hess = e_hess_ * std::max(1., u.norm());
  d->uh_hess_pow2 = d->uh_hess * d->uh_hess;
  for (std::size_t iu = 0; iu < nu; ++iu) {
    d->du(iu) = d->uh_hess;
    model_->calc(d->data_u[iu], x, u + d->du);
    const Scalar cp = d->data_u[iu]->cost;
    model_->calc(d->data_u[iu], x, u - d->du);
    const Scalar cm = d->data_u[iu]->cost;
    data->Luu(iu, iu) = (cp - 2 * c0 + cm) / d->uh_hess_pow2;
    for (std::size_t ju = iu + 1; ju < nu; ++ju) {
      d->du(ju) = d->uh_hess;
      model_->calc(d->data_u[iu], x, u + d->du);
      const Scalar cpp =
          d->data_u[iu]
              ->cost;  // cost due to positive disturbance in both directions
      d->du(iu) = 0.;
      model_->calc(d->data_u[iu], x, u + d->du);
      const Scalar czp =
          d->data_u[iu]->cost;  // cost due to zero disturance in 'i' and
                                // positive disturbance in 'j' direction
      data->Luu(iu, ju) = (cpp - czp - cp + c0) / d->uh_hess_pow2;
      data->Luu(ju, iu) = data->Luu(iu, ju);
      d->du(iu) = d->uh_hess;
      d->du(ju) = 0.;
    }
    d->du(iu) = 0.;
  }

  // Computing the d^2 cost(x,u) / dxu
  d->xuh_hess_pow2 = 4. * d->xh_hess * d->uh_hess;
  for (std::size_t ix = 0; ix < ndx; ++ix) {
    for (std::size_t ju = 0; ju < nu; ++ju) {
      d->dx(ix) = d->xh_hess;
      model_->get_state()->integrate(x, d->dx, d->xp);
      d->du(ju) = d->uh_hess;
      model_->calc(d->data_x[ix], d->xp, u + d->du);
      const Scalar cpp = d->data_x[ix]->cost;
      model_->calc(d->data_x[ix], d->xp, u - d->du);
      const Scalar cpm = d->data_x[ix]->cost;
      model_->get_state()->integrate(x, -d->dx, d->xp);
      model_->calc(d->data_x[ix], d->xp, u + d->du);
      const Scalar cmp = d->data_x[ix]->cost;
      model_->calc(d->data_x[ix], d->xp, u - d->du);
      const Scalar cmm = d->data_x[ix]->cost;
      data->Lxu(ix, ju) = (cpp - cpm - cmp + cmm) / d->xuh_hess_pow2;
      d->dx(ix) = 0.;
      d->du(ju) = 0.;
    }
  }
#endif

  if (get_with_gauss_approx()) {
    data->Lxx = d->Rx.transpose() * d->Rx;
    data->Lxu = d->Rx.transpose() * d->Ru;
    data->Luu = d->Ru.transpose() * d->Ru;
  }
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  const Scalar c0 = d->data_0->cost;
  data->xnext = d->data_0->xnext;
  data->cost = d->data_0->cost;
  const VectorXs& g0 = d->g;
  const VectorXs& h0 = d->h;
  const std::size_t ndx = model_->get_state()->get_ndx();
  d->Gx.resize(model_->get_ng(), ndx);
  d->Hx.resize(model_->get_nh(), ndx);

  assertStableStateFD(x);

  // Computing the d action(x,u) / dx
  model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
  d->x_norm = d->dx.norm();
  d->dx.setZero();
  d->xh_jac = e_jac_ * std::max(1., d->x_norm);
  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
    d->dx(ix) = d->xh_jac;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp);
    // cost
    data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
    if (get_with_gauss_approx()) {
      d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
    }
    // constraint
    d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
    d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
    d->dx(ix) = 0.;
  }

#ifdef NDEBUG
  // Computing the d^2 cost(x,u) / dx^2
  d->xh_hess = e_hess_ * std::max(1., d->x_norm);
  d->xh_hess_pow2 = d->xh_hess * d->xh_hess;
  for (std::size_t ix = 0; ix < ndx; ++ix) {
    // We can apply the same formulas for finite difference as above
    d->dx(ix) = d->xh_hess;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp);
    const Scalar cp = d->data_x[ix]->cost;
    model_->get_state()->integrate(x, -d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp);
    const Scalar cm = d->data_x[ix]->cost;
    data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2;
    for (std::size_t jx = ix + 1; jx < ndx; ++jx) {
      d->dx(jx) = d->xh_hess;
      model_->get_state()->integrate(x, d->dx, d->xp);
      model_->calc(d->data_x[ix], d->xp);
      const Scalar cpp =
          d->data_x[ix]
              ->cost;  // cost due to positive disturbance in both directions
      d->dx(ix) = 0.;
      model_->get_state()->integrate(x, d->dx, d->xp);
      model_->calc(d->data_x[ix], d->xp);
      const Scalar czp =
          d->data_x[ix]->cost;  // cost due to zero disturance in 'i' and
                                // positive disturbance in 'j' direction
      data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2;
      data->Lxx(jx, ix) = data->Lxx(ix, jx);
      d->dx(ix) = d->xh_hess;
      d->dx(jx) = 0.;
    }
    d->dx(ix) = 0.;
  }
#endif

  if (get_with_gauss_approx()) {
    data->Lxx = d->Rx.transpose() * d->Rx;
  }
}

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
ActionModelNumDiffTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
    const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
    const Scalar tol) {
  Data* d = static_cast<Data*>(data.get());
  model_->quasiStatic(d->data_0, u, x, maxiter, tol);
}

template <typename Scalar>
const boost::shared_ptr<ActionModelAbstractTpl<Scalar> >&
ActionModelNumDiffTpl<Scalar>::get_model() const {
  return model_;
}

template <typename Scalar>
const Scalar ActionModelNumDiffTpl<Scalar>::get_disturbance() const {
  return e_jac_;
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
  if (disturbance < 0.) {
    throw_pretty("Invalid argument: "
                 << "Disturbance constant is positive");
  }
  e_jac_ = disturbance;
  e_hess_ = std::sqrt(2.0 * e_jac_);
}

template <typename Scalar>
bool ActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
  return with_gauss_approx_;
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::print(std::ostream& os) const {
  os << "ActionModelNumDiffTpl {action=" << *model_ << "}";
}

template <typename Scalar>
void ActionModelNumDiffTpl<Scalar>::assertStableStateFD(
    const Eigen::Ref<const VectorXs>& /** x */) {
  // do nothing in the general case
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5			// New York University, Max Planck Gesellschaft,
6			// University of Oxford, Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#include "crocoddyl/core/numdiff/action.hpp"
12			#include "crocoddyl/core/utils/exception.hpp"
13
14			namespace crocoddyl {
15
16			template <typename Scalar>
17		258	ActionModelNumDiffTpl<Scalar>::ActionModelNumDiffTpl(
18			boost::shared_ptr<Base> model, bool with_gauss_approx)
19			: Base(model->get_state(), model->get_nu(), model->get_nr(),
20		516	model->get_ng(), model->get_nh()),
21			model_(model),
22		258	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())),
23	✓✗	258	with_gauss_approx_(with_gauss_approx) {
24		258	e_hess_ = std::sqrt(2.0 * e_jac_);
25	✓✗	258	this->set_u_lb(model_->get_u_lb());
26	✓✗	258	this->set_u_ub(model_->get_u_ub());
27		258	}
28
29			template <typename Scalar>
30		520	ActionModelNumDiffTpl<Scalar>::~ActionModelNumDiffTpl() {}
31
32			template <typename Scalar>
33		257	void ActionModelNumDiffTpl<Scalar>::calc(
34			const boost::shared_ptr<ActionDataAbstract>& data,
35			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
36	✗✓	257	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
37			throw_pretty("Invalid argument: "
38			<< "x has wrong dimension (it should be " +
39			std::to_string(state_->get_nx()) + ")");
40			}
41	✗✓	257	if (static_cast<std::size_t>(u.size()) != nu_) {
42			throw_pretty("Invalid argument: "
43			<< "u has wrong dimension (it should be " +
44			std::to_string(nu_) + ")");
45			}
46		257	Data* d = static_cast<Data*>(data.get());
47		257	model_->calc(d->data_0, x, u);
48		257	data->xnext = d->data_0->xnext;
49		257	data->cost = d->data_0->cost;
50		257	d->g = d->data_0->g;
51		257	d->h = d->data_0->h;
52		257	}
53
54			template <typename Scalar>
55		245	void ActionModelNumDiffTpl<Scalar>::calc(
56			const boost::shared_ptr<ActionDataAbstract>& data,
57			const Eigen::Ref<const VectorXs>& x) {
58	✗✓	245	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
59			throw_pretty("Invalid argument: "
60			<< "x has wrong dimension (it should be " +
61			std::to_string(state_->get_nx()) + ")");
62			}
63		245	Data* d = static_cast<Data*>(data.get());
64		245	model_->calc(d->data_0, x);
65		245	data->xnext = d->data_0->xnext;
66		245	data->cost = d->data_0->cost;
67		245	d->g = d->data_0->g;
68		245	d->h = d->data_0->h;
69		245	}
70
71			template <typename Scalar>
72		257	void ActionModelNumDiffTpl<Scalar>::calcDiff(
73			const boost::shared_ptr<ActionDataAbstract>& data,
74			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
75	✗✓	257	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
76			throw_pretty("Invalid argument: "
77			<< "x has wrong dimension (it should be " +
78			std::to_string(state_->get_nx()) + ")");
79			}
80	✗✓	257	if (static_cast<std::size_t>(u.size()) != nu_) {
81			throw_pretty("Invalid argument: "
82			<< "u has wrong dimension (it should be " +
83			std::to_string(nu_) + ")");
84			}
85		257	Data* d = static_cast<Data*>(data.get());
86
87		257	const VectorXs& x0 = d->data_0->xnext;
88		257	const Scalar c0 = d->data_0->cost;
89		257	data->xnext = d->data_0->xnext;
90		257	data->cost = d->data_0->cost;
91		257	const VectorXs& g0 = d->g;
92		257	const VectorXs& h0 = d->h;
93		257	const std::size_t ndx = model_->get_state()->get_ndx();
94		257	const std::size_t nu = model_->get_nu();
95		257	const std::size_t ng = model_->get_ng();
96		257	const std::size_t nh = model_->get_nh();
97		257	d->Gx.resize(ng, ndx);
98		257	d->Gu.resize(ng, nu);
99		257	d->Hx.resize(nh, ndx);
100		257	d->Hu.resize(nh, nu);
101		257	d->du.setZero();
102
103		257	assertStableStateFD(x);
104
105			// Computing the d action(x,u) / dx
106	✓✗✓✗ ✓✗	257	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
107		257	d->x_norm = d->dx.norm();
108		257	d->dx.setZero();
109		257	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
110	✓✓	10820	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
111		10563	d->dx(ix) = d->xh_jac;
112	✓✗✓✗	10563	model_->get_state()->integrate(x, d->dx, d->xp);
113	✓✗	10563	model_->calc(d->data_x[ix], d->xp, u);
114			// dynamics
115	✓✗✓✗ ✓✗✓✗	10563	model_->get_state()->diff(x0, d->data_x[ix]->xnext, d->Fx.col(ix));
116			// cost
117		10563	data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
118	✗✓	10563	if (get_with_gauss_approx()) {
119			d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
120			}
121			// constraint
122	✓✗✓✗ ✓✗	10563	data->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
123	✓✗✓✗ ✓✗	10563	data->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
124		10563	d->dx(ix) = 0.;
125			}
126		257	data->Fx /= d->xh_jac;
127
128			// Computing the d action(x,u) / du
129		257	d->uh_jac = e_jac_ * std::max(1., u.norm());
130	✓✓	6887	for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
131		6630	d->du(iu) = d->uh_jac;
132	✓✗✓✗	6630	model_->calc(d->data_u[iu], x, u + d->du);
133			// dynamics
134	✓✗✓✗ ✓✗✓✗	6630	model_->get_state()->diff(x0, d->data_u[iu]->xnext, d->Fu.col(iu));
135			// cost
136		6630	data->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
137	✗✓	6630	if (get_with_gauss_approx()) {
138			d->Ru.col(iu) = (d->data_u[iu]->r - d->data_0->r) / d->uh_jac;
139			}
140			// constraint
141	✓✗✓✗ ✓✗	6630	d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
142	✓✗✓✗ ✓✗	6630	d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
143		6630	d->du(iu) = 0.0;
144			}
145		257	data->Fu /= d->uh_jac;
146
147			#ifdef NDEBUG
148			// Computing the d^2 cost(x,u) / dx^2
149			d->xh_hess = e_hess_ * std::max(1., d->x_norm);
150			d->xh_hess_pow2 = d->xh_hess * d->xh_hess;
151			for (std::size_t ix = 0; ix < ndx; ++ix) {
152			d->dx(ix) = d->xh_hess;
153			model_->get_state()->integrate(x, d->dx, d->xp);
154			model_->calc(d->data_x[ix], d->xp, u);
155			const Scalar cp = d->data_x[ix]->cost;
156			model_->get_state()->integrate(x, -d->dx, d->xp);
157			model_->calc(d->data_x[ix], d->xp, u);
158			const Scalar cm = d->data_x[ix]->cost;
159			data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2;
160			for (std::size_t jx = ix + 1; jx < ndx; ++jx) {
161			d->dx(jx) = d->xh_hess;
162			model_->get_state()->integrate(x, d->dx, d->xp);
163			model_->calc(d->data_x[ix], d->xp, u);
164			const Scalar cpp =
165			d->data_x[ix]
166			->cost; // cost due to positive disturbance in both directions
167			d->dx(ix) = 0.;
168			model_->get_state()->integrate(x, d->dx, d->xp);
169			model_->calc(d->data_x[ix], d->xp, u);
170			const Scalar czp =
171			d->data_x[ix]->cost; // cost due to zero disturance in 'i' and
172			// positive disturbance in 'j' direction
173			data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2;
174			data->Lxx(jx, ix) = data->Lxx(ix, jx);
175			d->dx(ix) = d->xh_hess;
176			d->dx(jx) = 0.;
177			}
178			d->dx(ix) = 0.;
179			}
180
181			// Computing the d^2 cost(x,u) / du^2
182			d->uh_hess = e_hess_ * std::max(1., u.norm());
183			d->uh_hess_pow2 = d->uh_hess * d->uh_hess;
184			for (std::size_t iu = 0; iu < nu; ++iu) {
185			d->du(iu) = d->uh_hess;
186			model_->calc(d->data_u[iu], x, u + d->du);
187			const Scalar cp = d->data_u[iu]->cost;
188			model_->calc(d->data_u[iu], x, u - d->du);
189			const Scalar cm = d->data_u[iu]->cost;
190			data->Luu(iu, iu) = (cp - 2 * c0 + cm) / d->uh_hess_pow2;
191			for (std::size_t ju = iu + 1; ju < nu; ++ju) {
192			d->du(ju) = d->uh_hess;
193			model_->calc(d->data_u[iu], x, u + d->du);
194			const Scalar cpp =
195			d->data_u[iu]
196			->cost; // cost due to positive disturbance in both directions
197			d->du(iu) = 0.;
198			model_->calc(d->data_u[iu], x, u + d->du);
199			const Scalar czp =
200			d->data_u[iu]->cost; // cost due to zero disturance in 'i' and
201			// positive disturbance in 'j' direction
202			data->Luu(iu, ju) = (cpp - czp - cp + c0) / d->uh_hess_pow2;
203			data->Luu(ju, iu) = data->Luu(iu, ju);
204			d->du(iu) = d->uh_hess;
205			d->du(ju) = 0.;
206			}
207			d->du(iu) = 0.;
208			}
209
210			// Computing the d^2 cost(x,u) / dxu
211			d->xuh_hess_pow2 = 4. * d->xh_hess * d->uh_hess;
212			for (std::size_t ix = 0; ix < ndx; ++ix) {
213			for (std::size_t ju = 0; ju < nu; ++ju) {
214			d->dx(ix) = d->xh_hess;
215			model_->get_state()->integrate(x, d->dx, d->xp);
216			d->du(ju) = d->uh_hess;
217			model_->calc(d->data_x[ix], d->xp, u + d->du);
218			const Scalar cpp = d->data_x[ix]->cost;
219			model_->calc(d->data_x[ix], d->xp, u - d->du);
220			const Scalar cpm = d->data_x[ix]->cost;
221			model_->get_state()->integrate(x, -d->dx, d->xp);
222			model_->calc(d->data_x[ix], d->xp, u + d->du);
223			const Scalar cmp = d->data_x[ix]->cost;
224			model_->calc(d->data_x[ix], d->xp, u - d->du);
225			const Scalar cmm = d->data_x[ix]->cost;
226			data->Lxu(ix, ju) = (cpp - cpm - cmp + cmm) / d->xuh_hess_pow2;
227			d->dx(ix) = 0.;
228			d->du(ju) = 0.;
229			}
230			}
231			#endif
232
233	✗✓	257	if (get_with_gauss_approx()) {
234			data->Lxx = d->Rx.transpose() * d->Rx;
235			data->Lxu = d->Rx.transpose() * d->Ru;
236			data->Luu = d->Ru.transpose() * d->Ru;
237			}
238		257	}
239
240			template <typename Scalar>
241		245	void ActionModelNumDiffTpl<Scalar>::calcDiff(
242			const boost::shared_ptr<ActionDataAbstract>& data,
243			const Eigen::Ref<const VectorXs>& x) {
244	✗✓	245	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
245			throw_pretty("Invalid argument: "
246			<< "x has wrong dimension (it should be " +
247			std::to_string(state_->get_nx()) + ")");
248			}
249		245	Data* d = static_cast<Data*>(data.get());
250
251		245	const Scalar c0 = d->data_0->cost;
252		245	data->xnext = d->data_0->xnext;
253		245	data->cost = d->data_0->cost;
254		245	const VectorXs& g0 = d->g;
255		245	const VectorXs& h0 = d->h;
256		245	const std::size_t ndx = model_->get_state()->get_ndx();
257		245	d->Gx.resize(model_->get_ng(), ndx);
258		245	d->Hx.resize(model_->get_nh(), ndx);
259
260		245	assertStableStateFD(x);
261
262			// Computing the d action(x,u) / dx
263	✓✗✓✗ ✓✗	245	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
264		245	d->x_norm = d->dx.norm();
265		245	d->dx.setZero();
266		245	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
267	✓✓	10036	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
268		9791	d->dx(ix) = d->xh_jac;
269	✓✗✓✗	9791	model_->get_state()->integrate(x, d->dx, d->xp);
270	✓✗	9791	model_->calc(d->data_x[ix], d->xp);
271			// cost
272		9791	data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
273	✗✓	9791	if (get_with_gauss_approx()) {
274			d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
275			}
276			// constraint
277	✓✗✓✗ ✓✗	9791	d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
278	✓✗✓✗ ✓✗	9791	d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
279		9791	d->dx(ix) = 0.;
280			}
281
282			#ifdef NDEBUG
283			// Computing the d^2 cost(x,u) / dx^2
284			d->xh_hess = e_hess_ * std::max(1., d->x_norm);
285			d->xh_hess_pow2 = d->xh_hess * d->xh_hess;
286			for (std::size_t ix = 0; ix < ndx; ++ix) {
287			// We can apply the same formulas for finite difference as above
288			d->dx(ix) = d->xh_hess;
289			model_->get_state()->integrate(x, d->dx, d->xp);
290			model_->calc(d->data_x[ix], d->xp);
291			const Scalar cp = d->data_x[ix]->cost;
292			model_->get_state()->integrate(x, -d->dx, d->xp);
293			model_->calc(d->data_x[ix], d->xp);
294			const Scalar cm = d->data_x[ix]->cost;
295			data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2;
296			for (std::size_t jx = ix + 1; jx < ndx; ++jx) {
297			d->dx(jx) = d->xh_hess;
298			model_->get_state()->integrate(x, d->dx, d->xp);
299			model_->calc(d->data_x[ix], d->xp);
300			const Scalar cpp =
301			d->data_x[ix]
302			->cost; // cost due to positive disturbance in both directions
303			d->dx(ix) = 0.;
304			model_->get_state()->integrate(x, d->dx, d->xp);
305			model_->calc(d->data_x[ix], d->xp);
306			const Scalar czp =
307			d->data_x[ix]->cost; // cost due to zero disturance in 'i' and
308			// positive disturbance in 'j' direction
309			data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2;
310			data->Lxx(jx, ix) = data->Lxx(ix, jx);
311			d->dx(ix) = d->xh_hess;
312			d->dx(jx) = 0.;
313			}
314			d->dx(ix) = 0.;
315			}
316			#endif
317
318	✗✓	245	if (get_with_gauss_approx()) {
319			data->Lxx = d->Rx.transpose() * d->Rx;
320			}
321		245	}
322
323			template <typename Scalar>
324			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
325		259	ActionModelNumDiffTpl<Scalar>::createData() {
326	✓✗	259	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
327			}
328
329			template <typename Scalar>
330			void ActionModelNumDiffTpl<Scalar>::quasiStatic(
331			const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
332			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
333			const Scalar tol) {
334			Data* d = static_cast<Data*>(data.get());
335			model_->quasiStatic(d->data_0, u, x, maxiter, tol);
336			}
337
338			template <typename Scalar>
339			const boost::shared_ptr<ActionModelAbstractTpl<Scalar> >&
340		19791	ActionModelNumDiffTpl<Scalar>::get_model() const {
341		19791	return model_;
342			}
343
344			template <typename Scalar>
345		257	const Scalar ActionModelNumDiffTpl<Scalar>::get_disturbance() const {
346		257	return e_jac_;
347			}
348
349			template <typename Scalar>
350			void ActionModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
351			if (disturbance < 0.) {
352			throw_pretty("Invalid argument: "
353			<< "Disturbance constant is positive");
354			}
355			e_jac_ = disturbance;
356			e_hess_ = std::sqrt(2.0 * e_jac_);
357			}
358
359			template <typename Scalar>
360		27976	bool ActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
361		27976	return with_gauss_approx_;
362			}
363
364			template <typename Scalar>
365			void ActionModelNumDiffTpl<Scalar>::print(std::ostream& os) const {
366			os << "ActionModelNumDiffTpl {action=" << *model_ << "}";
367			}
368
369			template <typename Scalar>
370		502	void ActionModelNumDiffTpl<Scalar>::assertStableStateFD(
371			const Eigen::Ref<const VectorXs>& /** x */) {
372			// do nothing in the general case
373		502	}
374
375			} // namespace crocoddyl