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
12			template <typename Scalar>
13		✗	CostModelSumTpl<Scalar>::CostModelSumTpl(std::shared_ptr<StateAbstract> state,
14			const std::size_t nu)
15		✗	: state_(state), nu_(nu), nr_(0), nr_total_(0) {}
16
17			template <typename Scalar>
18		✗	CostModelSumTpl<Scalar>::CostModelSumTpl(std::shared_ptr<StateAbstract> state)
19		✗	: state_(state), nu_(state->get_nv()), nr_(0), nr_total_(0) {}
20
21			template <typename Scalar>
22		✗	CostModelSumTpl<Scalar>::~CostModelSumTpl() {}
23
24			template <typename Scalar>
25		✗	void CostModelSumTpl<Scalar>::addCost(const std::string& name,
26			std::shared_ptr<CostModelAbstract> cost,
27			const Scalar weight, const bool active) {
28		✗	if (cost->get_nu() != nu_) {
29		✗	throw_pretty(
30			name
31			<< " cost item doesn't have the same control dimension (it should be " +
32			std::to_string(nu_) + ")");
33			}
34			std::pair<typename CostModelContainer::iterator, bool> ret =
35		✗	costs_.insert(std::make_pair(
36			name, std::make_shared<CostItem>(name, cost, weight, active)));
37		✗	if (ret.second == false) {
38			std::cerr << "Warning: we couldn't add the " << name
39		✗	<< " cost item, it already existed." << std::endl;
40		✗	} else if (active) {
41		✗	nr_ += cost->get_activation()->get_nr();
42		✗	nr_total_ += cost->get_activation()->get_nr();
43		✗	active_set_.insert(name);
44		✗	} else if (!active) {
45		✗	nr_total_ += cost->get_activation()->get_nr();
46		✗	inactive_set_.insert(name);
47			}
48		✗	}
49
50			template <typename Scalar>
51		✗	void CostModelSumTpl<Scalar>::addCost(
52			const std::shared_ptr<CostItem>& cost_item) {
53		✗	if (cost_item->cost->get_nu() != nu_) {
54		✗	throw_pretty(
55			cost_item->name
56			<< " cost item doesn't have the same control dimension (it should be " +
57			std::to_string(nu_) + ")");
58			}
59		✗	costs_.insert(std::make_pair(cost_item->name, cost_item));
60		✗	if (cost_item->active) {
61		✗	nr_ += cost_item->cost->get_activation()->get_nr();
62		✗	nr_total_ += cost_item->cost->get_activation()->get_nr();
63		✗	active_set_.insert(cost_item->name);
64			} else {
65		✗	nr_total_ += cost_item->cost->get_activation()->get_nr();
66		✗	inactive_set_.insert(cost_item->name);
67			}
68		✗	}
69
70			template <typename Scalar>
71		✗	void CostModelSumTpl<Scalar>::removeCost(const std::string& name) {
72		✗	typename CostModelContainer::iterator it = costs_.find(name);
73		✗	if (it != costs_.end()) {
74		✗	nr_ -= it->second->cost->get_activation()->get_nr();
75		✗	nr_total_ -= it->second->cost->get_activation()->get_nr();
76		✗	costs_.erase(it);
77		✗	active_set_.erase(name);
78		✗	inactive_set_.erase(name);
79			} else {
80			std::cerr << "Warning: we couldn't remove the " << name
81		✗	<< " cost item, it doesn't exist." << std::endl;
82			}
83		✗	}
84
85			template <typename Scalar>
86		✗	void CostModelSumTpl<Scalar>::changeCostStatus(const std::string& name,
87			const bool active) {
88		✗	typename CostModelContainer::iterator it = costs_.find(name);
89		✗	if (it != costs_.end()) {
90		✗	if (active && !it->second->active) {
91		✗	nr_ += it->second->cost->get_activation()->get_nr();
92		✗	active_set_.insert(name);
93		✗	inactive_set_.erase(name);
94		✗	it->second->active = active;
95		✗	} else if (!active && it->second->active) {
96		✗	nr_ -= it->second->cost->get_activation()->get_nr();
97		✗	active_set_.erase(name);
98		✗	inactive_set_.insert(name);
99		✗	it->second->active = active;
100			}
101			} else {
102			std::cerr << "Warning: we couldn't change the status of the " << name
103		✗	<< " cost item, it doesn't exist." << std::endl;
104			}
105		✗	}
106
107			template <typename Scalar>
108		✗	void CostModelSumTpl<Scalar>::calc(const std::shared_ptr<CostDataSum>& data,
109			const Eigen::Ref<const VectorXs>& x,
110			const Eigen::Ref<const VectorXs>& u) {
111		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
112		✗	throw_pretty(
113			"Invalid argument: " << "x has wrong dimension (it should be " +
114			std::to_string(state_->get_nx()) + ")");
115			}
116		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
117		✗	throw_pretty(
118			"Invalid argument: " << "u has wrong dimension (it should be " +
119			std::to_string(nu_) + ")");
120			}
121		✗	if (data->costs.size() != costs_.size()) {
122		✗	throw_pretty("Invalid argument: "
123			<< "it doesn't match the number of cost datas and models");
124			}
125		✗	data->cost = Scalar(0.);
126
127		✗	typename CostModelContainer::iterator it_m, end_m;
128		✗	typename CostDataContainer::iterator it_d, end_d;
129		✗	for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
130		✗	end_d = data->costs.end();
131		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
132		✗	const std::shared_ptr<CostItem>& m_i = it_m->second;
133		✗	if (m_i->active) {
134		✗	const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
135		✗	assert_pretty(it_m->first == it_d->first,
136			"it doesn't match the cost name between model and data ("
137			<< it_m->first << " != " << it_d->first << ")");
138
139		✗	m_i->cost->calc(d_i, x, u);
140		✗	data->cost += m_i->weight * d_i->cost;
141			}
142			}
143		✗	}
144
145			template <typename Scalar>
146		✗	void CostModelSumTpl<Scalar>::calc(const std::shared_ptr<CostDataSum>& data,
147			const Eigen::Ref<const VectorXs>& x) {
148		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
149		✗	throw_pretty(
150			"Invalid argument: " << "x has wrong dimension (it should be " +
151			std::to_string(state_->get_nx()) + ")");
152			}
153		✗	if (data->costs.size() != costs_.size()) {
154		✗	throw_pretty("Invalid argument: "
155			<< "it doesn't match the number of cost datas and models");
156			}
157		✗	data->cost = Scalar(0.);
158
159		✗	typename CostModelContainer::iterator it_m, end_m;
160		✗	typename CostDataContainer::iterator it_d, end_d;
161		✗	for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
162		✗	end_d = data->costs.end();
163		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
164		✗	const std::shared_ptr<CostItem>& m_i = it_m->second;
165		✗	if (m_i->active) {
166		✗	const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
167		✗	assert_pretty(it_m->first == it_d->first,
168			"it doesn't match the cost name between model and data ("
169			<< it_m->first << " != " << it_d->first << ")");
170
171		✗	m_i->cost->calc(d_i, x);
172		✗	data->cost += m_i->weight * d_i->cost;
173			}
174			}
175		✗	}
176
177			template <typename Scalar>
178		✗	void CostModelSumTpl<Scalar>::calcDiff(const std::shared_ptr<CostDataSum>& data,
179			const Eigen::Ref<const VectorXs>& x,
180			const Eigen::Ref<const VectorXs>& u) {
181		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
182		✗	throw_pretty(
183			"Invalid argument: " << "x has wrong dimension (it should be " +
184			std::to_string(state_->get_nx()) + ")");
185			}
186		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
187		✗	throw_pretty(
188			"Invalid argument: " << "u has wrong dimension (it should be " +
189			std::to_string(nu_) + ")");
190			}
191		✗	if (data->costs.size() != costs_.size()) {
192		✗	throw_pretty("Invalid argument: "
193			<< "it doesn't match the number of cost datas and models");
194			}
195		✗	data->Lx.setZero();
196		✗	data->Lu.setZero();
197		✗	data->Lxx.setZero();
198		✗	data->Lxu.setZero();
199		✗	data->Luu.setZero();
200
201		✗	typename CostModelContainer::iterator it_m, end_m;
202		✗	typename CostDataContainer::iterator it_d, end_d;
203		✗	for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
204		✗	end_d = data->costs.end();
205		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
206		✗	const std::shared_ptr<CostItem>& m_i = it_m->second;
207		✗	if (m_i->active) {
208		✗	const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
209		✗	assert_pretty(it_m->first == it_d->first,
210			"it doesn't match the cost name between model and data ("
211			<< it_m->first << " != " << it_d->first << ")");
212
213		✗	m_i->cost->calcDiff(d_i, x, u);
214		✗	data->Lx += m_i->weight * d_i->Lx;
215		✗	data->Lu += m_i->weight * d_i->Lu;
216		✗	data->Lxx += m_i->weight * d_i->Lxx;
217		✗	data->Lxu += m_i->weight * d_i->Lxu;
218		✗	data->Luu += m_i->weight * d_i->Luu;
219			}
220			}
221		✗	}
222
223			template <typename Scalar>
224		✗	void CostModelSumTpl<Scalar>::calcDiff(const std::shared_ptr<CostDataSum>& data,
225			const Eigen::Ref<const VectorXs>& x) {
226		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
227		✗	throw_pretty(
228			"Invalid argument: " << "x has wrong dimension (it should be " +
229			std::to_string(state_->get_nx()) + ")");
230			}
231		✗	if (data->costs.size() != costs_.size()) {
232		✗	throw_pretty("Invalid argument: "
233			<< "it doesn't match the number of cost datas and models");
234			}
235		✗	data->Lx.setZero();
236		✗	data->Lxx.setZero();
237
238		✗	typename CostModelContainer::iterator it_m, end_m;
239		✗	typename CostDataContainer::iterator it_d, end_d;
240		✗	for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
241		✗	end_d = data->costs.end();
242		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
243		✗	const std::shared_ptr<CostItem>& m_i = it_m->second;
244		✗	if (m_i->active) {
245		✗	const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
246		✗	assert_pretty(it_m->first == it_d->first,
247			"it doesn't match the cost name between model and data ("
248			<< it_m->first << " != " << it_d->first << ")");
249
250		✗	m_i->cost->calcDiff(d_i, x);
251		✗	data->Lx += m_i->weight * d_i->Lx;
252		✗	data->Lxx += m_i->weight * d_i->Lxx;
253			}
254			}
255		✗	}
256
257			template <typename Scalar>
258		✗	std::shared_ptr<CostDataSumTpl<Scalar> > CostModelSumTpl<Scalar>::createData(
259			DataCollectorAbstract* const data) {
260			return std::allocate_shared<CostDataSum>(
261		✗	Eigen::aligned_allocator<CostDataSum>(), this, data);
262			}
263
264			template <typename Scalar>
265			template <typename NewScalar>
266		✗	CostModelSumTpl<NewScalar> CostModelSumTpl<Scalar>::cast() const {
267			typedef CostModelSumTpl<NewScalar> ReturnType;
268			typedef CostItemTpl<NewScalar> CostType;
269		✗	ReturnType ret(state_->template cast<NewScalar>(), nu_);
270		✗	typename CostModelContainer::const_iterator it_m, end_m;
271		✗	for (it_m = costs_.begin(), end_m = costs_.end(); it_m != end_m; ++it_m) {
272		✗	const std::string name = it_m->first;
273		✗	const CostType& m_i = it_m->second->template cast<NewScalar>();
274		✗	ret.addCost(name, m_i.cost, m_i.weight, m_i.active);
275			}
276		✗	return ret;
277		✗	}
278
279			template <typename Scalar>
280			const std::shared_ptr<StateAbstractTpl<Scalar> >&
281		✗	CostModelSumTpl<Scalar>::get_state() const {
282		✗	return state_;
283			}
284
285			template <typename Scalar>
286			const typename CostModelSumTpl<Scalar>::CostModelContainer&
287		✗	CostModelSumTpl<Scalar>::get_costs() const {
288		✗	return costs_;
289			}
290
291			template <typename Scalar>
292		✗	std::size_t CostModelSumTpl<Scalar>::get_nu() const {
293		✗	return nu_;
294			}
295
296			template <typename Scalar>
297		✗	std::size_t CostModelSumTpl<Scalar>::get_nr() const {
298		✗	return nr_;
299			}
300
301			template <typename Scalar>
302		✗	std::size_t CostModelSumTpl<Scalar>::get_nr_total() const {
303		✗	return nr_total_;
304			}
305
306			template <typename Scalar>
307		✗	const std::set<std::string>& CostModelSumTpl<Scalar>::get_active_set() const {
308		✗	return active_set_;
309			}
310
311			template <typename Scalar>
312		✗	const std::set<std::string>& CostModelSumTpl<Scalar>::get_inactive_set() const {
313		✗	return inactive_set_;
314			}
315
316			template <typename Scalar>
317		✗	bool CostModelSumTpl<Scalar>::getCostStatus(const std::string& name) const {
318		✗	typename CostModelContainer::const_iterator it = costs_.find(name);
319		✗	if (it != costs_.end()) {
320		✗	return it->second->active;
321			} else {
322			std::cerr << "Warning: we couldn't get the status of the " << name
323		✗	<< " cost item, it doesn't exist." << std::endl;
324		✗	return false;
325			}
326			}
327
328			template <typename Scalar>
329		✗	std::ostream& operator<<(std::ostream& os,
330			const CostModelSumTpl<Scalar>& model) {
331		✗	const std::set<std::string>& active = model.get_active_set();
332		✗	const std::set<std::string>& inactive = model.get_inactive_set();
333		✗	os << "CostModelSum:" << std::endl;
334		✗	os << " Active:" << std::endl;
335		✗	for (std::set<std::string>::const_iterator it = active.begin();
336		✗	it != active.end(); ++it) {
337			const std::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
338		✗	cost_item = model.get_costs().find(*it)->second;
339		✗	if (it != --active.end()) {
340		✗	os << " " << *it << ": " << *cost_item << std::endl;
341			} else {
342		✗	os << " " << *it << ": " << *cost_item << std::endl;
343			}
344			}
345		✗	os << " Inactive:" << std::endl;
346		✗	for (std::set<std::string>::const_iterator it = inactive.begin();
347		✗	it != inactive.end(); ++it) {
348			const std::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
349		✗	cost_item = model.get_costs().find(*it)->second;
350		✗	if (it != --inactive.end()) {
351		✗	os << " " << *it << ": " << *cost_item << std::endl;
352			} else {
353		✗	os << " " << *it << ": " << *cost_item;
354			}
355			}
356		✗	return os;
357			}
358
359			} // namespace crocoddyl
360