Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/residual.hxx	Lines:	66	66	100.0 %
Date:	2024-02-13 11:12:33	Branches:	36	60	60.0 %


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

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

namespace crocoddyl {

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

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

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

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

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

  const VectorXs& r0 = d->r;
  d->dx.setZero();
  d->du.setZero();

  assertStableStateFD(x);

  // Computing the d residual(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
    for (size_t i = 0; i < reevals_.size(); ++i) {
      reevals_[i](d->xp, u);
    }
    model_->calc(d->data_x[ix], d->xp, u);
    d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
    d->dx(ix) = 0.;
  }

  // Computing the d residual(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->Ru.col(iu) = (d->data_u[iu]->r - r0) / d->uh_jac;
    d->du(iu) = 0.;
  }
}

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

  const VectorXs& r0 = d->r;
  assertStableStateFD(x);

  // Computing the d residual(x,u) / dx
  d->dx.setZero();
  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
    for (size_t i = 0; i < reevals_.size(); ++i) {
      reevals_[i](d->xp, unone_);
    }
    model_->calc(d->data_x[ix], d->xp);
    d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
    d->dx(ix) = 0.;
  }
}

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

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

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

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

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

template <typename Scalar>
void ResidualModelNumDiffTpl<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) 2021-2023, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/numdiff/residual.hpp"
10			#include "crocoddyl/core/utils/exception.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		360	ResidualModelNumDiffTpl<Scalar>::ResidualModelNumDiffTpl(
16			const boost::shared_ptr<Base>& model)
17			: Base(model->get_state(), model->get_nr(), model->get_nu()),
18			model_(model),
19	✓✗	360	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21			template <typename Scalar>
22		720	ResidualModelNumDiffTpl<Scalar>::~ResidualModelNumDiffTpl() {}
23
24			template <typename Scalar>
25		360	void ResidualModelNumDiffTpl<Scalar>::calc(
26			const boost::shared_ptr<ResidualDataAbstract>& data,
27			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28		360	Data* d = static_cast<Data*>(data.get());
29		360	model_->calc(d->data_0, x, u);
30		360	d->r = d->data_0->r;
31		360	}
32
33			template <typename Scalar>
34		180	void ResidualModelNumDiffTpl<Scalar>::calc(
35			const boost::shared_ptr<ResidualDataAbstract>& data,
36			const Eigen::Ref<const VectorXs>& x) {
37		180	Data* d = static_cast<Data*>(data.get());
38		180	model_->calc(d->data_0, x);
39		180	d->r = d->data_0->r;
40		180	}
41
42			template <typename Scalar>
43		180	void ResidualModelNumDiffTpl<Scalar>::calcDiff(
44			const boost::shared_ptr<ResidualDataAbstract>& data,
45			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
46		180	Data* d = static_cast<Data*>(data.get());
47
48		180	const VectorXs& r0 = d->r;
49		180	d->dx.setZero();
50		180	d->du.setZero();
51
52		180	assertStableStateFD(x);
53
54			// Computing the d residual(x,u) / dx
55	✓✗✓✗ ✓✗	180	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
56		180	d->x_norm = d->dx.norm();
57		180	d->dx.setZero();
58		180	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
59	✓✓	8060	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
60		7880	d->dx(ix) = d->xh_jac;
61	✓✗✓✗	7880	model_->get_state()->integrate(x, d->dx, d->xp);
62			// call the update function
63	✓✓	23640	for (size_t i = 0; i < reevals_.size(); ++i) {
64	✓✗	15760	reevals_[i](d->xp, u);
65			}
66	✓✗	7880	model_->calc(d->data_x[ix], d->xp, u);
67	✓✗✓✗ ✓✗	7880	d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
68		7880	d->dx(ix) = 0.;
69			}
70
71			// Computing the d residual(x,u) / du
72		180	d->uh_jac = e_jac_ * std::max(1., u.norm());
73	✓✓	3860	for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
74		3680	d->du(iu) = d->uh_jac;
75	✓✗	3680	d->up = u + d->du;
76			// call the update function
77	✓✓	11040	for (std::size_t i = 0; i < reevals_.size(); ++i) {
78	✓✗	7360	reevals_[i](x, d->up);
79			}
80	✓✗	3680	model_->calc(d->data_u[iu], x, d->up);
81	✓✗✓✗ ✓✗	3680	d->Ru.col(iu) = (d->data_u[iu]->r - r0) / d->uh_jac;
82		3680	d->du(iu) = 0.;
83			}
84		180	}
85
86			template <typename Scalar>
87		180	void ResidualModelNumDiffTpl<Scalar>::calcDiff(
88			const boost::shared_ptr<ResidualDataAbstract>& data,
89			const Eigen::Ref<const VectorXs>& x) {
90		180	Data* d = static_cast<Data*>(data.get());
91
92		180	const VectorXs& r0 = d->r;
93		180	assertStableStateFD(x);
94
95			// Computing the d residual(x,u) / dx
96		180	d->dx.setZero();
97	✓✓	8060	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
98		7880	d->dx(ix) = d->xh_jac;
99	✓✗✓✗	7880	model_->get_state()->integrate(x, d->dx, d->xp);
100			// call the update function
101	✓✓	23640	for (size_t i = 0; i < reevals_.size(); ++i) {
102		15760	reevals_[i](d->xp, unone_);
103			}
104	✓✗	7880	model_->calc(d->data_x[ix], d->xp);
105	✓✗✓✗ ✓✗	7880	d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
106		7880	d->dx(ix) = 0.;
107			}
108		180	}
109
110			template <typename Scalar>
111			boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
112		360	ResidualModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
113			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
114	✓✗	360	data);
115			}
116
117			template <typename Scalar>
118			const boost::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
119		24200	ResidualModelNumDiffTpl<Scalar>::get_model() const {
120		24200	return model_;
121			}
122
123			template <typename Scalar>
124		180	const Scalar ResidualModelNumDiffTpl<Scalar>::get_disturbance() const {
125		180	return e_jac_;
126			}
127
128			template <typename Scalar>
129			void ResidualModelNumDiffTpl<Scalar>::set_disturbance(
130			const Scalar disturbance) {
131			if (disturbance < 0.) {
132			throw_pretty("Invalid argument: "
133			<< "Disturbance constant is positive");
134			}
135			e_jac_ = disturbance;
136			}
137
138			template <typename Scalar>
139		360	void ResidualModelNumDiffTpl<Scalar>::set_reevals(
140			const std::vector<ReevaluationFunction>& reevals) {
141		360	reevals_ = reevals;
142		360	}
143
144			template <typename Scalar>
145		360	void ResidualModelNumDiffTpl<Scalar>::assertStableStateFD(
146			const Eigen::Ref<const VectorXs>& /x/) {
147			// do nothing in the general case
148		360	}
149
150			} // namespace crocoddyl