Skip to content

Add batch-wise cost estimation #151

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
ll-nick wants to merge 1 commit into from
Closed

Add batch-wise cost estimation #151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
144 changes: 112 additions & 32 deletions include/arbitration_graphs/cost_arbitrator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,12 @@
#include <memory>
#include <optional>

#include <util_caching/cache.hpp>
#include <yaml-cpp/yaml.h>

#include "arbitrator.hpp"
#include "exceptions.hpp"
#include "types.hpp"


namespace arbitration_graphs {
Expand All @@ -21,6 +24,45 @@ struct CostEstimator {
bool isActive) = 0;
};

template <typename EnvironmentModelT, typename SubCommandT>
struct BatchCostEstimator {
using Ptr = std::shared_ptr<BatchCostEstimator>;
using ConstPtr = std::shared_ptr<const BatchCostEstimator>;

struct Candidate {
SubCommandT command;
bool isActive;
};

virtual std::vector<double> estimateCosts(const Time& time,
const EnvironmentModelT& environmentModel,
const std::vector<Candidate>& candidates) = 0;
};

template <typename EnvironmentModelT, typename SubCommandT>
class PerOptionToBatchAdapter : public BatchCostEstimator<EnvironmentModelT, SubCommandT> {
public:
using CandidateT = typename BatchCostEstimator<EnvironmentModelT, SubCommandT>::Candidate;
using CostEstimatorT = CostEstimator<EnvironmentModelT, SubCommandT>;
explicit PerOptionToBatchAdapter(typename CostEstimatorT::Ptr perOptionEstimator)
: perOptionEstimator_(std::move(perOptionEstimator)) {
}
std::vector<double> estimateCosts(const Time& time,
const EnvironmentModelT& environmentModel,
const std::vector<CandidateT>& candidates) override {
std::vector<double> costs;
costs.reserve(candidates.size());
for (const auto& candidate : candidates) {
costs.push_back(
perOptionEstimator_->estimateCost(time, environmentModel, candidate.command, candidate.isActive));
}
return costs;
}

private:
typename CostEstimatorT::Ptr perOptionEstimator_;
};

template <typename EnvironmentModelT, typename CommandT, typename SubCommandT = CommandT>
class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommandT> {
public:
Expand All @@ -29,6 +71,8 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand
using Ptr = std::shared_ptr<CostArbitrator>;
using ConstPtr = std::shared_ptr<const CostArbitrator>;

using BatchCostEstimatorT = BatchCostEstimator<EnvironmentModelT, SubCommandT>;
using CandidateT = typename BatchCostEstimatorT::Candidate;
using CostEstimatorT = CostEstimator<EnvironmentModelT, SubCommandT>;
using PlaceboVerifierT = verification::PlaceboVerifier<EnvironmentModelT, SubCommandT>;
using VerifierT = verification::Verifier<EnvironmentModelT, SubCommandT>;
Expand All @@ -43,20 +87,19 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand

Option(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior,
const FlagsT& flags,
const typename CostEstimatorT::Ptr& costEstimator)
const typename BatchCostEstimatorT::Ptr& costEstimator)
: ArbitratorBase::Option(behavior, flags), costEstimator_{costEstimator} {
}

double estimateCost(const Time& time,
const EnvironmentModelT& environmentModel,
const SubCommandT& command,
bool isActive) const {
double cost = costEstimator_->estimateCost(time, environmentModel, command, isActive);
lastEstimatedCost_ = cost;
return cost;
typename BatchCostEstimatorT::Ptr costEstimator() const {
return costEstimator_;
}

std::optional<double> lastEstimatedCost(const Time& time) const {
return lastEstimatedCost_.cached(time);
}
void resetLastEstimatedCost() const {
lastEstimatedCost_.reset();
void cacheLastEstimatedCost(const Time& time, const double& cost) const {
lastEstimatedCost_.cache(time, cost);
}

/*!
Expand Down Expand Up @@ -88,21 +131,29 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand
YAML::Node toYaml(const Time& time, const EnvironmentModelT& environmentModel) const override;

private:
typename CostEstimatorT::Ptr costEstimator_;
mutable std::optional<double> lastEstimatedCost_;
typename BatchCostEstimatorT::Ptr costEstimator_;
mutable util_caching::Cache<Time, double> lastEstimatedCost_;
};


explicit CostArbitrator(const std::string& name = "CostArbitrator",
typename VerifierT::Ptr verifier = std::make_shared<PlaceboVerifierT>())
: ArbitratorBase(name, verifier) {};

void addOption(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior,
const typename Option::FlagsT& flags,
const typename BatchCostEstimatorT::Ptr& batchCostEstimator) {
typename Option::Ptr option = std::make_shared<Option>(behavior, flags, batchCostEstimator);
this->addOptionImpl(option);
}


void addOption(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior,
const typename Option::FlagsT& flags,
const typename CostEstimatorT::Ptr& costEstimator) {
typename Option::Ptr option = std::make_shared<Option>(behavior, flags, costEstimator);
this->addOptionImpl(option);
typename BatchCostEstimatorT::Ptr batchEstimator =
std::make_shared<PerOptionToBatchAdapter<EnvironmentModelT, SubCommandT>>(costEstimator);
addOption(behavior, flags, batchEstimator);
}

/*!
Expand All @@ -124,34 +175,63 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand
const typename ArbitratorBase::Options& options,
const Time& time,
const EnvironmentModelT& environmentModel) const override {
// reset lastEstimatedCost for all behaviorOptions
for (const auto& optionBase : this->options()) {
typename Option::ConstPtr option = std::dynamic_pointer_cast<const Option>(optionBase);
option->resetLastEstimatedCost();
}

// sort given options by using a multiset
std::multimap<double, typename ArbitratorBase::Option::Ptr> sortedOptionsMap;
using CandidateT = typename BatchCostEstimatorT::Candidate;

std::unordered_map<typename BatchCostEstimatorT::Ptr, std::vector<typename Option::Ptr>> optionsByEstimator;
for (auto& optionBase : options) {
typename Option::Ptr option = std::dynamic_pointer_cast<Option>(optionBase);
optionsByEstimator[option->costEstimator()].push_back(option);
}

const bool isActive = this->isActive(option);
std::multimap<double, typename ArbitratorBase::Option::Ptr> sortedOptionsMap;

std::optional<SubCommandT> command;
if (isActive) {
command = this->getAndVerifyCommand(option, time, environmentModel);
} else {
option->behavior()->gainControl(time, environmentModel);
command = this->getAndVerifyCommand(option, time, environmentModel);
option->behavior()->loseControl(time, environmentModel);
for (const auto& group : optionsByEstimator) {
auto estimator = group.first;
auto& groupedOptions = group.second;

std::vector<typename Option::Ptr> validOptions;
for (auto& option : groupedOptions) {
const bool isActive = this->isActive(option);

std::optional<SubCommandT> command;
if (isActive) {
command = this->getAndVerifyCommand(option, time, environmentModel);
} else {
option->behavior()->gainControl(time, environmentModel);
command = this->getAndVerifyCommand(option, time, environmentModel);
option->behavior()->loseControl(time, environmentModel);
}
if (!command) {
continue;
}

validOptions.push_back(option);
}
if (!command) {

if (validOptions.empty()) {
continue;
}

double cost = option->estimateCost(time, environmentModel, command.value(), isActive);
sortedOptionsMap.insert({cost, option});
std::vector<CandidateT> candidates;
candidates.reserve(validOptions.size());
for (const auto& option : validOptions) {
const bool isActive = this->isActive(option);
const SubCommandT command = option->getCommand(time, environmentModel);
candidates.push_back(CandidateT{command, isActive});
}

const std::vector<double> estimatedCosts = estimator->estimateCosts(time, environmentModel, candidates);

if (estimatedCosts.size() != validOptions.size()) {
throw InvalidCostError("CostEstimator returned invalid number of costs!");
}

for (std::size_t i = 0; i < validOptions.size(); i++) {
const double cost = estimatedCosts[i];
validOptions[i]->cacheLastEstimatedCost(time, cost);
sortedOptionsMap.insert({cost, validOptions[i]});
}
}

// copy back to vector (these are pointers anyway, so copying is cheap)
Expand Down
4 changes: 4 additions & 0 deletions include/arbitration_graphs/exceptions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,10 @@ class InvalidArgumentsError : public std::runtime_error {
using std::runtime_error::runtime_error;
};

class InvalidCostError : public std::runtime_error {
using std::runtime_error::runtime_error;
};

class VerificationError : public std::runtime_error {
using std::runtime_error::runtime_error;
};
Expand Down
8 changes: 4 additions & 4 deletions include/arbitration_graphs/internal/cost_arbitrator_io.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ std::ostream& CostArbitrator<EnvironmentModelT, CommandT, SubCommandT>::Option::
const std::string& prefix,
const std::string& suffix) const {

if (lastEstimatedCost_) {
output << std::fixed << std::setprecision(3) << "- (cost: " << *lastEstimatedCost_ << ") ";
if (lastEstimatedCost(time)) {
output << std::fixed << std::setprecision(3) << "- (cost: " << *lastEstimatedCost(time) << ") ";
} else {
output << "- (cost: n.a.) ";
}
Expand All @@ -34,8 +34,8 @@ template <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
YAML::Node CostArbitrator<EnvironmentModelT, CommandT, SubCommandT>::Option::toYaml(
const Time& time, const EnvironmentModelT& environmentModel) const {
YAML::Node node = ArbitratorBase::Option::toYaml(time, environmentModel);
if (lastEstimatedCost_) {
node["cost"] = *lastEstimatedCost_;
if (lastEstimatedCost(time)) {
node["cost"] = *lastEstimatedCost(time);
}
return node;
}
Expand Down
Loading