Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/cost.hxx	Lines:	94	98	95.9 %
Date:	2024-02-13 11:12:33	Branches:	65	126	51.6 %


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

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

namespace crocoddyl {

template <typename Scalar>
CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(
    const boost::shared_ptr<Base>& model)
    : Base(model->get_state(), model->get_activation(), model->get_nu()),
      model_(model),
      e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}

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

template <typename Scalar>
void CostModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<CostDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  Data* d = static_cast<Data*>(data.get());
  d->data_0->cost = 0.0;
  model_->calc(d->data_0, x, u);
  d->cost = d->data_0->cost;
  d->residual->r = d->data_0->residual->r;
}

template <typename Scalar>
void CostModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<CostDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  Data* d = static_cast<Data*>(data.get());
  d->data_0->cost = 0.0;
  model_->calc(d->data_0, x);
  d->cost = d->data_0->cost;
  d->residual->r = d->data_0->residual->r;
}

template <typename Scalar>
void CostModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<CostDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  Data* d = static_cast<Data*>(data.get());

  const Scalar c0 = d->cost;
  const VectorXs& r0 = d->residual->r;
  if (get_with_gauss_approx()) {
    model_->get_activation()->calc(d->data_0->activation, r0);
    model_->get_activation()->calcDiff(d->data_0->activation, r0);
  }
  d->du.setZero();

  assertStableStateFD(x);

  // Computing the d cost(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);
    // call the update function on the pinocchio data
    for (size_t i = 0; i < reevals_.size(); ++i) {
      reevals_[i](d->xp, u);
    }
    model_->calc(d->data_x[ix], d->xp, u);
    d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
    if (get_with_gauss_approx()) {
      d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
    }
    d->dx(ix) = 0.;
  }

  // Computing the d cost(x,u) / du
  d->uh_jac = e_jac_ * std::max(1., u.norm());
  for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
    d->du(iu) = d->uh_jac;
    d->up = u + d->du;
    // call the update function
    for (std::size_t i = 0; i < reevals_.size(); ++i) {
      reevals_[i](x, d->up);
    }
    model_->calc(d->data_u[iu], x, d->up);
    d->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
    if (get_with_gauss_approx()) {
      d->residual->Ru.col(iu) = (d->data_u[iu]->residual->r - r0) / d->uh_jac;
    }
    d->du(iu) = 0.;
  }

  if (get_with_gauss_approx()) {
    const MatrixXs& Arr = d->data_0->activation->Arr;
    d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
    d->Lxu = d->residual->Rx.transpose() * Arr * d->residual->Ru;
    d->Luu = d->residual->Ru.transpose() * Arr * d->residual->Ru;
  } else {
    d->Lxx.fill(0.0);
    d->Lxu.fill(0.0);
    d->Luu.fill(0.0);
  }
}

template <typename Scalar>
void CostModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<CostDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  Data* d = static_cast<Data*>(data.get());

  const Scalar c0 = d->cost;
  const VectorXs& r0 = d->residual->r;
  if (get_with_gauss_approx()) {
    model_->get_activation()->calc(d->data_0->activation, r0);
    model_->get_activation()->calcDiff(d->data_0->activation, r0);
  }
  d->dx.setZero();

  assertStableStateFD(x);

  // Computing the d cost(x,u) / dx
  d->xh_jac = e_jac_ * std::max(1., 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);
    // call the update function on the pinocchio data
    for (size_t i = 0; i < reevals_.size(); ++i) {
      reevals_[i](d->xp, unone_);
    }
    model_->calc(d->data_x[ix], d->xp);
    d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
    if (get_with_gauss_approx()) {
      d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
    }
    d->dx(ix) = 0.;
  }

  if (get_with_gauss_approx()) {
    const MatrixXs& Arr = d->data_0->activation->Arr;
    d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
  } else {
    d->Lxx.fill(0.0);
  }
}

template <typename Scalar>
boost::shared_ptr<CostDataAbstractTpl<Scalar> >
CostModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
                                      data);
}

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

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

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

template <typename Scalar>
bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
  return activation_->get_nr() > 0;
}

template <typename Scalar>
void CostModelNumDiffTpl<Scalar>::set_reevals(
    const std::vector<ReevaluationFunction>& reevals) {
  reevals_ = reevals;
}

template <typename Scalar>
void CostModelNumDiffTpl<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, University of Edinburgh, LAAS-CNRS,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "crocoddyl/core/numdiff/cost.hpp"
11			#include "crocoddyl/core/utils/exception.hpp"
12
13			namespace crocoddyl {
14
15			template <typename Scalar>
16		652	CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(
17			const boost::shared_ptr<Base>& model)
18			: Base(model->get_state(), model->get_activation(), model->get_nu()),
19			model_(model),
20	✓✗	652	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
21
22			template <typename Scalar>
23		1304	CostModelNumDiffTpl<Scalar>::~CostModelNumDiffTpl() {}
24
25			template <typename Scalar>
26		652	void CostModelNumDiffTpl<Scalar>::calc(
27			const boost::shared_ptr<CostDataAbstract>& data,
28			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
29		652	Data* d = static_cast<Data*>(data.get());
30		652	d->data_0->cost = 0.0;
31		652	model_->calc(d->data_0, x, u);
32		652	d->cost = d->data_0->cost;
33		652	d->residual->r = d->data_0->residual->r;
34		652	}
35
36			template <typename Scalar>
37		324	void CostModelNumDiffTpl<Scalar>::calc(
38			const boost::shared_ptr<CostDataAbstract>& data,
39			const Eigen::Ref<const VectorXs>& x) {
40		324	Data* d = static_cast<Data*>(data.get());
41		324	d->data_0->cost = 0.0;
42		324	model_->calc(d->data_0, x);
43		324	d->cost = d->data_0->cost;
44		324	d->residual->r = d->data_0->residual->r;
45		324	}
46
47			template <typename Scalar>
48		326	void CostModelNumDiffTpl<Scalar>::calcDiff(
49			const boost::shared_ptr<CostDataAbstract>& data,
50			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
51		326	Data* d = static_cast<Data*>(data.get());
52
53		326	const Scalar c0 = d->cost;
54		326	const VectorXs& r0 = d->residual->r;
55	✓✗	326	if (get_with_gauss_approx()) {
56	✓✗	326	model_->get_activation()->calc(d->data_0->activation, r0);
57	✓✗	326	model_->get_activation()->calcDiff(d->data_0->activation, r0);
58			}
59		326	d->du.setZero();
60
61		326	assertStableStateFD(x);
62
63			// Computing the d cost(x,u) / dx
64	✓✗✓✗ ✓✗	326	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
65		326	d->x_norm = d->dx.norm();
66		326	d->dx.setZero();
67		326	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
68	✓✓	13404	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
69		13078	d->dx(ix) = d->xh_jac;
70	✓✗✓✗	13078	model_->get_state()->integrate(x, d->dx, d->xp);
71			// call the update function on the pinocchio data
72	✓✓	26282	for (size_t i = 0; i < reevals_.size(); ++i) {
73	✓✗	13204	reevals_[i](d->xp, u);
74			}
75	✓✗	13078	model_->calc(d->data_x[ix], d->xp, u);
76		13078	d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
77	✓✗	13078	if (get_with_gauss_approx()) {
78	✓✗✓✗ ✓✗	13078	d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
79			}
80		13078	d->dx(ix) = 0.;
81			}
82
83			// Computing the d cost(x,u) / du
84		326	d->uh_jac = e_jac_ * std::max(1., u.norm());
85	✓✓	6865	for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
86		6539	d->du(iu) = d->uh_jac;
87	✓✗	6539	d->up = u + d->du;
88			// call the update function
89	✓✓	13141	for (std::size_t i = 0; i < reevals_.size(); ++i) {
90	✓✗	6602	reevals_[i](x, d->up);
91			}
92	✓✗	6539	model_->calc(d->data_u[iu], x, d->up);
93		6539	d->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
94	✓✗	6539	if (get_with_gauss_approx()) {
95	✓✗✓✗ ✓✗	6539	d->residual->Ru.col(iu) = (d->data_u[iu]->residual->r - r0) / d->uh_jac;
96			}
97		6539	d->du(iu) = 0.;
98			}
99
100	✓✗	326	if (get_with_gauss_approx()) {
101	✓✗	326	const MatrixXs& Arr = d->data_0->activation->Arr;
102	✓✗✓✗ ✓✗✓✗	326	d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
103	✓✗✓✗ ✓✗✓✗	326	d->Lxu = d->residual->Rx.transpose() * Arr * d->residual->Ru;
104	✓✗✓✗ ✓✗✓✗	326	d->Luu = d->residual->Ru.transpose() * Arr * d->residual->Ru;
105			} else {
106			d->Lxx.fill(0.0);
107			d->Lxu.fill(0.0);
108			d->Luu.fill(0.0);
109			}
110		326	}
111
112			template <typename Scalar>
113		324	void CostModelNumDiffTpl<Scalar>::calcDiff(
114			const boost::shared_ptr<CostDataAbstract>& data,
115			const Eigen::Ref<const VectorXs>& x) {
116		324	Data* d = static_cast<Data*>(data.get());
117
118		324	const Scalar c0 = d->cost;
119		324	const VectorXs& r0 = d->residual->r;
120	✓✗	324	if (get_with_gauss_approx()) {
121	✓✗	324	model_->get_activation()->calc(d->data_0->activation, r0);
122	✓✗	324	model_->get_activation()->calcDiff(d->data_0->activation, r0);
123			}
124		324	d->dx.setZero();
125
126		324	assertStableStateFD(x);
127
128			// Computing the d cost(x,u) / dx
129		324	d->xh_jac = e_jac_ * std::max(1., x.norm());
130	✓✓	13302	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
131		12978	d->dx(ix) = d->xh_jac;
132	✓✗✓✗	12978	model_->get_state()->integrate(x, d->dx, d->xp);
133			// call the update function on the pinocchio data
134	✓✓	26082	for (size_t i = 0; i < reevals_.size(); ++i) {
135		13104	reevals_[i](d->xp, unone_);
136			}
137	✓✗	12978	model_->calc(d->data_x[ix], d->xp);
138		12978	d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
139	✓✗	12978	if (get_with_gauss_approx()) {
140	✓✗✓✗ ✓✗	12978	d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
141			}
142		12978	d->dx(ix) = 0.;
143			}
144
145	✓✗	324	if (get_with_gauss_approx()) {
146	✓✗	324	const MatrixXs& Arr = d->data_0->activation->Arr;
147	✓✗✓✗ ✓✗✓✗	324	d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
148			} else {
149			d->Lxx.fill(0.0);
150			}
151		324	}
152
153			template <typename Scalar>
154			boost::shared_ptr<CostDataAbstractTpl<Scalar> >
155		652	CostModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
156			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
157	✓✗	652	data);
158			}
159
160			template <typename Scalar>
161			const boost::shared_ptr<CostModelAbstractTpl<Scalar> >&
162		41190	CostModelNumDiffTpl<Scalar>::get_model() const {
163		41190	return model_;
164			}
165
166			template <typename Scalar>
167		326	const Scalar CostModelNumDiffTpl<Scalar>::get_disturbance() const {
168		326	return e_jac_;
169			}
170
171			template <typename Scalar>
172			void CostModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
173			if (disturbance < 0.) {
174			throw_pretty("Invalid argument: "
175			<< "Disturbance constant is positive");
176			}
177			e_jac_ = disturbance;
178			}
179
180			template <typename Scalar>
181		34545	bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
182		34545	return activation_->get_nr() > 0;
183			}
184
185			template <typename Scalar>
186		326	void CostModelNumDiffTpl<Scalar>::set_reevals(
187			const std::vector<ReevaluationFunction>& reevals) {
188		326	reevals_ = reevals;
189		326	}
190
191			template <typename Scalar>
192		650	void CostModelNumDiffTpl<Scalar>::assertStableStateFD(
193			const Eigen::Ref<const VectorXs>& /x/) {
194			// do nothing in the general case
195		650	}
196
197			} // namespace crocoddyl