Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/actions/diff-lqr.hxx	Lines:	77	133	57.9 %
Date:	2024-02-13 11:12:33	Branches:	85	486	17.5 %


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

#include "crocoddyl/core/actions/diff-lqr.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl(
    const std::size_t nq, const std::size_t nu, const bool drift_free)
    : Base(boost::make_shared<StateVector>(2 * nq), nu),
      drift_free_(drift_free) {
  // TODO(cmastalli): substitute by random (vectors) and random-orthogonal
  // (matrices)
  Fq_ = MatrixXs::Identity(state_->get_nq(), state_->get_nq());
  Fv_ = MatrixXs::Identity(state_->get_nv(), state_->get_nv());
  Fu_ = MatrixXs::Identity(state_->get_nq(), nu_);
  f0_ = VectorXs::Ones(state_->get_nv());
  Lxx_ = MatrixXs::Identity(state_->get_nx(), state_->get_nx());
  Lxu_ = MatrixXs::Identity(state_->get_nx(), nu_);
  Luu_ = MatrixXs::Identity(nu_, nu_);
  lx_ = VectorXs::Ones(state_->get_nx());
  lu_ = VectorXs::Ones(nu_);
}

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

template <typename Scalar>
void DifferentialActionModelLQRTpl<Scalar>::calc(
    const boost::shared_ptr<DifferentialActionDataAbstract>& 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_) + ")");
  }
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(state_->get_nq());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(state_->get_nv());

  if (drift_free_) {
    data->xout.noalias() = Fq_ * q;
    data->xout.noalias() += Fv_ * v;
    data->xout.noalias() += Fu_ * u;
  } else {
    data->xout.noalias() = Fq_ * q;
    data->xout.noalias() += Fv_ * v;
    data->xout.noalias() += Fu_ * u;
    data->xout += f0_;
  }
  data->cost = Scalar(0.5) * x.dot(Lxx_ * x) + Scalar(0.5) * u.dot(Luu_ * u) +
               x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
}

template <typename Scalar>
void DifferentialActionModelLQRTpl<Scalar>::calc(
    const boost::shared_ptr<DifferentialActionDataAbstract>& 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->cost = Scalar(0.5) * x.dot(Lxx_ * x) + lx_.dot(x);
}

template <typename Scalar>
void DifferentialActionModelLQRTpl<Scalar>::calcDiff(
    const boost::shared_ptr<DifferentialActionDataAbstract>& 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.leftCols(state_->get_nq()) = Fq_;
  data->Fx.rightCols(state_->get_nv()) = Fv_;
  data->Fu = Fu_;
  data->Lxx = Lxx_;
  data->Lxu = Lxu_;
  data->Luu = Luu_;
}

template <typename Scalar>
void DifferentialActionModelLQRTpl<Scalar>::calcDiff(
    const boost::shared_ptr<DifferentialActionDataAbstract>& 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<DifferentialActionDataAbstractTpl<Scalar> >
DifferentialActionModelLQRTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

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

template <typename Scalar>
void DifferentialActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
  os << "DifferentialActionModelLQR {nq=" << state_->get_nq() << ", nu=" << nu_
     << ", drift_free=" << drift_free_ << "}";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
DifferentialActionModelLQRTpl<Scalar>::get_Fq() const {
  return Fq_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixXs&
DifferentialActionModelLQRTpl<Scalar>::get_Fv() const {
  return Fv_;
}

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

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

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

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

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

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

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

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

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

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

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

template <typename Scalar>
void DifferentialActionModelLQRTpl<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 DifferentialActionModelLQRTpl<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 DifferentialActionModelLQRTpl<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 DifferentialActionModelLQRTpl<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 DifferentialActionModelLQRTpl<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-2021, LAAS-CNRS, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/actions/diff-lqr.hpp"
10			#include "crocoddyl/core/utils/exception.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		105	DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl(
16			const std::size_t nq, const std::size_t nu, const bool drift_free)
17			: Base(boost::make_shared<StateVector>(2 * nq), nu),
18	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	105	drift_free_(drift_free) {
19			// TODO(cmastalli): substitute by random (vectors) and random-orthogonal
20			// (matrices)
21	✓✗✓✗	105	Fq_ = MatrixXs::Identity(state_->get_nq(), state_->get_nq());
22	✓✗✓✗	105	Fv_ = MatrixXs::Identity(state_->get_nv(), state_->get_nv());
23	✓✗✓✗	105	Fu_ = MatrixXs::Identity(state_->get_nq(), nu_);
24	✓✗✓✗	105	f0_ = VectorXs::Ones(state_->get_nv());
25	✓✗✓✗	105	Lxx_ = MatrixXs::Identity(state_->get_nx(), state_->get_nx());
26	✓✗✓✗	105	Lxu_ = MatrixXs::Identity(state_->get_nx(), nu_);
27	✓✗✓✗	105	Luu_ = MatrixXs::Identity(nu_, nu_);
28	✓✗✓✗	105	lx_ = VectorXs::Ones(state_->get_nx());
29	✓✗✓✗	105	lu_ = VectorXs::Ones(nu_);
30		105	}
31
32			template <typename Scalar>
33		214	DifferentialActionModelLQRTpl<Scalar>::~DifferentialActionModelLQRTpl() {}
34
35			template <typename Scalar>
36		12980	void DifferentialActionModelLQRTpl<Scalar>::calc(
37			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
38			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
39	✓✗✗✓	12980	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	✓✗✗✓	12980	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	✓✗	12980	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
50			x.head(state_->get_nq());
51	✓✗	12980	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
52			x.tail(state_->get_nv());
53
54	✓✓	12980	if (drift_free_) {
55	✓✗✓✗ ✓✗	6491	data->xout.noalias() = Fq_ * q;
56	✓✗✓✗ ✓✗	6491	data->xout.noalias() += Fv_ * v;
57	✓✗✓✗ ✓✗	6491	data->xout.noalias() += Fu_ * u;
58			} else {
59	✓✗✓✗ ✓✗	6489	data->xout.noalias() = Fq_ * q;
60	✓✗✓✗ ✓✗	6489	data->xout.noalias() += Fv_ * v;
61	✓✗✓✗ ✓✗	6489	data->xout.noalias() += Fu_ * u;
62	✓✗	6489	data->xout += f0_;
63			}
64	✓✗✓✗ ✓✗✓✗ ✓✗	25960	data->cost = Scalar(0.5) * x.dot(Lxx_ * x) + Scalar(0.5) * u.dot(Luu_ * u) +
65	✓✗✓✗ ✓✗	25960	x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
66		12980	}
67
68			template <typename Scalar>
69		1716	void DifferentialActionModelLQRTpl<Scalar>::calc(
70			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
71			const Eigen::Ref<const VectorXs>& x) {
72	✗✓	1716	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
73			throw_pretty("Invalid argument: "
74			<< "x has wrong dimension (it should be " +
75			std::to_string(state_->get_nx()) + ")");
76			}
77
78	✓✗✓✗	1716	data->cost = Scalar(0.5) * x.dot(Lxx_ * x) + lx_.dot(x);
79		1716	}
80
81			template <typename Scalar>
82		2539	void DifferentialActionModelLQRTpl<Scalar>::calcDiff(
83			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
84			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
85	✗✓	2539	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
86			throw_pretty("Invalid argument: "
87			<< "x has wrong dimension (it should be " +
88			std::to_string(state_->get_nx()) + ")");
89			}
90	✗✓	2539	if (static_cast<std::size_t>(u.size()) != nu_) {
91			throw_pretty("Invalid argument: "
92			<< "u has wrong dimension (it should be " +
93			std::to_string(nu_) + ")");
94			}
95
96		2539	data->Lx = lx_;
97	✓✗✓✗	2539	data->Lx.noalias() += Lxx_ * x;
98	✓✗✓✗	2539	data->Lx.noalias() += Lxu_ * u;
99		2539	data->Lu = lu_;
100	✓✗✓✗ ✓✗	2539	data->Lu.noalias() += Lxu_.transpose() * x;
101	✓✗✓✗	2539	data->Lu.noalias() += Luu_ * u;
102	✓✗	2539	data->Fx.leftCols(state_->get_nq()) = Fq_;
103	✓✗	2539	data->Fx.rightCols(state_->get_nv()) = Fv_;
104		2539	data->Fu = Fu_;
105		2539	data->Lxx = Lxx_;
106		2539	data->Lxu = Lxu_;
107		2539	data->Luu = Luu_;
108		2539	}
109
110			template <typename Scalar>
111		45	void DifferentialActionModelLQRTpl<Scalar>::calcDiff(
112			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
113			const Eigen::Ref<const VectorXs>& x) {
114	✗✓	45	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
115			throw_pretty("Invalid argument: "
116			<< "x has wrong dimension (it should be " +
117			std::to_string(state_->get_nx()) + ")");
118			}
119
120		45	data->Lx = lx_;
121	✓✗✓✗	45	data->Lx.noalias() += Lxx_ * x;
122		45	data->Lxx = Lxx_;
123		45	}
124
125			template <typename Scalar>
126			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
127		13554	DifferentialActionModelLQRTpl<Scalar>::createData() {
128	✓✗	13554	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
129			}
130
131			template <typename Scalar>
132		1314	bool DifferentialActionModelLQRTpl<Scalar>::checkData(
133			const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
134		2628	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
135	✓✗	1314	if (d != NULL) {
136		1314	return true;
137			} else {
138			return false;
139			}
140			}
141
142			template <typename Scalar>
143		20	void DifferentialActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
144		20	os << "DifferentialActionModelLQR {nq=" << state_->get_nq() << ", nu=" << nu_
145		20	<< ", drift_free=" << drift_free_ << "}";
146		20	}
147
148			template <typename Scalar>
149			const typename MathBaseTpl<Scalar>::MatrixXs&
150		13554	DifferentialActionModelLQRTpl<Scalar>::get_Fq() const {
151		13554	return Fq_;
152			}
153
154			template <typename Scalar>
155			const typename MathBaseTpl<Scalar>::MatrixXs&
156		13554	DifferentialActionModelLQRTpl<Scalar>::get_Fv() const {
157		13554	return Fv_;
158			}
159
160			template <typename Scalar>
161			const typename MathBaseTpl<Scalar>::MatrixXs&
162		13554	DifferentialActionModelLQRTpl<Scalar>::get_Fu() const {
163		13554	return Fu_;
164			}
165
166			template <typename Scalar>
167			const typename MathBaseTpl<Scalar>::VectorXs&
168			DifferentialActionModelLQRTpl<Scalar>::get_f0() const {
169			return f0_;
170			}
171
172			template <typename Scalar>
173			const typename MathBaseTpl<Scalar>::VectorXs&
174			DifferentialActionModelLQRTpl<Scalar>::get_lx() const {
175			return lx_;
176			}
177
178			template <typename Scalar>
179			const typename MathBaseTpl<Scalar>::VectorXs&
180			DifferentialActionModelLQRTpl<Scalar>::get_lu() const {
181			return lu_;
182			}
183
184			template <typename Scalar>
185			const typename MathBaseTpl<Scalar>::MatrixXs&
186		13554	DifferentialActionModelLQRTpl<Scalar>::get_Lxx() const {
187		13554	return Lxx_;
188			}
189
190			template <typename Scalar>
191			const typename MathBaseTpl<Scalar>::MatrixXs&
192		13554	DifferentialActionModelLQRTpl<Scalar>::get_Lxu() const {
193		13554	return Lxu_;
194			}
195
196			template <typename Scalar>
197			const typename MathBaseTpl<Scalar>::MatrixXs&
198		13554	DifferentialActionModelLQRTpl<Scalar>::get_Luu() const {
199		13554	return Luu_;
200			}
201
202			template <typename Scalar>
203			void DifferentialActionModelLQRTpl<Scalar>::set_Fq(const MatrixXs& Fq) {
204			if (static_cast<std::size_t>(Fq.rows()) != state_->get_nq() \|\|
205			static_cast<std::size_t>(Fq.cols()) != state_->get_nq()) {
206			throw_pretty("Invalid argument: "
207			<< "Fq has wrong dimension (it should be " +
208			std::to_string(state_->get_nq()) + "," +
209			std::to_string(state_->get_nq()) + ")");
210			}
211			Fq_ = Fq;
212			}
213
214			template <typename Scalar>
215			void DifferentialActionModelLQRTpl<Scalar>::set_Fv(const MatrixXs& Fv) {
216			if (static_cast<std::size_t>(Fv.rows()) != state_->get_nv() \|\|
217			static_cast<std::size_t>(Fv.cols()) != state_->get_nv()) {
218			throw_pretty("Invalid argument: "
219			<< "Fv has wrong dimension (it should be " +
220			std::to_string(state_->get_nv()) + "," +
221			std::to_string(state_->get_nv()) + ")");
222			}
223			Fv_ = Fv;
224			}
225
226			template <typename Scalar>
227			void DifferentialActionModelLQRTpl<Scalar>::set_Fu(const MatrixXs& Fu) {
228			if (static_cast<std::size_t>(Fu.rows()) != state_->get_nq() \|\|
229			static_cast<std::size_t>(Fu.cols()) != nu_) {
230			throw_pretty("Invalid argument: "
231			<< "Fu has wrong dimension (it should be " +
232			std::to_string(state_->get_nq()) + "," +
233			std::to_string(nu_) + ")");
234			}
235			Fu_ = Fu;
236			}
237
238			template <typename Scalar>
239			void DifferentialActionModelLQRTpl<Scalar>::set_f0(const VectorXs& f0) {
240			if (static_cast<std::size_t>(f0.size()) != state_->get_nv()) {
241			throw_pretty("Invalid argument: "
242			<< "f0 has wrong dimension (it should be " +
243			std::to_string(state_->get_nv()) + ")");
244			}
245			f0_ = f0;
246			}
247
248			template <typename Scalar>
249			void DifferentialActionModelLQRTpl<Scalar>::set_lx(const VectorXs& lx) {
250			if (static_cast<std::size_t>(lx.size()) != state_->get_nx()) {
251			throw_pretty("Invalid argument: "
252			<< "lx has wrong dimension (it should be " +
253			std::to_string(state_->get_nx()) + ")");
254			}
255			lx_ = lx;
256			}
257
258			template <typename Scalar>
259			void DifferentialActionModelLQRTpl<Scalar>::set_lu(const VectorXs& lu) {
260			if (static_cast<std::size_t>(lu.size()) != nu_) {
261			throw_pretty("Invalid argument: "
262			<< "lu has wrong dimension (it should be " +
263			std::to_string(nu_) + ")");
264			}
265			lu_ = lu;
266			}
267
268			template <typename Scalar>
269			void DifferentialActionModelLQRTpl<Scalar>::set_Lxx(const MatrixXs& Lxx) {
270			if (static_cast<std::size_t>(Lxx.rows()) != state_->get_nx() \|\|
271			static_cast<std::size_t>(Lxx.cols()) != state_->get_nx()) {
272			throw_pretty("Invalid argument: "
273			<< "Lxx has wrong dimension (it should be " +
274			std::to_string(state_->get_nx()) + "," +
275			std::to_string(state_->get_nx()) + ")");
276			}
277			Lxx_ = Lxx;
278			}
279
280			template <typename Scalar>
281			void DifferentialActionModelLQRTpl<Scalar>::set_Lxu(const MatrixXs& Lxu) {
282			if (static_cast<std::size_t>(Lxu.rows()) != state_->get_nx() \|\|
283			static_cast<std::size_t>(Lxu.cols()) != nu_) {
284			throw_pretty("Invalid argument: "
285			<< "Lxu has wrong dimension (it should be " +
286			std::to_string(state_->get_nx()) + "," +
287			std::to_string(nu_) + ")");
288			}
289			Lxu_ = Lxu;
290			}
291
292			template <typename Scalar>
293			void DifferentialActionModelLQRTpl<Scalar>::set_Luu(const MatrixXs& Luu) {
294			if (static_cast<std::size_t>(Luu.rows()) != nu_ \|\|
295			static_cast<std::size_t>(Luu.cols()) != nu_) {
296			throw_pretty("Invalid argument: "
297			<< "Fq has wrong dimension (it should be " +
298			std::to_string(nu_) + "," + std::to_string(nu_) + ")");
299			}
300			Luu_ = Luu;
301			}
302
303			} // namespace crocoddyl