Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/constraint.hxx	Lines:	96	102	94.1 %
Date:	2024-02-13 11:12:33	Branches:	54	192	28.1 %


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

#include "crocoddyl/core/numdiff/constraint.hpp"

namespace crocoddyl {

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

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

template <typename Scalar>
void ConstraintModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ConstraintDataAbstract>& 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());
  d->data_0->g.setZero();
  d->data_0->h.setZero();
  model_->calc(d->data_0, x, u);
  d->g = d->data_0->g;
  d->h = d->data_0->h;
}

template <typename Scalar>
void ConstraintModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ConstraintDataAbstract>& 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());

  d->data_0->g.setZero();
  d->data_0->h.setZero();
  model_->calc(d->data_0, x);
  d->g = d->data_0->g;
  d->h = d->data_0->h;
}

template <typename Scalar>
void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ConstraintDataAbstract>& 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& 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 constraint(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->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.;
  }

  // Computing the d constraint(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->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.;
  }
}

template <typename Scalar>
void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ConstraintDataAbstract>& 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 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 constraint(x) / 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) {
    // x + dx
    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->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.;
  }
}

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

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

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

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

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

template <typename Scalar>
void ConstraintModelNumDiffTpl<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) 2020-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/constraint.hpp"
10
11			namespace crocoddyl {
12
13			template <typename Scalar>
14		140	ConstraintModelNumDiffTpl<Scalar>::ConstraintModelNumDiffTpl(
15			const boost::shared_ptr<Base>& model)
16			: Base(model->get_state(), model->get_nu(), model->get_ng(),
17			model->get_nh()),
18			model_(model),
19	✓✗	140	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21			template <typename Scalar>
22		280	ConstraintModelNumDiffTpl<Scalar>::~ConstraintModelNumDiffTpl() {}
23
24			template <typename Scalar>
25		140	void ConstraintModelNumDiffTpl<Scalar>::calc(
26			const boost::shared_ptr<ConstraintDataAbstract>& data,
27			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28	✗✓	140	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
29			throw_pretty("Invalid argument: "
30			<< "x has wrong dimension (it should be " +
31			std::to_string(state_->get_nx()) + ")");
32			}
33	✗✓	140	if (static_cast<std::size_t>(u.size()) != nu_) {
34			throw_pretty("Invalid argument: "
35			<< "u has wrong dimension (it should be " +
36			std::to_string(nu_) + ")");
37			}
38
39		140	Data* d = static_cast<Data*>(data.get());
40		140	d->data_0->g.setZero();
41		140	d->data_0->h.setZero();
42		140	model_->calc(d->data_0, x, u);
43		140	d->g = d->data_0->g;
44		140	d->h = d->data_0->h;
45		140	}
46
47			template <typename Scalar>
48		70	void ConstraintModelNumDiffTpl<Scalar>::calc(
49			const boost::shared_ptr<ConstraintDataAbstract>& data,
50			const Eigen::Ref<const VectorXs>& x) {
51	✗✓	70	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
52			throw_pretty("Invalid argument: "
53			<< "x has wrong dimension (it should be " +
54			std::to_string(state_->get_nx()) + ")");
55			}
56		70	Data* d = static_cast<Data*>(data.get());
57
58		70	d->data_0->g.setZero();
59		70	d->data_0->h.setZero();
60		70	model_->calc(d->data_0, x);
61		70	d->g = d->data_0->g;
62		70	d->h = d->data_0->h;
63		70	}
64
65			template <typename Scalar>
66		70	void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
67			const boost::shared_ptr<ConstraintDataAbstract>& data,
68			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
69	✗✓	70	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
70			throw_pretty("Invalid argument: "
71			<< "x has wrong dimension (it should be " +
72			std::to_string(state_->get_nx()) + ")");
73			}
74	✗✓	70	if (static_cast<std::size_t>(u.size()) != nu_) {
75			throw_pretty("Invalid argument: "
76			<< "u has wrong dimension (it should be " +
77			std::to_string(nu_) + ")");
78			}
79		70	Data* d = static_cast<Data*>(data.get());
80
81		70	const VectorXs& g0 = d->g;
82		70	const VectorXs& h0 = d->h;
83		70	const std::size_t ndx = model_->get_state()->get_ndx();
84		70	const std::size_t nu = model_->get_nu();
85		70	const std::size_t ng = model_->get_ng();
86		70	const std::size_t nh = model_->get_nh();
87		70	d->Gx.resize(ng, ndx);
88		70	d->Gu.resize(ng, nu);
89		70	d->Hx.resize(nh, ndx);
90		70	d->Hu.resize(nh, nu);
91		70	d->du.setZero();
92
93		70	assertStableStateFD(x);
94
95			// Computing the d constraint(x,u) / dx
96	✓✗✓✗ ✓✗	70	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
97		70	d->x_norm = d->dx.norm();
98		70	d->dx.setZero();
99		70	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
100	✓✓	2926	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
101		2856	d->dx(ix) = d->xh_jac;
102	✓✗✓✗	2856	model_->get_state()->integrate(x, d->dx, d->xp);
103			// call the update function
104	✓✓	5712	for (size_t i = 0; i < reevals_.size(); ++i) {
105	✓✗	2856	reevals_[i](d->xp, u);
106			}
107	✓✗	2856	model_->calc(d->data_x[ix], d->xp, u);
108	✓✗✓✗ ✓✗	2856	d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
109	✓✗✓✗ ✓✗	2856	d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
110		2856	d->dx(ix) = 0.;
111			}
112
113			// Computing the d constraint(x,u) / du
114		70	d->uh_jac = e_jac_ * std::max(1., u.norm());
115	✓✓	1498	for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
116		1428	d->du(iu) = d->uh_jac;
117	✓✗	1428	d->up = u + d->du;
118			// call the update function
119	✓✓	2856	for (std::size_t i = 0; i < reevals_.size(); ++i) {
120	✓✗	1428	reevals_[i](x, d->up);
121			}
122	✓✗	1428	model_->calc(d->data_u[iu], x, d->up);
123	✓✗✓✗ ✓✗	1428	d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
124	✓✗✓✗ ✓✗	1428	d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
125		1428	d->du(iu) = 0.;
126			}
127		70	}
128
129			template <typename Scalar>
130		70	void ConstraintModelNumDiffTpl<Scalar>::calcDiff(
131			const boost::shared_ptr<ConstraintDataAbstract>& data,
132			const Eigen::Ref<const VectorXs>& x) {
133	✗✓	70	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
134			throw_pretty("Invalid argument: "
135			<< "x has wrong dimension (it should be " +
136			std::to_string(state_->get_nx()) + ")");
137			}
138		70	Data* d = static_cast<Data*>(data.get());
139
140		70	const VectorXs& g0 = d->g;
141		70	const VectorXs& h0 = d->h;
142		70	const std::size_t ndx = model_->get_state()->get_ndx();
143		70	d->Gx.resize(model_->get_ng(), ndx);
144		70	d->Hx.resize(model_->get_nh(), ndx);
145
146		70	assertStableStateFD(x);
147
148			// Computing the d constraint(x) / dx
149	✓✗✓✗ ✓✗	70	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
150		70	d->x_norm = d->dx.norm();
151		70	d->dx.setZero();
152		70	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
153	✓✓	2926	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
154			// x + dx
155		2856	d->dx(ix) = d->xh_jac;
156	✓✗✓✗	2856	model_->get_state()->integrate(x, d->dx, d->xp);
157			// call the update function
158	✓✓	5712	for (size_t i = 0; i < reevals_.size(); ++i) {
159		2856	reevals_[i](d->xp, unone_);
160			}
161	✓✗	2856	model_->calc(d->data_x[ix], d->xp);
162	✓✗✓✗ ✓✗	2856	d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
163	✓✗✓✗ ✓✗	2856	d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
164		2856	d->dx(ix) = 0.;
165			}
166		70	}
167
168			template <typename Scalar>
169			boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
170		140	ConstraintModelNumDiffTpl<Scalar>::createData(
171			DataCollectorAbstract* const data) {
172			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
173	✓✗	140	data);
174			}
175
176			template <typename Scalar>
177			const boost::shared_ptr<ConstraintModelAbstractTpl<Scalar> >&
178		8988	ConstraintModelNumDiffTpl<Scalar>::get_model() const {
179		8988	return model_;
180			}
181
182			template <typename Scalar>
183		70	const Scalar ConstraintModelNumDiffTpl<Scalar>::get_disturbance() const {
184		70	return e_jac_;
185			}
186
187			template <typename Scalar>
188			void ConstraintModelNumDiffTpl<Scalar>::set_disturbance(
189			const Scalar disturbance) {
190			if (disturbance < 0.) {
191			throw_pretty("Invalid argument: "
192			<< "Disturbance constant is positive");
193			}
194			e_jac_ = disturbance;
195			}
196
197			template <typename Scalar>
198		70	void ConstraintModelNumDiffTpl<Scalar>::set_reevals(
199			const std::vector<ReevaluationFunction>& reevals) {
200		70	reevals_ = reevals;
201		70	}
202
203			template <typename Scalar>
204		140	void ConstraintModelNumDiffTpl<Scalar>::assertStableStateFD(
205			const Eigen::Ref<const VectorXs>& /x/) {
206			// do nothing in the general case
207		140	}
208
209			} // namespace crocoddyl