GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, University of Pisa,
6			// Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			namespace crocoddyl {
12
13			template <typename Scalar>
14		✗	IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
15			std::shared_ptr<DifferentialActionModelAbstract> model,
16			std::shared_ptr<ControlParametrizationModelAbstract> control,
17			const Scalar time_step, const bool with_cost_residual)
18		✗	: Base(model, control, time_step, with_cost_residual) {}
19
20			template <typename Scalar>
21		✗	IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
22			std::shared_ptr<DifferentialActionModelAbstract> model,
23			const Scalar time_step, const bool with_cost_residual)
24		✗	: Base(model, time_step, with_cost_residual) {}
25
26			template <typename Scalar>
27		✗	void IntegratedActionModelEulerTpl<Scalar>::calc(
28			const std::shared_ptr<ActionDataAbstract>& data,
29			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
30		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
31		✗	throw_pretty(
32			"Invalid argument: " << "x has wrong dimension (it should be " +
33			std::to_string(state_->get_nx()) + ")");
34			}
35		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
36		✗	throw_pretty(
37			"Invalid argument: " << "u has wrong dimension (it should be " +
38			std::to_string(nu_) + ")");
39			}
40		✗	const std::size_t nv = differential_->get_state()->get_nv();
41		✗	Data* d = static_cast<Data*>(data.get());
42			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
43		✗	x.tail(nv);
44
45		✗	control_->calc(d->control, Scalar(0.), u);
46		✗	differential_->calc(d->differential, x, d->control->w);
47		✗	const VectorXs& a = d->differential->xout;
48		✗	d->dx.head(nv).noalias() = v * time_step_ + a * time_step2_;
49		✗	d->dx.tail(nv).noalias() = a * time_step_;
50		✗	differential_->get_state()->integrate(x, d->dx, d->xnext);
51		✗	d->cost = time_step_ * d->differential->cost;
52		✗	d->g = d->differential->g;
53		✗	d->h = d->differential->h;
54		✗	if (with_cost_residual_) {
55		✗	d->r = d->differential->r;
56			}
57		✗	}
58
59			template <typename Scalar>
60		✗	void IntegratedActionModelEulerTpl<Scalar>::calc(
61			const std::shared_ptr<ActionDataAbstract>& data,
62			const Eigen::Ref<const VectorXs>& x) {
63		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
64		✗	throw_pretty(
65			"Invalid argument: " << "x has wrong dimension (it should be " +
66			std::to_string(state_->get_nx()) + ")");
67			}
68		✗	Data* d = static_cast<Data*>(data.get());
69
70		✗	differential_->calc(d->differential, x);
71		✗	d->dx.setZero();
72		✗	d->xnext = x;
73		✗	d->cost = d->differential->cost;
74		✗	d->g = d->differential->g;
75		✗	d->h = d->differential->h;
76		✗	if (with_cost_residual_) {
77		✗	d->r = d->differential->r;
78			}
79		✗	}
80
81			template <typename Scalar>
82		✗	void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
83			const std::shared_ptr<ActionDataAbstract>& data,
84			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
85		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
86		✗	throw_pretty(
87			"Invalid argument: " << "x has wrong dimension (it should be " +
88			std::to_string(state_->get_nx()) + ")");
89			}
90		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
91		✗	throw_pretty(
92			"Invalid argument: " << "u has wrong dimension (it should be " +
93			std::to_string(nu_) + ")");
94			}
95
96		✗	const std::size_t nv = state_->get_nv();
97		✗	Data* d = static_cast<Data*>(data.get());
98
99		✗	control_->calc(d->control, Scalar(0.), u);
100		✗	differential_->calcDiff(d->differential, x, d->control->w);
101		✗	const MatrixXs& da_dx = d->differential->Fx;
102		✗	const MatrixXs& da_du = d->differential->Fu;
103		✗	control_->multiplyByJacobian(d->control, da_du, d->da_du);
104		✗	d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
105		✗	d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
106		✗	d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);
107		✗	d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
108		✗	d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;
109		✗	state_->JintegrateTransport(x, d->dx, d->Fx, second);
110		✗	state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
111		✗	state_->JintegrateTransport(x, d->dx, d->Fu, second);
112
113		✗	d->Lx.noalias() = time_step_ * d->differential->Lx;
114		✗	control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
115		✗	d->Lu *= time_step_;
116		✗	d->Lxx.noalias() = time_step_ * d->differential->Lxx;
117		✗	control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
118		✗	d->Lxu *= time_step_;
119		✗	control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
120		✗	control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
121		✗	d->Luu *= time_step_;
122		✗	d->Gx = d->differential->Gx;
123		✗	d->Hx = d->differential->Hx;
124		✗	d->Gu.conservativeResize(differential_->get_ng(), nu_);
125		✗	d->Hu.conservativeResize(differential_->get_nh(), nu_);
126		✗	control_->multiplyByJacobian(d->control, d->differential->Gu, d->Gu);
127		✗	control_->multiplyByJacobian(d->control, d->differential->Hu, d->Hu);
128		✗	}
129
130			template <typename Scalar>
131		✗	void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
132			const std::shared_ptr<ActionDataAbstract>& data,
133			const Eigen::Ref<const VectorXs>& x) {
134		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
135		✗	throw_pretty(
136			"Invalid argument: " << "x has wrong dimension (it should be " +
137			std::to_string(state_->get_nx()) + ")");
138			}
139		✗	Data* d = static_cast<Data*>(data.get());
140
141		✗	differential_->calcDiff(d->differential, x);
142		✗	state_->Jintegrate(x, d->dx, d->Fx, d->Fx);
143		✗	d->Lx = d->differential->Lx;
144		✗	d->Lxx = d->differential->Lxx;
145		✗	d->Gx = d->differential->Gx;
146		✗	d->Hx = d->differential->Hx;
147		✗	}
148
149			template <typename Scalar>
150			std::shared_ptr<ActionDataAbstractTpl<Scalar> >
151		✗	IntegratedActionModelEulerTpl<Scalar>::createData() {
152		✗	if (control_->get_nu() > differential_->get_nu())
153		✗	std::cerr << "Warning: It is useless to use an Euler integrator with a "
154			"control parametrization larger than PolyZero"
155		✗	<< std::endl;
156		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
157			}
158
159			template <typename Scalar>
160			template <typename NewScalar>
161			IntegratedActionModelEulerTpl<NewScalar>
162		✗	IntegratedActionModelEulerTpl<Scalar>::cast() const {
163			typedef IntegratedActionModelEulerTpl<NewScalar> ReturnType;
164		✗	if (control_) {
165		✗	ReturnType ret(differential_->template cast<NewScalar>(),
166		✗	control_->template cast<NewScalar>(),
167		✗	scalar_cast<NewScalar>(time_step_), with_cost_residual_);
168		✗	return ret;
169		✗	} else {
170		✗	ReturnType ret(differential_->template cast<NewScalar>(),
171		✗	scalar_cast<NewScalar>(time_step_), with_cost_residual_);
172		✗	return ret;
173		✗	}
174			}
175
176			template <typename Scalar>
177		✗	bool IntegratedActionModelEulerTpl<Scalar>::checkData(
178			const std::shared_ptr<ActionDataAbstract>& data) {
179		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
180		✗	if (data != NULL) {
181		✗	return differential_->checkData(d->differential);
182			} else {
183		✗	return false;
184			}
185		✗	}
186
187			template <typename Scalar>
188		✗	void IntegratedActionModelEulerTpl<Scalar>::quasiStatic(
189			const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
190			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
191			const Scalar tol) {
192		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
193		✗	throw_pretty(
194			"Invalid argument: " << "u has wrong dimension (it should be " +
195			std::to_string(nu_) + ")");
196			}
197		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
198		✗	throw_pretty(
199			"Invalid argument: " << "x has wrong dimension (it should be " +
200			std::to_string(state_->get_nx()) + ")");
201			}
202
203		✗	const std::shared_ptr<Data>& d = std::static_pointer_cast<Data>(data);
204
205		✗	d->control->w.setZero();
206		✗	differential_->quasiStatic(d->differential, d->control->w, x, maxiter, tol);
207		✗	control_->params(d->control, Scalar(0.), d->control->w);
208		✗	u = d->control->u;
209		✗	}
210
211			template <typename Scalar>
212		✗	void IntegratedActionModelEulerTpl<Scalar>::print(std::ostream& os) const {
213		✗	os << "IntegratedActionModelEuler {dt=" << time_step_ << ", "
214		✗	<< *differential_ << "}";
215		✗	}
216
217			} // namespace crocoddyl
218