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