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			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const MatrixXs& A,
14			const MatrixXs& B,
15			const MatrixXs& Q,
16			const MatrixXs& R,
17			const MatrixXs& N)
18		✗	: Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0),
19		✗	drift_free_(true),
20		✗	updated_lqr_(false) {
21		✗	const std::size_t nx = state_->get_nx();
22		✗	MatrixXs G = MatrixXs::Zero(ng_, nx + nu_);
23		✗	MatrixXs H = MatrixXs::Zero(nh_, nx + nu_);
24		✗	VectorXs f = VectorXs::Zero(nx);
25		✗	VectorXs q = VectorXs::Zero(nx);
26		✗	VectorXs r = VectorXs::Zero(nu_);
27		✗	VectorXs g = VectorXs::Zero(ng_);
28		✗	VectorXs h = VectorXs::Zero(nh_);
29		✗	set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
30		✗	}
31
32			template <typename Scalar>
33		✗	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(
34			const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q, const MatrixXs& R,
35			const MatrixXs& N, const VectorXs& f, const VectorXs& q, const VectorXs& r)
36		✗	: Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0),
37		✗	drift_free_(false),
38		✗	updated_lqr_(false) {
39		✗	const std::size_t nx = state_->get_nx();
40		✗	MatrixXs G = MatrixXs::Zero(ng_, nx + nu_);
41		✗	MatrixXs H = MatrixXs::Zero(ng_, nx + nu_);
42		✗	VectorXs g = VectorXs::Zero(ng_);
43		✗	VectorXs h = VectorXs::Zero(nh_);
44		✗	set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
45		✗	}
46
47			template <typename Scalar>
48		✗	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(
49			const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q, const MatrixXs& R,
50			const MatrixXs& N, const MatrixXs& G, const MatrixXs& H, const VectorXs& f,
51			const VectorXs& q, const VectorXs& r, const VectorXs& g, const VectorXs& h)
52		✗	: Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0, G.rows(),
53		✗	H.rows(), G.rows(), H.rows()),
54		✗	drift_free_(false),
55		✗	updated_lqr_(false) {
56		✗	set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
57		✗	}
58
59			template <typename Scalar>
60		✗	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const std::size_t nx,
61			const std::size_t nu,
62			const bool drift_free)
63			: Base(std::make_shared<StateVector>(nx), nu, 0),
64		✗	A_(MatrixXs::Identity(nx, nx)),
65		✗	B_(MatrixXs::Identity(nx, nu)),
66		✗	Q_(MatrixXs::Identity(nx, nx)),
67		✗	R_(MatrixXs::Identity(nu, nu)),
68		✗	N_(MatrixXs::Zero(nx, nu)),
69		✗	G_(MatrixXs::Zero(0, nx + nu)),
70		✗	H_(MatrixXs::Zero(0, nx + nu)),
71		✗	f_(drift_free ? VectorXs::Zero(nx) : VectorXs::Ones(nx)),
72		✗	q_(VectorXs::Ones(nx)),
73		✗	r_(VectorXs::Ones(nu)),
74		✗	g_(VectorXs::Zero(0)),
75		✗	h_(VectorXs::Zero(0)),
76		✗	drift_free_(drift_free) {}
77
78			template <typename Scalar>
79		✗	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const ActionModelLQRTpl& copy)
80		✗	: Base(std::make_shared<StateVector>(copy.get_A().cols()),
81		✗	copy.get_B().cols(), 0, copy.get_G().rows(), copy.get_H().rows(),
82		✗	copy.get_G().rows(), copy.get_H().rows()),
83		✗	drift_free_(false),
84		✗	updated_lqr_(false) {
85		✗	set_LQR(copy.get_A(), copy.get_B(), copy.get_Q(), copy.get_R(), copy.get_N(),
86		✗	copy.get_G(), copy.get_H(), copy.get_f(), copy.get_q(), copy.get_r(),
87		✗	copy.get_g(), copy.get_h());
88		✗	}
89
90			template <typename Scalar>
91		✗	void ActionModelLQRTpl<Scalar>::calc(
92			const std::shared_ptr<ActionDataAbstract>& data,
93			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
94		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
95		✗	throw_pretty(
96			"Invalid argument: " << "x has wrong dimension (it should be " +
97			std::to_string(state_->get_nx()) + ")");
98			}
99		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
100		✗	throw_pretty(
101			"Invalid argument: " << "u has wrong dimension (it should be " +
102			std::to_string(nu_) + ")");
103			}
104		✗	Data* d = static_cast<Data*>(data.get());
105
106		✗	data->xnext.noalias() = A_ * x;
107		✗	data->xnext.noalias() += B_ * u;
108		✗	data->xnext += f_;
109
110			// cost = 0.5 * x^T * Q * x + 0.5 * u^T * R * u + x^T * N * u + q^T * x + r^T
111			// * u
112		✗	d->Q_x_tmp.noalias() = Q_ * x;
113		✗	data->cost = Scalar(0.5) * x.dot(d->Q_x_tmp);
114		✗	d->R_u_tmp.noalias() = R_ * u;
115		✗	data->cost += Scalar(0.5) * u.dot(d->R_u_tmp);
116		✗	d->Q_x_tmp.noalias() = N_ * u;
117		✗	data->cost += x.dot(d->Q_x_tmp);
118		✗	data->cost += q_.dot(x);
119		✗	data->cost += r_.dot(u);
120
121			// constraints
122		✗	const std::size_t nx = state_->get_nx();
123		✗	data->g.noalias() = G_.leftCols(nx) * x;
124		✗	data->g.noalias() += G_.rightCols(nu_) * u;
125		✗	data->g += g_;
126		✗	data->h.noalias() = H_.leftCols(nx) * x;
127		✗	data->h.noalias() += H_.rightCols(nu_) * u;
128		✗	data->h += h_;
129		✗	}
130
131			template <typename Scalar>
132		✗	void ActionModelLQRTpl<Scalar>::calc(
133			const std::shared_ptr<ActionDataAbstract>& data,
134			const Eigen::Ref<const VectorXs>& x) {
135		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
136		✗	throw_pretty(
137			"Invalid argument: " << "x has wrong dimension (it should be " +
138			std::to_string(state_->get_nx()) + ")");
139			}
140		✗	Data* d = static_cast<Data*>(data.get());
141
142		✗	d->xnext = x;
143
144			// cost = 0.5 * x^T * Q * x + q^T * x
145		✗	d->Q_x_tmp.noalias() = Q_ * x;
146		✗	data->cost = Scalar(0.5) * x.dot(d->Q_x_tmp);
147		✗	data->cost += q_.dot(x);
148
149			// constraints
150		✗	const std::size_t nx = state_->get_nx();
151		✗	data->g.noalias() = G_.leftCols(nx) * x;
152		✗	data->g += g_;
153		✗	data->h.noalias() = H_.leftCols(nx) * x;
154		✗	data->h += h_;
155		✗	}
156
157			template <typename Scalar>
158		✗	void ActionModelLQRTpl<Scalar>::calcDiff(
159			const std::shared_ptr<ActionDataAbstract>& data,
160			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
161		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
162		✗	throw_pretty(
163			"Invalid argument: " << "x has wrong dimension (it should be " +
164			std::to_string(state_->get_nx()) + ")");
165			}
166		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
167		✗	throw_pretty(
168			"Invalid argument: " << "u has wrong dimension (it should be " +
169			std::to_string(nu_) + ")");
170			}
171
172		✗	const std::size_t nx = state_->get_nx();
173		✗	if (!updated_lqr_) {
174		✗	data->Fx = A_;
175		✗	data->Fu = B_;
176		✗	data->Lxx = Q_;
177		✗	data->Luu = R_;
178		✗	data->Lxu = N_;
179		✗	data->Gx = G_.leftCols(nx);
180		✗	data->Gu = G_.rightCols(nu_);
181		✗	data->Hx = H_.leftCols(nx);
182		✗	data->Hu = H_.rightCols(nu_);
183		✗	updated_lqr_ = true;
184			}
185		✗	data->Lx = q_;
186		✗	data->Lx.noalias() += Q_ * x;
187		✗	data->Lx.noalias() += N_ * u;
188		✗	data->Lu = r_;
189		✗	data->Lu.noalias() += N_.transpose() * x;
190		✗	data->Lu.noalias() += R_ * u;
191		✗	}
192
193			template <typename Scalar>
194		✗	void ActionModelLQRTpl<Scalar>::calcDiff(
195			const std::shared_ptr<ActionDataAbstract>& data,
196			const Eigen::Ref<const VectorXs>& x) {
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::size_t nx = state_->get_nx();
204		✗	if (!updated_lqr_) {
205		✗	data->Lxx = Q_;
206		✗	data->Gx = G_.leftCols(nx);
207		✗	data->Hx = H_.leftCols(nx);
208		✗	updated_lqr_ = true;
209			}
210		✗	data->Lx = q_;
211		✗	data->Lx.noalias() += Q_ * x;
212		✗	}
213
214			template <typename Scalar>
215			std::shared_ptr<ActionDataAbstractTpl<Scalar>>
216		✗	ActionModelLQRTpl<Scalar>::createData() {
217		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
218			}
219
220			template <typename Scalar>
221			template <typename NewScalar>
222		✗	ActionModelLQRTpl<NewScalar> ActionModelLQRTpl<Scalar>::cast() const {
223			typedef ActionModelLQRTpl<NewScalar> ReturnType;
224		✗	ReturnType ret(A_.template cast<NewScalar>(), B_.template cast<NewScalar>(),
225		✗	Q_.template cast<NewScalar>(), R_.template cast<NewScalar>(),
226		✗	N_.template cast<NewScalar>(), G_.template cast<NewScalar>(),
227		✗	H_.template cast<NewScalar>(), f_.template cast<NewScalar>(),
228		✗	q_.template cast<NewScalar>(), r_.template cast<NewScalar>(),
229		✗	g_.template cast<NewScalar>(), h_.template cast<NewScalar>());
230		✗	return ret;
231			}
232
233			template <typename Scalar>
234		✗	bool ActionModelLQRTpl<Scalar>::checkData(
235			const std::shared_ptr<ActionDataAbstract>& data) {
236		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
237		✗	if (d != NULL) {
238		✗	return true;
239			} else {
240		✗	return false;
241			}
242		✗	}
243
244			template <typename Scalar>
245		✗	ActionModelLQRTpl<Scalar> ActionModelLQRTpl<Scalar>::Random(
246			const std::size_t nx, const std::size_t nu, const std::size_t ng,
247			const std::size_t nh) {
248		✗	MatrixXs A = MatrixXs::Random(nx, nx);
249		✗	MatrixXs B = MatrixXs::Random(nx, nu);
250		✗	MatrixXs L_tmp = MatrixXs::Random(nx + nu, nx + nu);
251		✗	MatrixXs L = L_tmp.transpose() * L_tmp;
252		✗	const Eigen::Block<MatrixXs> Q = L.topLeftCorner(nx, nx);
253		✗	const Eigen::Block<MatrixXs> R = L.bottomRightCorner(nu, nu);
254		✗	const Eigen::Block<MatrixXs> N = L.topRightCorner(nx, nu);
255		✗	MatrixXs G = MatrixXs::Random(ng, nx + nu);
256		✗	MatrixXs H = MatrixXs::Random(nh, nx + nu);
257		✗	VectorXs f = VectorXs::Random(nx);
258		✗	VectorXs q = VectorXs::Random(nx);
259		✗	VectorXs r = VectorXs::Random(nu);
260		✗	VectorXs g = VectorXs::Random(ng);
261		✗	VectorXs h = VectorXs::Random(nh);
262		✗	return ActionModelLQRTpl<Scalar>(A, B, Q, R, N, G, H, f, q, r, g, h);
263		✗	}
264
265			template <typename Scalar>
266		✗	void ActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
267		✗	os << "ActionModelLQR {nx=" << state_->get_nx() << ", nu=" << nu_
268		✗	<< ", ng=" << ng_ << ", nh=" << nh_ << ", drift_free=" << drift_free_
269		✗	<< "}";
270		✗	}
271
272			template <typename Scalar>
273		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_A()
274			const {
275		✗	return A_;
276			}
277
278			template <typename Scalar>
279		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_B()
280			const {
281		✗	return B_;
282			}
283
284			template <typename Scalar>
285		✗	const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_f()
286			const {
287		✗	return f_;
288			}
289
290			template <typename Scalar>
291		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Q()
292			const {
293		✗	return Q_;
294			}
295
296			template <typename Scalar>
297		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_R()
298			const {
299		✗	return R_;
300			}
301
302			template <typename Scalar>
303		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_N()
304			const {
305		✗	return N_;
306			}
307
308			template <typename Scalar>
309		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_G()
310			const {
311		✗	return G_;
312			}
313
314			template <typename Scalar>
315		✗	const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_H()
316			const {
317		✗	return H_;
318			}
319
320			template <typename Scalar>
321		✗	const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_q()
322			const {
323		✗	return q_;
324			}
325
326			template <typename Scalar>
327		✗	const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_r()
328			const {
329		✗	return r_;
330			}
331
332			template <typename Scalar>
333		✗	const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_g()
334			const {
335		✗	return g_;
336			}
337
338			template <typename Scalar>
339		✗	const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_h()
340			const {
341		✗	return h_;
342			}
343
344			template <typename Scalar>
345		✗	void ActionModelLQRTpl<Scalar>::set_LQR(const MatrixXs& A, const MatrixXs& B,
346			const MatrixXs& Q, const MatrixXs& R,
347			const MatrixXs& N, const MatrixXs& G,
348			const MatrixXs& H, const VectorXs& f,
349			const VectorXs& q, const VectorXs& r,
350			const VectorXs& g, const VectorXs& h) {
351		✗	const std::size_t nx = state_->get_nx();
352		✗	if (static_cast<std::size_t>(A.rows()) != nx) {
353		✗	throw_pretty(
354			"Invalid argument: " << "A should be a squared matrix with size " +
355			std::to_string(nx));
356			}
357		✗	if (static_cast<std::size_t>(B.rows()) != nx) {
358		✗	throw_pretty(
359			"Invalid argument: " << "B has wrong dimension (it should have " +
360			std::to_string(nx) + " rows)");
361			}
362		✗	if (static_cast<std::size_t>(Q.rows()) != nx ||
363		✗	static_cast<std::size_t>(Q.cols()) != nx) {
364		✗	throw_pretty("Invalid argument: "
365			<< "Q has wrong dimension (it should be " +
366			std::to_string(nx) + " x " + std::to_string(nx) + ")");
367			}
368		✗	if (static_cast<std::size_t>(R.rows()) != nu_ ||
369		✗	static_cast<std::size_t>(R.cols()) != nu_) {
370		✗	throw_pretty(
371			"Invalid argument: " << "R has wrong dimension (it should be " +
372			std::to_string(nu_) + " x " +
373			std::to_string(nu_) + ")");
374			}
375		✗	if (static_cast<std::size_t>(N.rows()) != nx ||
376		✗	static_cast<std::size_t>(N.cols()) != nu_) {
377		✗	throw_pretty("Invalid argument: "
378			<< "N has wrong dimension (it should be " +
379			std::to_string(nx) + " x " + std::to_string(nu_) + ")");
380			}
381		✗	if (static_cast<std::size_t>(G.rows()) != ng_ ||
382		✗	static_cast<std::size_t>(G.cols()) != nx + nu_) {
383		✗	throw_pretty(
384			"Invalid argument: " << "G has wrong dimension (it should be " +
385			std::to_string(ng_) + " x " +
386			std::to_string(nx + nu_) + ")");
387			}
388		✗	if (static_cast<std::size_t>(H.rows()) != nh_ ||
389		✗	static_cast<std::size_t>(H.cols()) != nx + nu_) {
390		✗	throw_pretty(
391			"Invalid argument: " << "H has wrong dimension (it should be " +
392			std::to_string(nh_) + " x " +
393			std::to_string(nx + nu_) + ")");
394			}
395		✗	if (static_cast<std::size_t>(f.size()) != nx) {
396		✗	throw_pretty(
397			"Invalid argument: " << "f has wrong dimension (it should be " +
398			std::to_string(nx) + ")");
399			}
400		✗	if (static_cast<std::size_t>(q.size()) != nx) {
401		✗	throw_pretty(
402			"Invalid argument: " << "q has wrong dimension (it should be " +
403			std::to_string(nx) + ")");
404			}
405		✗	if (static_cast<std::size_t>(r.size()) != nu_) {
406		✗	throw_pretty(
407			"Invalid argument: " << "r has wrong dimension (it should be " +
408			std::to_string(nu_) + ")");
409			}
410		✗	if (static_cast<std::size_t>(g.size()) != ng_) {
411		✗	throw_pretty(
412			"Invalid argument: " << "g has wrong dimension (it should be " +
413			std::to_string(ng_) + ")");
414			}
415		✗	if (static_cast<std::size_t>(h.size()) != nh_) {
416		✗	throw_pretty(
417			"Invalid argument: " << "h has wrong dimension (it should be " +
418			std::to_string(nh_) + ")");
419			}
420		✗	L_ = MatrixXs::Zero(nx + nu_, nx + nu_);
421		✗	L_ << Q, N, N.transpose(), R;
422		✗	if (!checkPSD(L_)) {
423		✗	throw_pretty("Invalid argument "
424			<< "[Q, N; N.T, R] is not positive semi-definite");
425			}
426		✗	A_ = A;
427		✗	B_ = B;
428		✗	f_ = f;
429		✗	Q_ = Q;
430		✗	R_ = R;
431		✗	N_ = N;
432		✗	G_ = G;
433		✗	H_ = H;
434		✗	q_ = q;
435		✗	r_ = r;
436		✗	g_ = g;
437		✗	h_ = h;
438		✗	updated_lqr_ = false;
439		✗	}
440
441			} // namespace crocoddyl
442