GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, University of Trento,
6			// Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#include "crocoddyl/core/controls/poly-zero.hpp"
12			#include "crocoddyl/core/integ-action-base.hpp"
13
14			namespace crocoddyl {
15
16			template <typename Scalar>
17		✗	IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
18			std::shared_ptr<DifferentialActionModelAbstract> model,
19			std::shared_ptr<ControlParametrizationModelAbstract> control,
20			const Scalar time_step, const bool with_cost_residual)
21			: Base(model->get_state(), control->get_nu(), model->get_nr(),
22		✗	model->get_ng(), model->get_nh()),
23		✗	differential_(model),
24		✗	control_(control),
25		✗	time_step_(time_step),
26		✗	with_cost_residual_(with_cost_residual) {
27		✗	if (control->get_nw() != model->get_nu()) {
28		✗	throw_pretty("Invalid argument: "
29			<< "control.nw (" + std::to_string(control->get_nw()) +
30			") is not equal to model.nu (" +
31			std::to_string(model->get_nu()) + ")");
32			}
33		✗	init();
34		✗	}
35
36			template <typename Scalar>
37		✗	IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
38			std::shared_ptr<DifferentialActionModelAbstract> model,
39			const Scalar time_step, const bool with_cost_residual)
40			: Base(model->get_state(), model->get_nu(), model->get_nr(),
41		✗	model->get_ng(), model->get_nh()),
42		✗	differential_(model),
43		✗	control_(
44		✗	new ControlParametrizationModelPolyZeroTpl<Scalar>(model->get_nu())),
45		✗	time_step_(time_step),
46		✗	with_cost_residual_(with_cost_residual) {
47		✗	init();
48		✗	}
49
50			template <typename Scalar>
51		✗	void IntegratedActionModelAbstractTpl<Scalar>::init() {
52		✗	time_step2_ = time_step_ * time_step_;
53		✗	VectorXs u_lb(nu_), u_ub(nu_);
54		✗	control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
55			u_lb, u_ub);
56		✗	Base::set_u_lb(u_lb);
57		✗	Base::set_u_ub(u_ub);
58		✗	if (time_step_ < Scalar(0.)) {
59		✗	time_step_ = Scalar(1e-3);
60		✗	time_step2_ = time_step_ * time_step_;
61		✗	std::cerr << "Warning: dt should be positive, set to 1e-3" << std::endl;
62			}
63		✗	}
64
65			template <typename Scalar>
66			std::shared_ptr<ActionDataAbstractTpl<Scalar> >
67		✗	IntegratedActionModelAbstractTpl<Scalar>::createData() {
68		✗	if (control_->get_nu() > differential_->get_nu())
69		✗	std::cerr << "Warning: It is useless to use an Euler integrator with a "
70			"control parametrization larger than PolyZero"
71		✗	<< std::endl;
72		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
73			}
74
75			template <typename Scalar>
76		✗	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng() const {
77		✗	return differential_->get_ng();
78			}
79
80			template <typename Scalar>
81		✗	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh() const {
82		✗	return differential_->get_nh();
83			}
84
85			template <typename Scalar>
86		✗	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng_T() const {
87		✗	return differential_->get_ng_T();
88			}
89
90			template <typename Scalar>
91		✗	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh_T() const {
92		✗	return differential_->get_nh_T();
93			}
94
95			template <typename Scalar>
96			const typename MathBaseTpl<Scalar>::VectorXs&
97		✗	IntegratedActionModelAbstractTpl<Scalar>::get_g_lb() const {
98		✗	return differential_->get_g_lb();
99			}
100
101			template <typename Scalar>
102			const typename MathBaseTpl<Scalar>::VectorXs&
103		✗	IntegratedActionModelAbstractTpl<Scalar>::get_g_ub() const {
104		✗	return differential_->get_g_ub();
105			}
106
107			template <typename Scalar>
108			const std::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
109		✗	IntegratedActionModelAbstractTpl<Scalar>::get_differential() const {
110		✗	return differential_;
111			}
112
113			template <typename Scalar>
114			const std::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
115		✗	IntegratedActionModelAbstractTpl<Scalar>::get_control() const {
116		✗	return control_;
117			}
118
119			template <typename Scalar>
120		✗	const Scalar IntegratedActionModelAbstractTpl<Scalar>::get_dt() const {
121		✗	return time_step_;
122			}
123
124			template <typename Scalar>
125		✗	void IntegratedActionModelAbstractTpl<Scalar>::set_dt(const Scalar dt) {
126		✗	if (dt < 0.) {
127		✗	throw_pretty("Invalid argument: " << "dt has positive value");
128			}
129		✗	time_step_ = dt;
130		✗	time_step2_ = dt * dt;
131		✗	}
132
133			template <typename Scalar>
134		✗	void IntegratedActionModelAbstractTpl<Scalar>::set_differential(
135			std::shared_ptr<DifferentialActionModelAbstract> model) {
136		✗	if (control_->get_nw() != model->get_nu()) {
137		✗	throw_pretty("Invalid argument: "
138			<< "control.nw (" + std::to_string(control_->get_nw()) +
139			") is not equal to model.nu (" +
140			std::to_string(model->get_nu()) + ")");
141			}
142
143		✗	nr_ = model->get_nr();
144		✗	state_ = model->get_state();
145		✗	differential_ = model;
146
147		✗	VectorXs p_lb(nu_), p_ub(nu_);
148		✗	control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
149			p_lb, p_ub);
150		✗	Base::set_u_lb(p_lb);
151		✗	Base::set_u_ub(p_ub);
152		✗	}
153
154			} // namespace crocoddyl
155