Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/costs/cost-sum.hxx	Lines:	146	171	85.4 %
Date:	2024-02-13 11:12:33	Branches:	120	478	25.1 %


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

#include <iostream>

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateAbstract> state,
                                         const std::size_t nu)
    : state_(state), nu_(nu), nr_(0), nr_total_(0) {}

template <typename Scalar>
CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateAbstract> state)
    : state_(state), nu_(state->get_nv()), nr_(0), nr_total_(0) {}

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

template <typename Scalar>
void CostModelSumTpl<Scalar>::addCost(const std::string& name,
                                      boost::shared_ptr<CostModelAbstract> cost,
                                      const Scalar weight, const bool active) {
  if (cost->get_nu() != nu_) {
    throw_pretty(
        name
        << " cost item doesn't have the same control dimension (it should be " +
               std::to_string(nu_) + ")");
  }
  std::pair<typename CostModelContainer::iterator, bool> ret =
      costs_.insert(std::make_pair(
          name, boost::make_shared<CostItem>(name, cost, weight, active)));
  if (ret.second == false) {
    std::cerr << "Warning: we couldn't add the " << name
              << " cost item, it already existed." << std::endl;
  } else if (active) {
    nr_ += cost->get_activation()->get_nr();
    nr_total_ += cost->get_activation()->get_nr();
    active_set_.insert(name);
  } else if (!active) {
    nr_total_ += cost->get_activation()->get_nr();
    inactive_set_.insert(name);
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::removeCost(const std::string& name) {
  typename CostModelContainer::iterator it = costs_.find(name);
  if (it != costs_.end()) {
    nr_ -= it->second->cost->get_activation()->get_nr();
    nr_total_ -= it->second->cost->get_activation()->get_nr();
    costs_.erase(it);
    active_set_.erase(name);
    inactive_set_.erase(name);
  } else {
    std::cerr << "Warning: we couldn't remove the " << name
              << " cost item, it doesn't exist." << std::endl;
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::changeCostStatus(const std::string& name,
                                               const bool active) {
  typename CostModelContainer::iterator it = costs_.find(name);
  if (it != costs_.end()) {
    if (active && !it->second->active) {
      nr_ += it->second->cost->get_activation()->get_nr();
      active_set_.insert(name);
      inactive_set_.erase(name);
      it->second->active = active;
    } else if (!active && it->second->active) {
      nr_ -= it->second->cost->get_activation()->get_nr();
      active_set_.erase(name);
      inactive_set_.insert(name);
      it->second->active = active;
    }
  } else {
    std::cerr << "Warning: we couldn't change the status of the " << name
              << " cost item, it doesn't exist." << std::endl;
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::calc(const boost::shared_ptr<CostDataSum>& 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_) + ")");
  }
  if (data->costs.size() != costs_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of cost datas and models");
  }
  data->cost = Scalar(0.);

  typename CostModelContainer::iterator it_m, end_m;
  typename CostDataContainer::iterator it_d, end_d;
  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
      end_d = data->costs.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<CostItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the cost name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->cost->calc(d_i, x, u);
      data->cost += m_i->weight * d_i->cost;
    }
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::calc(const boost::shared_ptr<CostDataSum>& 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()) + ")");
  }
  if (data->costs.size() != costs_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of cost datas and models");
  }
  data->cost = Scalar(0.);

  typename CostModelContainer::iterator it_m, end_m;
  typename CostDataContainer::iterator it_d, end_d;
  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
      end_d = data->costs.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<CostItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the cost name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->cost->calc(d_i, x);
      data->cost += m_i->weight * d_i->cost;
    }
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::calcDiff(
    const boost::shared_ptr<CostDataSum>& 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_) + ")");
  }
  if (data->costs.size() != costs_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of cost datas and models");
  }
  data->Lx.setZero();
  data->Lu.setZero();
  data->Lxx.setZero();
  data->Lxu.setZero();
  data->Luu.setZero();

  typename CostModelContainer::iterator it_m, end_m;
  typename CostDataContainer::iterator it_d, end_d;
  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
      end_d = data->costs.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<CostItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the cost name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->cost->calcDiff(d_i, x, u);
      data->Lx += m_i->weight * d_i->Lx;
      data->Lu += m_i->weight * d_i->Lu;
      data->Lxx += m_i->weight * d_i->Lxx;
      data->Lxu += m_i->weight * d_i->Lxu;
      data->Luu += m_i->weight * d_i->Luu;
    }
  }
}

template <typename Scalar>
void CostModelSumTpl<Scalar>::calcDiff(
    const boost::shared_ptr<CostDataSum>& 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()) + ")");
  }
  if (data->costs.size() != costs_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of cost datas and models");
  }
  data->Lx.setZero();
  data->Lxx.setZero();

  typename CostModelContainer::iterator it_m, end_m;
  typename CostDataContainer::iterator it_d, end_d;
  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
      end_d = data->costs.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<CostItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the cost name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->cost->calcDiff(d_i, x);
      data->Lx += m_i->weight * d_i->Lx;
      data->Lxx += m_i->weight * d_i->Lxx;
    }
  }
}

template <typename Scalar>
boost::shared_ptr<CostDataSumTpl<Scalar> > CostModelSumTpl<Scalar>::createData(
    DataCollectorAbstract* const data) {
  return boost::allocate_shared<CostDataSum>(
      Eigen::aligned_allocator<CostDataSum>(), this, data);
}

template <typename Scalar>
const boost::shared_ptr<StateAbstractTpl<Scalar> >&
CostModelSumTpl<Scalar>::get_state() const {
  return state_;
}

template <typename Scalar>
const typename CostModelSumTpl<Scalar>::CostModelContainer&
CostModelSumTpl<Scalar>::get_costs() const {
  return costs_;
}

template <typename Scalar>
std::size_t CostModelSumTpl<Scalar>::get_nu() const {
  return nu_;
}

template <typename Scalar>
std::size_t CostModelSumTpl<Scalar>::get_nr() const {
  return nr_;
}

template <typename Scalar>
std::size_t CostModelSumTpl<Scalar>::get_nr_total() const {
  return nr_total_;
}

template <typename Scalar>
const std::set<std::string>& CostModelSumTpl<Scalar>::get_active_set() const {
  return active_set_;
}

template <typename Scalar>
const std::set<std::string>& CostModelSumTpl<Scalar>::get_inactive_set() const {
  return inactive_set_;
}

template <typename Scalar>
bool CostModelSumTpl<Scalar>::getCostStatus(const std::string& name) const {
  typename CostModelContainer::const_iterator it = costs_.find(name);
  if (it != costs_.end()) {
    return it->second->active;
  } else {
    std::cerr << "Warning: we couldn't get the status of the " << name
              << " cost item, it doesn't exist." << std::endl;
    return false;
  }
}

template <typename Scalar>
std::ostream& operator<<(std::ostream& os,
                         const CostModelSumTpl<Scalar>& model) {
  const std::set<std::string>& active = model.get_active_set();
  const std::set<std::string>& inactive = model.get_inactive_set();
  os << "CostModelSum:" << std::endl;
  os << "  Active:" << std::endl;
  for (std::set<std::string>::const_iterator it = active.begin();
       it != active.end(); ++it) {
    const boost::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
        cost_item = model.get_costs().find(*it)->second;
    if (it != --active.end()) {
      os << "    " << *it << ": " << *cost_item << std::endl;
    } else {
      os << "    " << *it << ": " << *cost_item << std::endl;
    }
  }
  os << "  Inactive:" << std::endl;
  for (std::set<std::string>::const_iterator it = inactive.begin();
       it != inactive.end(); ++it) {
    const boost::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
        cost_item = model.get_costs().find(*it)->second;
    if (it != --inactive.end()) {
      os << "    " << *it << ": " << *cost_item << std::endl;
    } else {
      os << "    " << *it << ": " << *cost_item;
    }
  }
  return os;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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