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			template <typename Scalar>
12		✗	ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
13			: ActionModelAbstractTpl<Scalar>(
14		✗	std::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
15		✗	dt_(Scalar(0.1)) {
16		✗	cost_weights_ << Scalar(10.), Scalar(1.);
17		✗	}
18
19			template <typename Scalar>
20		✗	void ActionModelUnicycleTpl<Scalar>::calc(
21			const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
22			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
23		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
24		✗	throw_pretty(
25			"Invalid argument: " << "x has wrong dimension (it should be " +
26			std::to_string(state_->get_nx()) + ")");
27			}
28		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
29		✗	throw_pretty(
30			"Invalid argument: " << "u has wrong dimension (it should be " +
31			std::to_string(nu_) + ")");
32			}
33		✗	Data* d = static_cast<Data*>(data.get());
34
35		✗	const Scalar c = cos(x[2]);
36		✗	const Scalar s = sin(x[2]);
37		✗	d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
38		✗	d->r.template head<3>() = cost_weights_[0] * x;
39		✗	d->r.template tail<2>() = cost_weights_[1] * u;
40		✗	d->cost = Scalar(0.5) * d->r.dot(d->r);
41		✗	}
42
43			template <typename Scalar>
44		✗	void ActionModelUnicycleTpl<Scalar>::calc(
45			const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
46			const Eigen::Ref<const VectorXs>& x) {
47		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
48		✗	throw_pretty(
49			"Invalid argument: " << "x has wrong dimension (it should be " +
50			std::to_string(state_->get_nx()) + ")");
51			}
52		✗	Data* d = static_cast<Data*>(data.get());
53
54		✗	d->xnext = x;
55		✗	d->r.template head<3>() = cost_weights_[0] * x;
56		✗	d->r.template tail<2>().setZero();
57		✗	d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
58		✗	}
59
60			template <typename Scalar>
61		✗	void ActionModelUnicycleTpl<Scalar>::calcDiff(
62			const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
63			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
64		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
65		✗	throw_pretty(
66			"Invalid argument: " << "x has wrong dimension (it should be " +
67			std::to_string(state_->get_nx()) + ")");
68			}
69		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
70		✗	throw_pretty(
71			"Invalid argument: " << "u has wrong dimension (it should be " +
72			std::to_string(nu_) + ")");
73			}
74		✗	Data* d = static_cast<Data*>(data.get());
75
76		✗	const Scalar c = static_cast<Scalar>(cos(x[2]));
77		✗	const Scalar s = static_cast<Scalar>(sin(x[2]));
78		✗	const Scalar w_x = cost_weights_[0] * cost_weights_[0];
79		✗	const Scalar w_u = cost_weights_[1] * cost_weights_[1];
80		✗	d->Lx = x * w_x;
81		✗	d->Lu = u * w_u;
82		✗	d->Lxx.diagonal().setConstant(w_x);
83		✗	d->Luu.diagonal().setConstant(w_u);
84		✗	d->Fx(0, 2) = -s * u[0] * dt_;
85		✗	d->Fx(1, 2) = c * u[0] * dt_;
86		✗	d->Fu(0, 0) = c * dt_;
87		✗	d->Fu(1, 0) = s * dt_;
88		✗	d->Fu(2, 1) = dt_;
89		✗	}
90
91			template <typename Scalar>
92		✗	void ActionModelUnicycleTpl<Scalar>::calcDiff(
93			const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
94			const Eigen::Ref<const VectorXs>& x) {
95		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
96		✗	throw_pretty(
97			"Invalid argument: " << "x has wrong dimension (it should be " +
98			std::to_string(state_->get_nx()) + ")");
99			}
100		✗	Data* d = static_cast<Data*>(data.get());
101
102		✗	const Scalar w_x = cost_weights_[0] * cost_weights_[0];
103		✗	d->Lx = x * w_x;
104		✗	d->Lxx.diagonal().setConstant(w_x);
105		✗	}
106
107			template <typename Scalar>
108			std::shared_ptr<ActionDataAbstractTpl<Scalar> >
109		✗	ActionModelUnicycleTpl<Scalar>::createData() {
110		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
111			}
112
113			template <typename Scalar>
114			template <typename NewScalar>
115		✗	ActionModelUnicycleTpl<NewScalar> ActionModelUnicycleTpl<Scalar>::cast() const {
116			typedef ActionModelUnicycleTpl<NewScalar> ReturnType;
117		✗	ReturnType ret;
118		✗	return ret;
119			}
120
121			template <typename Scalar>
122		✗	bool ActionModelUnicycleTpl<Scalar>::checkData(
123			const std::shared_ptr<ActionDataAbstract>& data) {
124		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
125		✗	if (d != NULL) {
126		✗	return true;
127			} else {
128		✗	return false;
129			}
130		✗	}
131
132			template <typename Scalar>
133		✗	void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
134		✗	os << "ActionModelUnicycle {dt=" << dt_ << "}";
135		✗	}
136
137			template <typename Scalar>
138			const typename MathBaseTpl<Scalar>::Vector2s&
139		✗	ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
140		✗	return cost_weights_;
141			}
142
143			template <typename Scalar>
144		✗	void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
145			const typename MathBase::Vector2s& weights) {
146		✗	cost_weights_ = weights;
147		✗	}
148
149			template <typename Scalar>
150		✗	Scalar ActionModelUnicycleTpl<Scalar>::get_dt() const {
151		✗	return dt_;
152			}
153
154			template <typename Scalar>
155		✗	void ActionModelUnicycleTpl<Scalar>::set_dt(const Scalar dt) {
156		✗	if (dt <= 0)
157		✗	throw_pretty("Invalid argument: dt should be strictly positive.");
158		✗	dt_ = dt;
159		✗	}
160
161			} // namespace crocoddyl
162