Commit 1596ed0d authored by JaehyunShim's avatar JaehyunShim
Browse files

[impulse] Added active,inactive_set in impulse models

parent bcae46f4
Pipeline #17319 failed with stage
in 83 minutes and 26 seconds
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -118,14 +118,24 @@ void exposeImpulseMultiple() {
.add_property("ni_total",
bp::make_function(&ImpulseModelMultiple::get_nc_total, deprecated<>("Deprecated. Use nc_total.")),
"dimension of the total impulse vector")
.add_property("active",
bp::make_function(&ImpulseModelMultiple::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(&ImpulseModelMultiple::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",
bp::make_function(&ImpulseModelMultiple::get_active, bp::return_value_policy<bp::return_by_value>()),
"name of active impulse items")
"active_set",
bp::make_function(&ImpulseModelMultiple::get_active_set, bp::return_value_policy<bp::return_by_value>()),
"set of names of active contact items")
.add_property(
"inactive",
bp::make_function(&ImpulseModelMultiple::get_inactive, bp::return_value_policy<bp::return_by_value>()),
"name of inactive impulse items")
"inactive_set",
bp::make_function(&ImpulseModelMultiple::get_inactive_set, bp::return_value_policy<bp::return_by_value>()),
"set of names of inactive contact items")
.def("getImpulseStatus", &ImpulseModelMultiple::getImpulseStatus, bp::args("self", "name"),
"Return the impulse status of a given impulse name.\n\n"
":param name: impulse name")
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -11,6 +11,7 @@
#include <string>
#include <map>
#include <set>
#include <utility>
#include "crocoddyl/multibody/fwd.hpp"
......@@ -189,14 +190,34 @@ class ImpulseModelMultipleTpl {
DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const;)
/**
* @brief Return the names of the active impulses
* @brief Return the names of the set of active contacts
*/
const std::vector<std::string>& get_active() const;
const std::set<std::string>& get_active_set() const;
/**
* @brief Return the names of the inactive impulses
* @brief Return the names of the set of inactive contacts
*/
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 impulse name
......@@ -214,6 +235,11 @@ class ImpulseModelMultipleTpl {
ImpulseModelContainer impulses_;
std::size_t nc_;
std::size_t nc_total_;
std::set<std::string> active_set_;
std::set<std::string> inactive_set_;
// 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_;
};
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -28,14 +28,11 @@ void ImpulseModelMultipleTpl<Scalar>::addImpulse(const std::string& name,
} else if (active) {
nc_ += impulse->get_nc();
nc_total_ += impulse->get_nc();
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) {
nc_total_ += impulse->get_nc();
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);
}
}
......@@ -46,8 +43,8 @@ void ImpulseModelMultipleTpl<Scalar>::removeImpulse(const std::string& name) {
nc_ -= it->second->impulse->get_nc();
nc_total_ -= it->second->impulse->get_nc();
impulses_.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 << " impulse item, it doesn't exist." << std::endl;
}
......@@ -59,16 +56,12 @@ void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus(const std::string& nam
if (it != impulses_.end()) {
if (active && !it->second->active) {
nc_ += it->second->impulse->get_nc();
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);
} else if (!active && it->second->active) {
nc_ -= it->second->impulse->get_nc();
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);
inactive_set_.insert(name);
active_set_.erase(name);
}
it->second->active = active;
} else {
......@@ -263,13 +256,13 @@ std::size_t ImpulseModelMultipleTpl<Scalar>::get_ni_total() const {
}
template <typename Scalar>
const std::vector<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active() const {
return active_;
const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active_set() const {
return active_set_;
}
template <typename Scalar>
const std::vector<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive() const {
return inactive_;
const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive_set() const {
return inactive_set_;
}
template <typename Scalar>
......@@ -286,11 +279,11 @@ bool ImpulseModelMultipleTpl<Scalar>::getImpulseStatus(const std::string& name)
template <class Scalar>
std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model) {
const std::vector<std::string>& active = model.get_active();
const std::vector<std::string>& inactive = model.get_inactive();
const auto& active = model.get_active_set();
const auto& inactive = model.get_inactive_set();
os << "ImpulseModelMultiple:" << 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 ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
model.get_impulses().find(*it)->second;
if (it != --active.end()) {
......@@ -300,7 +293,7 @@ std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>
}
}
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 ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
model.get_impulses().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