Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/actions/lqr.hxx	Lines:	80	130	61.5 %
Date:	2024-02-13 11:12:33	Branches:	67	422	15.9 %


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

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

namespace crocoddyl {

template <typename Scalar>
ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const std::size_t nx,
                                             const std::size_t nu,
                                             const bool drift_free)
    : Base(boost::make_shared<StateVector>(nx), nu, 0),
      drift_free_(drift_free) {
  // TODO(cmastalli): substitute by random (vectors) and random-orthogonal
  // (matrices)
  Fx_ = MatrixXs::Identity(nx, nx);
  Fu_ = MatrixXs::Identity(nx, nu);
  f0_ = VectorXs::Ones(nx);
  Lxx_ = MatrixXs::Identity(nx, nx);
  Lxu_ = MatrixXs::Identity(nx, nu);
  Luu_ = MatrixXs::Identity(nu, nu);
  lx_ = VectorXs::Ones(nx);
  lu_ = VectorXs::Ones(nu);
}

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

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

  if (drift_free_) {
    data->xnext.noalias() = Fx_ * x;
    data->xnext.noalias() += Fu_ * u;
  } else {
    data->xnext.noalias() = Fx_ * x;
    data->xnext.noalias() += Fu_ * u;
    data->xnext += f0_;
  }

  // cost = 0.5 * x^T*Lxx*x + 0.5 * u^T*Luu*u + x^T*Lxu*u + lx^T*x + lu^T*u
  d->Lxx_x_tmp.noalias() = Lxx_ * x;
  data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
  d->Luu_u_tmp.noalias() = Luu_ * u;
  data->cost += Scalar(0.5) * u.dot(d->Luu_u_tmp);
  d->Lxx_x_tmp.noalias() = Lxu_ * u;
  data->cost += x.dot(d->Lxx_x_tmp);
  data->cost += lx_.transpose() * x;
  data->cost += lu_.transpose() * u;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& 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->xnext = x;
  // cost = 0.5 * x^T*Lxx*x + lx^T*x
  d->Lxx_x_tmp.noalias() = Lxx_ * x;
  data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
  data->cost += lx_.dot(x);
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& 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->Lx = lx_;
  data->Lx.noalias() += Lxx_ * x;
  data->Lx.noalias() += Lxu_ * u;
  data->Lu = lu_;
  data->Lu.noalias() += Lxu_.transpose() * x;
  data->Lu.noalias() += Luu_ * u;
  data->Fx = Fx_;
  data->Fu = Fu_;
  data->Lxx = Lxx_;
  data->Lxu = Lxu_;
  data->Luu = Luu_;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& 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->Lx = lx_;
  data->Lx.noalias() += Lxx_ * x;
  data->Lxx = Lxx_;
}

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar>>
ActionModelLQRTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
bool ActionModelLQRTpl<Scalar>::checkData(
    const boost::shared_ptr<ActionDataAbstract>& data) {
  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
  if (d != NULL) {
    return true;
  } else {
    return false;
  }
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
  os << "ActionModelLQR {nx=" << state_->get_nx() << ", nu=" << nu_
     << ", drift_free=" << drift_free_ << "}";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
ActionModelLQRTpl<Scalar>::get_Fx() const {
  return Fx_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
ActionModelLQRTpl<Scalar>::get_Fu() const {
  return Fu_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ActionModelLQRTpl<Scalar>::get_f0() const {
  return f0_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ActionModelLQRTpl<Scalar>::get_lx() const {
  return lx_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ActionModelLQRTpl<Scalar>::get_lu() const {
  return lu_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
ActionModelLQRTpl<Scalar>::get_Lxx() const {
  return Lxx_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
ActionModelLQRTpl<Scalar>::get_Lxu() const {
  return Lxu_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
ActionModelLQRTpl<Scalar>::get_Luu() const {
  return Luu_;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_Fx(const MatrixXs& Fx) {
  if (static_cast<std::size_t>(Fx.rows()) != state_->get_nx() ||
      static_cast<std::size_t>(Fx.cols()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "Fx has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + "," +
                        std::to_string(state_->get_nx()) + ")");
  }
  Fx_ = Fx;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_Fu(const MatrixXs& Fu) {
  if (static_cast<std::size_t>(Fu.rows()) != state_->get_nx() ||
      static_cast<std::size_t>(Fu.cols()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "Fu has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + "," +
                        std::to_string(nu_) + ")");
  }
  Fu_ = Fu;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_f0(const VectorXs& f0) {
  if (static_cast<std::size_t>(f0.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "f0 has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  f0_ = f0;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_lx(const VectorXs& lx) {
  if (static_cast<std::size_t>(lx.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "lx has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  lx_ = lx;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_lu(const VectorXs& lu) {
  if (static_cast<std::size_t>(lu.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "lu has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  lu_ = lu;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_Lxx(const MatrixXs& Lxx) {
  if (static_cast<std::size_t>(Lxx.rows()) != state_->get_nx() ||
      static_cast<std::size_t>(Lxx.cols()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "Lxx has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + "," +
                        std::to_string(state_->get_nx()) + ")");
  }
  Lxx_ = Lxx;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_Lxu(const MatrixXs& Lxu) {
  if (static_cast<std::size_t>(Lxu.rows()) != state_->get_nx() ||
      static_cast<std::size_t>(Lxu.cols()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "Lxu has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + "," +
                        std::to_string(nu_) + ")");
  }
  Lxu_ = Lxu;
}

template <typename Scalar>
void ActionModelLQRTpl<Scalar>::set_Luu(const MatrixXs& Luu) {
  if (static_cast<std::size_t>(Luu.rows()) != nu_ ||
      static_cast<std::size_t>(Luu.cols()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "Fq has wrong dimension (it should be " +
                        std::to_string(nu_) + "," + std::to_string(nu_) + ")");
  }
  Luu_ = Luu;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, 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			#include "crocoddyl/core/utils/exception.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		72	ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const std::size_t nx,
16			const std::size_t nu,
17			const bool drift_free)
18			: Base(boost::make_shared<StateVector>(nx), nu, 0),
19	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	72	drift_free_(drift_free) {
20			// TODO(cmastalli): substitute by random (vectors) and random-orthogonal
21			// (matrices)
22	✓✗✓✗	72	Fx_ = MatrixXs::Identity(nx, nx);
23	✓✗✓✗	72	Fu_ = MatrixXs::Identity(nx, nu);
24	✓✗✓✗	72	f0_ = VectorXs::Ones(nx);
25	✓✗✓✗	72	Lxx_ = MatrixXs::Identity(nx, nx);
26	✓✗✓✗	72	Lxu_ = MatrixXs::Identity(nx, nu);
27	✓✗✓✗	72	Luu_ = MatrixXs::Identity(nu, nu);
28	✓✗✓✗	72	lx_ = VectorXs::Ones(nx);
29	✓✗✓✗	72	lu_ = VectorXs::Ones(nu);
30		72	}
31
32			template <typename Scalar>
33		148	ActionModelLQRTpl<Scalar>::~ActionModelLQRTpl() {}
34
35			template <typename Scalar>
36		1368	void ActionModelLQRTpl<Scalar>::calc(
37			const boost::shared_ptr<ActionDataAbstract>& data,
38			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
39	✗✓	1368	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
40			throw_pretty("Invalid argument: "
41			<< "x has wrong dimension (it should be " +
42			std::to_string(state_->get_nx()) + ")");
43			}
44	✗✓	1368	if (static_cast<std::size_t>(u.size()) != nu_) {
45			throw_pretty("Invalid argument: "
46			<< "u has wrong dimension (it should be " +
47			std::to_string(nu_) + ")");
48			}
49		1368	Data* d = static_cast<Data*>(data.get());
50
51	✓✓	1368	if (drift_free_) {
52	✓✗✓✗	655	data->xnext.noalias() = Fx_ * x;
53	✓✗✓✗	655	data->xnext.noalias() += Fu_ * u;
54			} else {
55	✓✗✓✗	713	data->xnext.noalias() = Fx_ * x;
56	✓✗✓✗	713	data->xnext.noalias() += Fu_ * u;
57		713	data->xnext += f0_;
58			}
59
60			// cost = 0.5 * x^TLxxx + 0.5 * u^TLuuu + x^TLxuu + lx^Tx + lu^Tu
61	✓✗✓✗	1368	d->Lxx_x_tmp.noalias() = Lxx_ * x;
62		1368	data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
63	✓✗✓✗	1368	d->Luu_u_tmp.noalias() = Luu_ * u;
64		1368	data->cost += Scalar(0.5) * u.dot(d->Luu_u_tmp);
65	✓✗✓✗	1368	d->Lxx_x_tmp.noalias() = Lxu_ * u;
66		1368	data->cost += x.dot(d->Lxx_x_tmp);
67	✓✗✓✗	1368	data->cost += lx_.transpose() * x;
68	✓✗✓✗	1368	data->cost += lu_.transpose() * u;
69		1368	}
70
71			template <typename Scalar>
72		116	void ActionModelLQRTpl<Scalar>::calc(
73			const boost::shared_ptr<ActionDataAbstract>& data,
74			const Eigen::Ref<const VectorXs>& x) {
75	✗✓	116	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
76			throw_pretty("Invalid argument: "
77			<< "x has wrong dimension (it should be " +
78			std::to_string(state_->get_nx()) + ")");
79			}
80		116	Data* d = static_cast<Data*>(data.get());
81
82		116	d->xnext = x;
83			// cost = 0.5 * x^TLxxx + lx^T*x
84	✓✗✓✗	116	d->Lxx_x_tmp.noalias() = Lxx_ * x;
85		116	data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
86		116	data->cost += lx_.dot(x);
87		116	}
88
89			template <typename Scalar>
90		863	void ActionModelLQRTpl<Scalar>::calcDiff(
91			const boost::shared_ptr<ActionDataAbstract>& data,
92			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
93	✗✓	863	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
94			throw_pretty("Invalid argument: "
95			<< "x has wrong dimension (it should be " +
96			std::to_string(state_->get_nx()) + ")");
97			}
98	✗✓	863	if (static_cast<std::size_t>(u.size()) != nu_) {
99			throw_pretty("Invalid argument: "
100			<< "u has wrong dimension (it should be " +
101			std::to_string(nu_) + ")");
102			}
103
104		863	data->Lx = lx_;
105	✓✗✓✗	863	data->Lx.noalias() += Lxx_ * x;
106	✓✗✓✗	863	data->Lx.noalias() += Lxu_ * u;
107		863	data->Lu = lu_;
108	✓✗✓✗ ✓✗	863	data->Lu.noalias() += Lxu_.transpose() * x;
109	✓✗✓✗	863	data->Lu.noalias() += Luu_ * u;
110		863	data->Fx = Fx_;
111		863	data->Fu = Fu_;
112		863	data->Lxx = Lxx_;
113		863	data->Lxu = Lxu_;
114		863	data->Luu = Luu_;
115		863	}
116
117			template <typename Scalar>
118		55	void ActionModelLQRTpl<Scalar>::calcDiff(
119			const boost::shared_ptr<ActionDataAbstract>& data,
120			const Eigen::Ref<const VectorXs>& x) {
121	✗✓	55	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
122			throw_pretty("Invalid argument: "
123			<< "x has wrong dimension (it should be " +
124			std::to_string(state_->get_nx()) + ")");
125			}
126
127		55	data->Lx = lx_;
128	✓✗✓✗	55	data->Lx.noalias() += Lxx_ * x;
129		55	data->Lxx = Lxx_;
130		55	}
131
132			template <typename Scalar>
133			boost::shared_ptr<ActionDataAbstractTpl<Scalar>>
134		823	ActionModelLQRTpl<Scalar>::createData() {
135	✓✗	823	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
136			}
137
138			template <typename Scalar>
139		128	bool ActionModelLQRTpl<Scalar>::checkData(
140			const boost::shared_ptr<ActionDataAbstract>& data) {
141		256	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
142	✓✗	128	if (d != NULL) {
143		128	return true;
144			} else {
145			return false;
146			}
147			}
148
149			template <typename Scalar>
150		44	void ActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
151		44	os << "ActionModelLQR {nx=" << state_->get_nx() << ", nu=" << nu_
152		44	<< ", drift_free=" << drift_free_ << "}";
153		44	}
154
155			template <typename Scalar>
156			const typename MathBaseTpl<Scalar>::MatrixXs&
157		823	ActionModelLQRTpl<Scalar>::get_Fx() const {
158		823	return Fx_;
159			}
160
161			template <typename Scalar>
162			const typename MathBaseTpl<Scalar>::MatrixXs&
163		823	ActionModelLQRTpl<Scalar>::get_Fu() const {
164		823	return Fu_;
165			}
166
167			template <typename Scalar>
168			const typename MathBaseTpl<Scalar>::VectorXs&
169			ActionModelLQRTpl<Scalar>::get_f0() const {
170			return f0_;
171			}
172
173			template <typename Scalar>
174			const typename MathBaseTpl<Scalar>::VectorXs&
175			ActionModelLQRTpl<Scalar>::get_lx() const {
176			return lx_;
177			}
178
179			template <typename Scalar>
180			const typename MathBaseTpl<Scalar>::VectorXs&
181			ActionModelLQRTpl<Scalar>::get_lu() const {
182			return lu_;
183			}
184
185			template <typename Scalar>
186			const typename MathBaseTpl<Scalar>::MatrixXs&
187		823	ActionModelLQRTpl<Scalar>::get_Lxx() const {
188		823	return Lxx_;
189			}
190
191			template <typename Scalar>
192			const typename MathBaseTpl<Scalar>::MatrixXs&
193		823	ActionModelLQRTpl<Scalar>::get_Lxu() const {
194		823	return Lxu_;
195			}
196
197			template <typename Scalar>
198			const typename MathBaseTpl<Scalar>::MatrixXs&
199		823	ActionModelLQRTpl<Scalar>::get_Luu() const {
200		823	return Luu_;
201			}
202
203			template <typename Scalar>
204			void ActionModelLQRTpl<Scalar>::set_Fx(const MatrixXs& Fx) {
205			if (static_cast<std::size_t>(Fx.rows()) != state_->get_nx() \|\|
206			static_cast<std::size_t>(Fx.cols()) != state_->get_nx()) {
207			throw_pretty("Invalid argument: "
208			<< "Fx has wrong dimension (it should be " +
209			std::to_string(state_->get_nx()) + "," +
210			std::to_string(state_->get_nx()) + ")");
211			}
212			Fx_ = Fx;
213			}
214
215			template <typename Scalar>
216			void ActionModelLQRTpl<Scalar>::set_Fu(const MatrixXs& Fu) {
217			if (static_cast<std::size_t>(Fu.rows()) != state_->get_nx() \|\|
218			static_cast<std::size_t>(Fu.cols()) != nu_) {
219			throw_pretty("Invalid argument: "
220			<< "Fu has wrong dimension (it should be " +
221			std::to_string(state_->get_nx()) + "," +
222			std::to_string(nu_) + ")");
223			}
224			Fu_ = Fu;
225			}
226
227			template <typename Scalar>
228			void ActionModelLQRTpl<Scalar>::set_f0(const VectorXs& f0) {
229			if (static_cast<std::size_t>(f0.size()) != state_->get_nx()) {
230			throw_pretty("Invalid argument: "
231			<< "f0 has wrong dimension (it should be " +
232			std::to_string(state_->get_nx()) + ")");
233			}
234			f0_ = f0;
235			}
236
237			template <typename Scalar>
238			void ActionModelLQRTpl<Scalar>::set_lx(const VectorXs& lx) {
239			if (static_cast<std::size_t>(lx.size()) != state_->get_nx()) {
240			throw_pretty("Invalid argument: "
241			<< "lx has wrong dimension (it should be " +
242			std::to_string(state_->get_nx()) + ")");
243			}
244			lx_ = lx;
245			}
246
247			template <typename Scalar>
248			void ActionModelLQRTpl<Scalar>::set_lu(const VectorXs& lu) {
249			if (static_cast<std::size_t>(lu.size()) != nu_) {
250			throw_pretty("Invalid argument: "
251			<< "lu has wrong dimension (it should be " +
252			std::to_string(nu_) + ")");
253			}
254			lu_ = lu;
255			}
256
257			template <typename Scalar>
258			void ActionModelLQRTpl<Scalar>::set_Lxx(const MatrixXs& Lxx) {
259			if (static_cast<std::size_t>(Lxx.rows()) != state_->get_nx() \|\|
260			static_cast<std::size_t>(Lxx.cols()) != state_->get_nx()) {
261			throw_pretty("Invalid argument: "
262			<< "Lxx has wrong dimension (it should be " +
263			std::to_string(state_->get_nx()) + "," +
264			std::to_string(state_->get_nx()) + ")");
265			}
266			Lxx_ = Lxx;
267			}
268
269			template <typename Scalar>
270			void ActionModelLQRTpl<Scalar>::set_Lxu(const MatrixXs& Lxu) {
271			if (static_cast<std::size_t>(Lxu.rows()) != state_->get_nx() \|\|
272			static_cast<std::size_t>(Lxu.cols()) != nu_) {
273			throw_pretty("Invalid argument: "
274			<< "Lxu has wrong dimension (it should be " +
275			std::to_string(state_->get_nx()) + "," +
276			std::to_string(nu_) + ")");
277			}
278			Lxu_ = Lxu;
279			}
280
281			template <typename Scalar>
282			void ActionModelLQRTpl<Scalar>::set_Luu(const MatrixXs& Luu) {
283			if (static_cast<std::size_t>(Luu.rows()) != nu_ \|\|
284			static_cast<std::size_t>(Luu.cols()) != nu_) {
285			throw_pretty("Invalid argument: "
286			<< "Fq has wrong dimension (it should be " +
287			std::to_string(nu_) + "," + std::to_string(nu_) + ")");
288			}
289			Luu_ = Luu;
290			}
291
292			} // namespace crocoddyl