Commit a36f0de8 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[deprecated] Used std::set for active/inactive in cost sum

parent e30a9492
Pipeline #20340 passed with stage
in 68 minutes and 33 seconds
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -9,14 +10,13 @@
#include <functional>
#include <map>
#include <memory>
#include <utility>
#include <string>
#include "python/crocoddyl/core/core.hpp"
#include "python/crocoddyl/core/action-base.hpp"
#include "python/crocoddyl/core/diff-action-base.hpp"
#include "python/crocoddyl/utils/map-converter.hpp"
#include "python/crocoddyl/utils/printable.hpp"
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "python/crocoddyl/utils/deprecate.hpp"
namespace crocoddyl {
namespace python {
......@@ -28,14 +28,14 @@ void exposeCostSum() {
typedef boost::shared_ptr<CostItem> CostItemPtr;
typedef boost::shared_ptr<CostDataAbstract> CostDataPtr;
StdMapPythonVisitor<std::string, CostItemPtr, std::less<std::string>,
std::allocator<std::pair<const std::string, CostItemPtr> >, true>::expose("StdMap_CostItem");
std::allocator<std::pair<const std::string, CostItemPtr>>, true>::expose("StdMap_CostItem");
StdMapPythonVisitor<std::string, CostDataPtr, std::less<std::string>,
std::allocator<std::pair<const std::string, CostDataPtr> >, true>::expose("StdMap_CostData");
std::allocator<std::pair<const std::string, CostDataPtr>>, true>::expose("StdMap_CostData");
bp::register_ptr_to_python<boost::shared_ptr<CostItem> >();
bp::register_ptr_to_python<boost::shared_ptr<CostItem>>();
bp::class_<CostItem>("CostItem", "Describe a cost item.\n\n",
bp::init<std::string, boost::shared_ptr<CostModelAbstract>, double, bp::optional<bool> >(
bp::init<std::string, boost::shared_ptr<CostModelAbstract>, double, bp::optional<bool>>(
bp::args("self", "name", "cost", "weight", "active"),
"Initialize the cost item.\n\n"
":param name: cost name\n"
......@@ -49,7 +49,7 @@ void exposeCostSum() {
.def_readwrite("active", &CostItem::active, "cost status")
.def(PrintableVisitor<CostItem>());
bp::register_ptr_to_python<boost::shared_ptr<CostModelSum> >();
bp::register_ptr_to_python<boost::shared_ptr<CostModelSum>>();
bp::class_<CostModelSum>("CostModelSum", bp::init<boost::shared_ptr<StateAbstract>, std::size_t>(
bp::args("self", "state", "nu"),
......@@ -62,10 +62,10 @@ void exposeCostSum() {
"For this case the default nu is equals to model.nv.\n"
":param state: state description\n"
":param nu: dimension of control vector"))
.def(bp::init<boost::shared_ptr<StateAbstract> >(bp::args("self", "state"),
"Initialize the total cost model.\n\n"
"For this case the default nu is equals to model.nv.\n"
":param state: state description"))
.def(bp::init<boost::shared_ptr<StateAbstract>>(bp::args("self", "state"),
"Initialize the total cost model.\n\n"
"For this case the default nu is equals to model.nv.\n"
":param state: state description"))
.def("addCost", &CostModelSum::addCost,
CostModelSum_addCost_wrap(bp::args("self", "name", "cost", "weight", "active"),
"Add a cost item.\n\n"
......@@ -124,18 +124,28 @@ void exposeCostSum() {
.add_property("nr", bp::make_function(&CostModelSum::get_nr), "dimension of the residual vector of active cost")
.add_property("nr_total", bp::make_function(&CostModelSum::get_nr_total),
"dimension of the total residual vector")
.add_property("active",
bp::make_function(&CostModelSum::get_active, bp::return_value_policy<bp::return_by_value>()),
"name of active cost items")
.add_property("inactive",
bp::make_function(&CostModelSum::get_inactive, bp::return_value_policy<bp::return_by_value>()),
"name of inactive cost items")
.add_property(
"active",
bp::make_function(&CostModelSum::get_active, deprecated<bp::return_value_policy<bp::return_by_value>>(
"Deprecated. Use property active_set")),
"list of names of active contact items")
.add_property(
"inactive",
bp::make_function(&CostModelSum::get_inactive, deprecated<bp::return_value_policy<bp::return_by_value>>(
"Deprecated. Use property inactive_set")),
"list of names of inactive contact items")
.add_property("active_set",
bp::make_function(&CostModelSum::get_active_set, bp::return_value_policy<bp::return_by_value>()),
"name of the active set of cost items")
.add_property("inactive_set",
bp::make_function(&CostModelSum::get_inactive_set, bp::return_value_policy<bp::return_by_value>()),
"name of the inactive set of cost items")
.def("getCostStatus", &CostModelSum::getCostStatus, bp::args("self", "name"),
"Return the cost status of a given cost name.\n\n"
":param name: cost name")
.def(PrintableVisitor<CostModelSum>());
bp::register_ptr_to_python<boost::shared_ptr<CostDataSum> >();
bp::register_ptr_to_python<boost::shared_ptr<CostDataSum>>();
bp::class_<CostDataSum>(
"CostDataSum", "Class for total cost data.\n\n",
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -11,6 +12,7 @@
#include <string>
#include <map>
#include <set>
#include <utility>
#include "crocoddyl/core/fwd.hpp"
......@@ -205,14 +207,34 @@ class CostModelSumTpl {
std::size_t get_nr_total() const;
/**
* @brief Return the names of the active costs
* @brief Return the names of the set of active costs
*/
const std::vector<std::string>& get_active() const;
const std::set<std::string>& get_active_set() const;
/**
* @brief Return the names of the inactive costs
* @brief Return the names of the set of inactive costs
*/
const std::vector<std::string>& get_inactive() const;
const std::set<std::string>& get_inactive_set() const;
DEPRECATED("get_active() is deprecated and will be replaced with get_active_set()",
const std::vector<std::string>& get_active() {
active_.clear();
active_.reserve(active_set_.size());
for (const auto& contact : active_set_) {
active_.push_back(contact);
}
return active_;
};)
DEPRECATED("get_inactive() is deprecated and will be replaced with get_inactive_set()",
const std::vector<std::string>& get_inactive() {
inactive_.clear();
inactive_.reserve(inactive_set_.size());
for (const auto& contact : inactive_set_) {
inactive_.push_back(contact);
}
return inactive_;
};)
/**
* @brief Return the status of a given cost name
......@@ -233,8 +255,13 @@ class CostModelSumTpl {
std::size_t nu_; //!< Dimension of the control input
std::size_t nr_; //!< Dimension of the active residual vector
std::size_t nr_total_; //!< Dimension of the total residual vector
std::vector<std::string> active_; //!< Names of the active cost items
std::vector<std::string> inactive_; //!< Names of the inactive cost items
std::set<std::string> active_set_; //!< Names of the active set of cost items
std::set<std::string> inactive_set_; //!< Names of the inactive set of cost items
// Vector variants. These are to maintain the API compatibility for the deprecated syntax.
// These will be removed in future versions along with get_active() / get_inactive()
std::vector<std::string> active_;
std::vector<std::string> inactive_;
};
template <typename _Scalar>
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -36,14 +37,10 @@ void CostModelSumTpl<Scalar>::addCost(const std::string& name, boost::shared_ptr
} else if (active) {
nr_ += cost->get_activation()->get_nr();
nr_total_ += cost->get_activation()->get_nr();
std::vector<std::string>::iterator it =
std::lower_bound(active_.begin(), active_.end(), name, std::less<std::string>());
active_.insert(it, name);
active_set_.insert(name);
} else if (!active) {
nr_total_ += cost->get_activation()->get_nr();
std::vector<std::string>::iterator it =
std::lower_bound(inactive_.begin(), inactive_.end(), name, std::less<std::string>());
inactive_.insert(it, name);
inactive_set_.insert(name);
}
}
......@@ -54,8 +51,8 @@ void CostModelSumTpl<Scalar>::removeCost(const std::string& name) {
nr_ -= it->second->cost->get_activation()->get_nr();
nr_total_ -= it->second->cost->get_activation()->get_nr();
costs_.erase(it);
active_.erase(std::remove(active_.begin(), active_.end(), name), active_.end());
inactive_.erase(std::remove(inactive_.begin(), inactive_.end(), name), inactive_.end());
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;
}
......@@ -67,18 +64,15 @@ void CostModelSumTpl<Scalar>::changeCostStatus(const std::string& name, const bo
if (it != costs_.end()) {
if (active && !it->second->active) {
nr_ += it->second->cost->get_activation()->get_nr();
std::vector<std::string>::iterator it =
std::lower_bound(active_.begin(), active_.end(), name, std::less<std::string>());
active_.insert(it, name);
inactive_.erase(std::remove(inactive_.begin(), inactive_.end(), name), inactive_.end());
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_.erase(std::remove(active_.begin(), active_.end(), name), active_.end());
std::vector<std::string>::iterator it =
std::lower_bound(inactive_.begin(), inactive_.end(), name, std::less<std::string>());
inactive_.insert(it, name);
active_set_.erase(name);
inactive_set_.insert(name);
it->second->active = active;
}
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;
......@@ -249,13 +243,13 @@ std::size_t CostModelSumTpl<Scalar>::get_nr_total() const {
}
template <typename Scalar>
const std::vector<std::string>& CostModelSumTpl<Scalar>::get_active() const {
return active_;
const std::set<std::string>& CostModelSumTpl<Scalar>::get_active_set() const {
return active_set_;
}
template <typename Scalar>
const std::vector<std::string>& CostModelSumTpl<Scalar>::get_inactive() const {
return inactive_;
const std::set<std::string>& CostModelSumTpl<Scalar>::get_inactive_set() const {
return inactive_set_;
}
template <typename Scalar>
......@@ -271,11 +265,11 @@ bool CostModelSumTpl<Scalar>::getCostStatus(const std::string& name) const {
template <typename Scalar>
std::ostream& operator<<(std::ostream& os, const CostModelSumTpl<Scalar>& model) {
const std::vector<std::string>& active = model.get_active();
const std::vector<std::string>& inactive = model.get_inactive();
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::vector<std::string>::const_iterator it = active.begin(); it != active.end(); ++it) {
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()) {
......@@ -285,7 +279,7 @@ std::ostream& operator<<(std::ostream& os, const CostModelSumTpl<Scalar>& model)
}
}
os << " Inactive:" << std::endl;
for (std::vector<std::string>::const_iterator it = inactive.begin(); it != inactive.end(); ++it) {
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()) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment