GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, University of Edinburgh, University of Trento,
5			// LAAS-CNRS, IRI: CSIC-UPC, 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		✗	IntegratedActionModelRKTpl<Scalar>::IntegratedActionModelRKTpl(
14			std::shared_ptr<DifferentialActionModelAbstract> model,
15			std::shared_ptr<ControlParametrizationModelAbstract> control,
16			const RKType rktype, const Scalar time_step, const bool with_cost_residual)
17		✗	: Base(model, control, time_step, with_cost_residual), rk_type_(rktype) {
18		✗	set_rk_type(rktype);
19		✗	}
20
21			template <typename Scalar>
22		✗	IntegratedActionModelRKTpl<Scalar>::IntegratedActionModelRKTpl(
23			std::shared_ptr<DifferentialActionModelAbstract> model, const RKType rktype,
24			const Scalar time_step, const bool with_cost_residual)
25		✗	: Base(model, time_step, with_cost_residual), rk_type_(rktype) {
26		✗	set_rk_type(rktype);
27		✗	}
28
29			template <typename Scalar>
30		✗	void IntegratedActionModelRKTpl<Scalar>::calc(
31			const std::shared_ptr<ActionDataAbstract>& data,
32			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
33		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
34		✗	throw_pretty(
35			"Invalid argument: " << "x has wrong dimension (it should be " +
36			std::to_string(state_->get_nx()) + ")");
37			}
38		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
39		✗	throw_pretty(
40			"Invalid argument: " << "u has wrong dimension (it should be " +
41			std::to_string(nu_) + ")");
42			}
43		✗	const std::size_t nv = state_->get_nv();
44		✗	Data* d = static_cast<Data*>(data.get());
45
46			const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
47		✗	d->differential[0];
48			const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
49		✗	d->control[0];
50		✗	control_->calc(u0_data, rk_c_[0], u);
51		✗	d->ws[0] = u0_data->w;
52		✗	differential_->calc(k0_data, x, d->ws[0]);
53		✗	d->y[0] = x;
54		✗	d->ki[0].head(nv) = d->y[0].tail(nv);
55		✗	d->ki[0].tail(nv) = k0_data->xout;
56		✗	d->integral[0] = k0_data->cost;
57		✗	for (std::size_t i = 1; i < ni_; ++i) {
58			const std::shared_ptr<DifferentialActionDataAbstract>& ki_data =
59		✗	d->differential[i];
60			const std::shared_ptr<ControlParametrizationDataAbstract>& ui_data =
61		✗	d->control[i];
62		✗	d->dx_rk[i].noalias() = time_step_ * rk_c_[i] * d->ki[i - 1];
63		✗	state_->integrate(x, d->dx_rk[i], d->y[i]);
64		✗	control_->calc(ui_data, rk_c_[i], u);
65		✗	d->ws[i] = ui_data->w;
66		✗	differential_->calc(ki_data, d->y[i], d->ws[i]);
67		✗	d->ki[i].head(nv) = d->y[i].tail(nv);
68		✗	d->ki[i].tail(nv) = ki_data->xout;
69		✗	d->integral[i] = ki_data->cost;
70			}
71
72		✗	if (ni_ == 2) {
73		✗	d->dx = d->ki[1] * time_step_;
74		✗	d->cost = d->integral[1] * time_step_;
75		✗	} else if (ni_ == 3) {
76		✗	d->dx = (d->ki[0] + Scalar(3.) * d->ki[2]) * time_step_ / Scalar(4.);
77		✗	d->cost = (d->integral[0] + Scalar(3.) * d->integral[2]) * time_step_ /
78			Scalar(4.);
79			} else {
80		✗	d->dx =
81		✗	(d->ki[0] + Scalar(2.) * d->ki[1] + Scalar(2.) * d->ki[2] + d->ki[3]) *
82		✗	time_step_ / Scalar(6.);
83		✗	d->cost = (d->integral[0] + Scalar(2.) * d->integral[1] +
84		✗	Scalar(2.) * d->integral[2] + d->integral[3]) *
85		✗	time_step_ / Scalar(6.);
86			}
87		✗	state_->integrate(x, d->dx, d->xnext);
88		✗	d->g = k0_data->g;
89		✗	d->h = k0_data->h;
90		✗	if (with_cost_residual_) {
91		✗	d->r = k0_data->r;
92			}
93		✗	}
94
95			template <typename Scalar>
96		✗	void IntegratedActionModelRKTpl<Scalar>::calc(
97			const std::shared_ptr<ActionDataAbstract>& data,
98			const Eigen::Ref<const VectorXs>& x) {
99		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
100		✗	throw_pretty(
101			"Invalid argument: " << "x has wrong dimension (it should be " +
102			std::to_string(state_->get_nx()) + ")");
103			}
104		✗	Data* d = static_cast<Data*>(data.get());
105
106			const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
107		✗	d->differential[0];
108		✗	differential_->calc(k0_data, x);
109		✗	d->dx.setZero();
110		✗	d->xnext = x;
111		✗	d->cost = k0_data->cost;
112		✗	d->g = k0_data->g;
113		✗	d->h = k0_data->h;
114		✗	if (with_cost_residual_) {
115		✗	d->r = k0_data->r;
116			}
117		✗	}
118
119			template <typename Scalar>
120		✗	void IntegratedActionModelRKTpl<Scalar>::calcDiff(
121			const std::shared_ptr<ActionDataAbstract>& data,
122			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
123		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
124		✗	throw_pretty(
125			"Invalid argument: " << "x has wrong dimension (it should be " +
126			std::to_string(state_->get_nx()) + ")");
127			}
128		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
129		✗	throw_pretty(
130			"Invalid argument: " << "u has wrong dimension (it should be " +
131			std::to_string(nu_) + ")");
132			}
133		✗	const std::size_t nv = state_->get_nv();
134		✗	const std::size_t nu = control_->get_nu();
135		✗	Data* d = static_cast<Data*>(data.get());
136		✗	assert_pretty(
137			MatrixXs(d->dyi_dx[0])
138			.isApprox(MatrixXs::Identity(state_->get_ndx(), state_->get_ndx())),
139			"you have changed dyi_dx[0] values that supposed to be constant.");
140		✗	assert_pretty(
141			MatrixXs(d->dki_dx[0])
142			.topRightCorner(nv, nv)
143			.isApprox(MatrixXs::Identity(nv, nv)),
144			"you have changed dki_dx[0] values that supposed to be constant.");
145
146		✗	for (std::size_t i = 0; i < ni_; ++i) {
147		✗	differential_->calcDiff(d->differential[i], d->y[i], d->ws[i]);
148			}
149
150			const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
151		✗	d->differential[0];
152			const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
153		✗	d->control[0];
154		✗	d->dki_dx[0].bottomRows(nv) = k0_data->Fx;
155		✗	control_->multiplyByJacobian(
156		✗	u0_data, k0_data->Fu,
157		✗	d->dki_du[0].bottomRows(nv)); // dki_du = dki_dw * dw_du
158
159		✗	d->dli_dx[0] = k0_data->Lx;
160		✗	control_->multiplyJacobianTransposeBy(
161		✗	u0_data, k0_data->Lu,
162		✗	d->dli_du[0]); // dli_du = dli_dw * dw_du
163
164		✗	d->ddli_ddx[0] = k0_data->Lxx;
165		✗	d->ddli_ddw[0] = k0_data->Luu;
166		✗	control_->multiplyByJacobian(
167		✗	u0_data, d->ddli_ddw[0],
168		✗	d->ddli_dwdu[0]); // ddli_dwdu = ddli_ddw * dw_du
169		✗	control_->multiplyJacobianTransposeBy(
170		✗	u0_data, d->ddli_dwdu[0],
171		✗	d->ddli_ddu[0]); // ddli_ddu = dw_du.T * ddli_dwdu
172		✗	d->ddli_dxdw[0] = k0_data->Lxu;
173		✗	control_->multiplyByJacobian(
174		✗	u0_data, d->ddli_dxdw[0],
175		✗	d->ddli_dxdu[0]); // ddli_dxdu = ddli_dxdw * dw_du
176
177		✗	for (std::size_t i = 1; i < ni_; ++i) {
178			const std::shared_ptr<DifferentialActionDataAbstract>& ki_data =
179		✗	d->differential[i];
180			const std::shared_ptr<ControlParametrizationDataAbstract>& ui_data =
181		✗	d->control[i];
182		✗	d->dyi_dx[i].noalias() = d->dki_dx[i - 1] * rk_c_[i] * time_step_;
183		✗	d->dyi_du[i].noalias() = d->dki_du[i - 1] * rk_c_[i] * time_step_;
184		✗	state_->JintegrateTransport(x, d->dx_rk[i], d->dyi_dx[i], second);
185		✗	state_->Jintegrate(x, d->dx_rk[i], d->dyi_dx[i], d->dyi_dx[i], first,
186			addto);
187		✗	state_->JintegrateTransport(x, d->dx_rk[i], d->dyi_du[i],
188			second); // dyi_du = Jintegrate * dyi_du
189
190			// Sparse matrix-matrix multiplication for computing:
191		✗	Eigen::Block<MatrixXs> dkvi_dq = d->dki_dx[i].bottomLeftCorner(nv, nv);
192		✗	Eigen::Block<MatrixXs> dkvi_dv = d->dki_dx[i].bottomRightCorner(nv, nv);
193		✗	Eigen::Block<MatrixXs> dkqi_du = d->dki_du[i].topLeftCorner(nv, nu);
194		✗	Eigen::Block<MatrixXs> dkvi_du = d->dki_du[i].bottomLeftCorner(nv, nu);
195		✗	const Eigen::Block<MatrixXs> dki_dqi = ki_data->Fx.bottomLeftCorner(nv, nv);
196			const Eigen::Block<MatrixXs> dki_dvi =
197		✗	ki_data->Fx.bottomRightCorner(nv, nv);
198		✗	const Eigen::Block<MatrixXs> dqi_dq = d->dyi_dx[i].topLeftCorner(nv, nv);
199		✗	const Eigen::Block<MatrixXs> dqi_dv = d->dyi_dx[i].topRightCorner(nv, nv);
200		✗	const Eigen::Block<MatrixXs> dvi_dq = d->dyi_dx[i].bottomLeftCorner(nv, nv);
201			const Eigen::Block<MatrixXs> dvi_dv =
202		✗	d->dyi_dx[i].bottomRightCorner(nv, nv);
203		✗	const Eigen::Block<MatrixXs> dqi_du = d->dyi_du[i].topLeftCorner(nv, nu);
204		✗	const Eigen::Block<MatrixXs> dvi_du = d->dyi_du[i].bottomLeftCorner(nv, nu);
205			// i. d->dki_dx[i].noalias() = d->dki_dy[i] * d->dyi_dx[i], where dki_dy
206			// is ki_data.Fx
207		✗	d->dki_dx[i].topRows(nv) = d->dyi_dx[i].bottomRows(nv);
208		✗	dkvi_dq.noalias() = dki_dqi * dqi_dq;
209		✗	if (i == 1) {
210		✗	dkvi_dv = time_step_ / Scalar(2.) * dki_dqi;
211			} else {
212		✗	dkvi_dv.noalias() = dki_dqi * dqi_dv;
213			}
214		✗	dkvi_dq.noalias() += dki_dvi * dvi_dq;
215		✗	dkvi_dv.noalias() += dki_dvi * dvi_dv;
216			// ii. d->dki_du[i].noalias() = d->dki_dy[i] * d->dyi_du[i], where dki_dy
217			// is ki_data.Fx
218		✗	dkqi_du = dvi_du;
219		✗	dkvi_du.noalias() = dki_dqi * dqi_du;
220		✗	dkvi_du.noalias() += dki_dvi * dvi_du;
221
222		✗	control_->multiplyByJacobian(ui_data, ki_data->Fu,
223		✗	d->dki_du[i].bottomRows(nv),
224			addto); // dfi_du = dki_dw * dw_du
225
226		✗	d->dli_dx[i].noalias() = ki_data->Lx.transpose() * d->dyi_dx[i];
227		✗	control_->multiplyJacobianTransposeBy(ui_data, ki_data->Lu,
228		✗	d->dli_du[i]); // dli_du = Lu * dw_du
229		✗	d->dli_du[i].noalias() += ki_data->Lx.transpose() * d->dyi_du[i];
230
231		✗	d->Lxx_partialx[i].noalias() = ki_data->Lxx * d->dyi_dx[i];
232		✗	d->ddli_ddx[i].noalias() = d->dyi_dx[i].transpose() * d->Lxx_partialx[i];
233
234		✗	control_->multiplyByJacobian(ui_data, ki_data->Lxu,
235		✗	d->Lxu_i[i]); // Lxu = Lxw * dw_du
236		✗	d->Luu_partialx[i].noalias() = d->Lxu_i[i].transpose() * d->dyi_du[i];
237		✗	d->Lxx_partialu[i].noalias() = ki_data->Lxx * d->dyi_du[i];
238		✗	control_->multiplyByJacobian(
239		✗	ui_data, ki_data->Luu,
240		✗	d->ddli_dwdu[i]); // ddli_dwdu = ddli_ddw * dw_du
241		✗	control_->multiplyJacobianTransposeBy(
242		✗	ui_data, d->ddli_dwdu[i],
243		✗	d->ddli_ddu[i]); // ddli_ddu = dw_du.T * ddli_dwdu
244		✗	d->ddli_ddu[i].noalias() += d->Luu_partialx[i].transpose() +
245		✗	d->Luu_partialx[i] +
246		✗	d->dyi_du[i].transpose() * d->Lxx_partialu[i];
247
248		✗	d->ddli_dxdw[i].noalias() = d->dyi_dx[i].transpose() * ki_data->Lxu;
249		✗	control_->multiplyByJacobian(
250		✗	ui_data, d->ddli_dxdw[i],
251		✗	d->ddli_dxdu[i]); // ddli_dxdu = ddli_dxdw * dw_du
252		✗	d->ddli_dxdu[i].noalias() += d->dyi_dx[i].transpose() * d->Lxx_partialu[i];
253			}
254
255		✗	if (ni_ == 2) {
256		✗	d->Fx.noalias() = time_step_ * d->dki_dx[1];
257		✗	d->Fu.noalias() = time_step_ * d->dki_du[1];
258		✗	d->Lx.noalias() = time_step_ * d->dli_dx[1];
259		✗	d->Lu.noalias() = time_step_ * d->dli_du[1];
260		✗	d->Lxx.noalias() = time_step_ * d->ddli_ddx[1];
261		✗	d->Luu.noalias() = time_step_ * d->ddli_ddu[1];
262		✗	d->Lxu.noalias() = time_step_ * d->ddli_dxdu[1];
263		✗	} else if (ni_ == 3) {
264		✗	d->Fx.noalias() =
265		✗	time_step_ / Scalar(4.) * (d->dki_dx[0] + Scalar(3.) * d->dki_dx[2]);
266		✗	d->Fu.noalias() =
267		✗	time_step_ / Scalar(4.) * (d->dki_du[0] + Scalar(3.) * d->dki_du[2]);
268		✗	d->Lx.noalias() =
269		✗	time_step_ / Scalar(4.) * (d->dli_dx[0] + Scalar(3.) * d->dli_dx[2]);
270		✗	d->Lu.noalias() =
271		✗	time_step_ / Scalar(4.) * (d->dli_du[0] + Scalar(3.) * d->dli_du[2]);
272		✗	d->Lxx.noalias() = time_step_ / Scalar(4.) *
273		✗	(d->ddli_ddx[0] + Scalar(3.) * d->ddli_ddx[2]);
274		✗	d->Luu.noalias() = time_step_ / Scalar(4.) *
275		✗	(d->ddli_ddu[0] + Scalar(3.) * d->ddli_ddu[2]);
276		✗	d->Lxu.noalias() = time_step_ / Scalar(4.) *
277		✗	(d->ddli_dxdu[0] + Scalar(3.) * d->ddli_dxdu[2]);
278			} else {
279		✗	d->Fx.noalias() = time_step_ / Scalar(6.) *
280		✗	(d->dki_dx[0] + Scalar(2.) * d->dki_dx[1] +
281		✗	Scalar(2.) * d->dki_dx[2] + d->dki_dx[3]);
282		✗	d->Fu.noalias() = time_step_ / Scalar(6.) *
283		✗	(d->dki_du[0] + Scalar(2.) * d->dki_du[1] +
284		✗	Scalar(2.) * d->dki_du[2] + d->dki_du[3]);
285		✗	d->Lx.noalias() = time_step_ / Scalar(6.) *
286		✗	(d->dli_dx[0] + Scalar(2.) * d->dli_dx[1] +
287		✗	Scalar(2.) * d->dli_dx[2] + d->dli_dx[3]);
288		✗	d->Lu.noalias() = time_step_ / Scalar(6.) *
289		✗	(d->dli_du[0] + Scalar(2.) * d->dli_du[1] +
290		✗	Scalar(2.) * d->dli_du[2] + d->dli_du[3]);
291		✗	d->Lxx.noalias() = time_step_ / Scalar(6.) *
292		✗	(d->ddli_ddx[0] + Scalar(2.) * d->ddli_ddx[1] +
293		✗	Scalar(2.) * d->ddli_ddx[2] + d->ddli_ddx[3]);
294		✗	d->Luu.noalias() = time_step_ / Scalar(6.) *
295		✗	(d->ddli_ddu[0] + Scalar(2.) * d->ddli_ddu[1] +
296		✗	Scalar(2.) * d->ddli_ddu[2] + d->ddli_ddu[3]);
297		✗	d->Lxu.noalias() = time_step_ / Scalar(6.) *
298		✗	(d->ddli_dxdu[0] + Scalar(2.) * d->ddli_dxdu[1] +
299		✗	Scalar(2.) * d->ddli_dxdu[2] + d->ddli_dxdu[3]);
300			}
301		✗	d->Gx = k0_data->Gx;
302		✗	d->Hx = k0_data->Hx;
303		✗	d->Gu.conservativeResize(differential_->get_ng(), nu_);
304		✗	d->Hu.conservativeResize(differential_->get_nh(), nu_);
305		✗	control_->multiplyByJacobian(u0_data, k0_data->Gu, d->Gu);
306		✗	control_->multiplyByJacobian(u0_data, k0_data->Hu, d->Hu);
307
308		✗	state_->JintegrateTransport(x, d->dx, d->Fx, second);
309		✗	state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
310		✗	state_->JintegrateTransport(x, d->dx, d->Fu, second);
311		✗	}
312
313			template <typename Scalar>
314		✗	void IntegratedActionModelRKTpl<Scalar>::calcDiff(
315			const std::shared_ptr<ActionDataAbstract>& data,
316			const Eigen::Ref<const VectorXs>& x) {
317		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
318		✗	throw_pretty(
319			"Invalid argument: " << "x has wrong dimension (it should be " +
320			std::to_string(state_->get_nx()) + ")");
321			}
322		✗	Data* d = static_cast<Data*>(data.get());
323
324			const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
325		✗	d->differential[0];
326		✗	differential_->calcDiff(k0_data, x);
327		✗	d->Lx = k0_data->Lx;
328		✗	d->Lxx = k0_data->Lxx;
329		✗	d->Gx = k0_data->Gx;
330		✗	d->Hx = k0_data->Hx;
331		✗	}
332
333			template <typename Scalar>
334			std::shared_ptr<ActionDataAbstractTpl<Scalar> >
335		✗	IntegratedActionModelRKTpl<Scalar>::createData() {
336		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
337			}
338
339			template <typename Scalar>
340			template <typename NewScalar>
341		✗	IntegratedActionModelRKTpl<NewScalar> IntegratedActionModelRKTpl<Scalar>::cast()
342			const {
343			typedef IntegratedActionModelRKTpl<NewScalar> ReturnType;
344		✗	if (control_) {
345		✗	ReturnType ret(differential_->template cast<NewScalar>(),
346		✗	control_->template cast<NewScalar>(), rk_type_,
347		✗	scalar_cast<NewScalar>(time_step_), with_cost_residual_);
348		✗	return ret;
349		✗	} else {
350		✗	ReturnType ret(differential_->template cast<NewScalar>(), rk_type_,
351		✗	scalar_cast<NewScalar>(time_step_), with_cost_residual_);
352		✗	return ret;
353		✗	}
354			}
355
356			template <typename Scalar>
357		✗	bool IntegratedActionModelRKTpl<Scalar>::checkData(
358			const std::shared_ptr<ActionDataAbstract>& data) {
359		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
360		✗	if (data != NULL) {
361		✗	for (std::size_t i = 0; i < ni_; ++i) {
362		✗	if (!differential_->checkData(d->differential[i])) {
363		✗	return false;
364			}
365			}
366		✗	return true;
367			} else {
368		✗	return false;
369			}
370		✗	}
371
372			template <typename Scalar>
373		✗	void IntegratedActionModelRKTpl<Scalar>::quasiStatic(
374			const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
375			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
376			const Scalar tol) {
377		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
378		✗	throw_pretty(
379			"Invalid argument: " << "u has wrong dimension (it should be " +
380			std::to_string(nu_) + ")");
381			}
382		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
383		✗	throw_pretty(
384			"Invalid argument: " << "x has wrong dimension (it should be " +
385			std::to_string(state_->get_nx()) + ")");
386			}
387
388		✗	Data* d = static_cast<Data*>(data.get());
389			const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
390		✗	d->control[0];
391		✗	u0_data->w *= Scalar(0.);
392		✗	differential_->quasiStatic(d->differential[0], u0_data->w, x, maxiter, tol);
393		✗	control_->params(u0_data, Scalar(0.), u0_data->w);
394		✗	u = u0_data->u;
395		✗	}
396
397			template <typename Scalar>
398		✗	std::size_t IntegratedActionModelRKTpl<Scalar>::get_ni() const {
399		✗	return ni_;
400			}
401
402			template <typename Scalar>
403		✗	void IntegratedActionModelRKTpl<Scalar>::print(std::ostream& os) const {
404		✗	os << "IntegratedActionModelRK {dt=" << time_step_ << ", " << *differential_
405		✗	<< "}";
406		✗	}
407
408			template <typename Scalar>
409		✗	void IntegratedActionModelRKTpl<Scalar>::set_rk_type(const RKType rktype) {
410		✗	switch (rktype) {
411		✗	case two:
412		✗	ni_ = 2;
413		✗	rk_c_.resize(ni_);
414		✗	rk_c_[0] = Scalar(0.);
415		✗	rk_c_[1] = Scalar(0.5);
416		✗	break;
417		✗	case three:
418		✗	ni_ = 3;
419		✗	rk_c_.resize(ni_);
420		✗	rk_c_[0] = Scalar(0.);
421		✗	rk_c_[1] = Scalar(1. / 3.);
422		✗	rk_c_[2] = Scalar(2. / 3.);
423		✗	break;
424		✗	case four:
425		✗	ni_ = 4;
426		✗	rk_c_.resize(ni_);
427		✗	rk_c_[0] = Scalar(0.);
428		✗	rk_c_[1] = Scalar(0.5);
429		✗	rk_c_[2] = Scalar(0.5);
430		✗	rk_c_[3] = Scalar(1.);
431		✗	break;
432			}
433		✗	}
434
435			} // namespace crocoddyl
436