BREAKING CHANGE: Add batch-wise cost estimation

demo/include/demo/pacman_agent.hpp

@@ -46,12 +46,10 @@ class PacmanAgent {
         moveRandomlyBehavior_ = std::make_shared<MoveRandomlyBehavior>(parameters_.moveRandomlyBehavior);
         stayInPlaceBehavior_ = std::make_shared<StayInPlaceBehavior>();
 
-        eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots", verifier_);
-        costEstimator_ = std::make_shared<CostEstimator>(parameters_.costEstimator);
-        eatDotsArbitrator_->addOption(
-            changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
-        eatDotsArbitrator_->addOption(
-            eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
+        CostEstimator::Ptr costEstimator = std::make_shared<CostEstimator>(parameters_.costEstimator);
+        eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots", costEstimator, verifier_);
+        eatDotsArbitrator_->addOption(changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable);
+        eatDotsArbitrator_->addOption(eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable);
 
         rootArbitrator_ = std::make_shared<PriorityArbitrator>("Pac-Man", verifier_);
         rootArbitrator_->addOption(chaseGhostBehavior_, PriorityArbitrator::Option::Flags::Interruptable);
@@ -98,7 +96,6 @@ class PacmanAgent {
     PriorityArbitrator::Ptr rootArbitrator_;
     CostArbitrator::Ptr eatDotsArbitrator_;
 
-    CostEstimator::Ptr costEstimator_;
     Verifier::Ptr verifier_;
 };
 

docs/tasks/4_cost_arbitration.md

@@ -34,9 +34,9 @@ Finish the implementation of the `CostEstimator` and replace the random arbitrat
 - Run the unit tests and note that some of the `CostEstimator` tests are failing
 - In `cost_estimator.cpp`, fill in the blanks to compute `nDots` and `nCells`.
 - Compile and run the unit tests for the `CostEstimator` to verify that your implementation is correct.
-- Add an instance of the `CostEstimator` to the `PacmanAgent` class and initialize it in the constructor.
+- Create an instance of the `CostEstimator` in the `PacmanAgent` constructor.
   Don't forget to include the necessary headers and extend the parameter struct with the parameters for the `CostEstimator`.
-- Replace the random arbitrator with a cost arbitrator in the `PacmanAgent` class. Pass the `CostEstimator` instance to the `addOption()` method.
+- Replace the random arbitrator with a cost arbitrator in the `PacmanAgent` class passing the `CostEstimator` instance to the constructor.
 
 ## Solution
 
@@ -86,12 +86,10 @@ To keep things tidy and consistent, add an alias definition analogous to the exi
 using CostArbitrator = arbitration_graphs::CostArbitrator<EnvironmentModel, Command>;
 ```
 
-Change the type of the `eatDotsArbitrator_` member in the `PacmanAgent` class to `CostArbitrator` and add an instance of the `CostEstimator`:
+Change the type of the `eatDotsArbitrator_` member in the `PacmanAgent` class to `CostArbitrator`:
 ```cpp
 private:
     CostArbitrator::Ptr eatDotsArbitrator_;
-
-    CostEstimator::Ptr costEstimator_;
 ```
 
 Extend the `Parameters` struct to contain the parameters for the `CostEstimator`:
@@ -107,7 +105,7 @@ struct Parameters {
 ```
 
 As always, the magic happens in the constructor of the `PacmanAgent` class.
-Instantiate the cost estimator and pass it in the `addOption` calls:
+Instantiate the cost estimator and pass it to the new cost arbitrator:
 ```cpp
 explicit PacmanAgent(const entt::Game& game) : parameters_{}, environmentModel_{game} {
     avoidGhostBehavior_ = std::make_shared<AvoidGhostBehavior>(parameters_.avoidGhostBehavior);
@@ -116,16 +114,13 @@ explicit PacmanAgent(const entt::Game& game) : parameters_{}, environmentModel_{
     eatClosestDotBehavior_ = std::make_shared<EatClosestDotBehavior>();
     moveRandomlyBehavior_ = std::make_shared<MoveRandomlyBehavior>(parameters_.moveRandomlyBehavior);
 
-    // This is now a cost arbitrator
-    eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots");
     // Construct the cost estimator
-    costEstimator_ = std::make_shared<CostEstimator>(parameters_.costEstimator);
-    // Add the ChangeDotCluster and EatClosestDot behavior components as options to the
-    // cost arbitrator while also passing the cost estimator
-    eatDotsArbitrator_->addOption(
-        changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
-    eatDotsArbitrator_->addOption(
-        eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
+    CostEstimator::Ptr costEstimator = std::make_shared<CostEstimator>(parameters_.costEstimator);
+    // This is now a cost arbitrator using the cost estimator
+    eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots", costEstimator);
+    // Add the ChangeDotCluster and EatClosestDot behavior components as options to the cost arbitrator
+    eatDotsArbitrator_->addOption(changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable);
+    eatDotsArbitrator_->addOption(eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable);
 
     rootArbitrator_ = std::make_shared<PriorityArbitrator>("Pac-Man");
     rootArbitrator_->addOption(chaseGhostBehavior_, PriorityArbitrator::Option::Flags::Interruptable);

docs/tasks/5_verification.md

@@ -116,12 +116,10 @@ explicit PacmanAgent(const entt::Game& game) : parameters_{}, environmentModel_{
     stayInPlaceBehavior_ = std::make_shared<StayInPlaceBehavior>();
 
     // Pass the verifier instance to the cost arbitrator
-    eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots", verifier_);
-    costEstimator_ = std::make_shared<CostEstimator>(parameters_.costEstimator);
-    eatDotsArbitrator_->addOption(
-        changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
-    eatDotsArbitrator_->addOption(
-        eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_);
+    CostEstimator::Ptr costEstimator = std::make_shared<CostEstimator>(parameters_.costEstimator);
+    eatDotsArbitrator_ = std::make_shared<CostArbitrator>("EatDots", costEstimator, verifier_);
+    eatDotsArbitrator_->addOption(changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable);
+    eatDotsArbitrator_->addOption(eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable);
 
     // Pass the verifier instance to the priority arbitrator
     rootArbitrator_ = std::make_shared<PriorityArbitrator>("Pac-Man", verifier_);

include/arbitration_graphs/cost_arbitrator.hpp

@@ -1,15 +1,28 @@
 #pragma once
 
 #include <memory>
+#include <numeric>
 #include <optional>
 
+#include <util_caching/cache.hpp>
 #include <yaml-cpp/yaml.h>
 
 #include "arbitrator.hpp"
+#include "exceptions.hpp"
+#include "types.hpp"
 
 
 namespace arbitration_graphs {
 
+/**
+ * \brief Interface for estimating the cost of a single command.
+ *
+ * A CostEstimator computes a scalar cost value for a single command given
+ * the current environment state and execution context.
+ *
+ * The CostArbitrator will use the cost estimates to sort the behavior options
+ * and select the one with the lowest cost.
+ */
 template <typename EnvironmentModelT, typename SubCommandT>
 struct CostEstimator {
     using Ptr = std::shared_ptr<CostEstimator>;
@@ -21,6 +34,84 @@ struct CostEstimator {
                                 bool isActive) = 0;
 };
 
+/**
+ * \brief Interface for estimating costs for multiple commands in a single batch.
+ *
+ * An alternative to the per-option CostEstimator for more advanced use cases.
+ * A BatchCostEstimator computes cost values for multiple commands at once.
+ * This interface enables implementations to exploit shared computation,
+ * vectorization, or global context across candidates.
+ *
+ * \note The returned cost vector must have the same order and size as the input candidates vector.
+ */
+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_;
+};
+
+/**
+ * \brief The PlaceboCostEstimator is a dummy estimator assigning monotonically increasing costs.
+ *
+ * This estimator assigns costs purely based on the candidate order:
+ * the first candidate gets cost 0.0, the second 1.0, and so on.
+ *
+ * As a result, the CostArbitrator effectively degrades into a
+ * priority-based arbitrator where earlier options always win over
+ * later ones, regardless of the command or environment state.
+ *
+ * \warning Users will very likely *not* want to rely on this default
+ *          in real applications. It is mainly provided to keep the CostArbitrator
+ *          constructor analogous to other arbitrators.
+ */
+template <typename EnvironmentModelT, typename SubCommandT>
+class PlaceboCostEstimator : public BatchCostEstimator<EnvironmentModelT, SubCommandT> {
+public:
+    using CandidateT = typename BatchCostEstimator<EnvironmentModelT, SubCommandT>::Candidate;
+
+    std::vector<double> estimateCosts(const Time& /*time*/,
+                                      const EnvironmentModelT& /*environmentModel*/,
+                                      const std::vector<CandidateT>& candidates) override {
+        std::vector<double> costs(candidates.size());
+        std::iota(costs.begin(), costs.end(), 0.0);
+        return costs;
+    }
+};
+
+
 template <typename EnvironmentModelT, typename CommandT, typename SubCommandT = CommandT>
 class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommandT> {
 public:
@@ -29,7 +120,10 @@ 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 PerOptionToBatchAdapterT = PerOptionToBatchAdapter<EnvironmentModelT, SubCommandT>;
     using PlaceboVerifierT = verification::PlaceboVerifier<EnvironmentModelT, SubCommandT>;
     using VerifierT = verification::Verifier<EnvironmentModelT, SubCommandT>;
 
@@ -41,22 +135,15 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand
 
         enum Flags { NoFlags = 0b0, Interruptable = 0b1, Fallback = 0b10 };
 
-        Option(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior,
-               const FlagsT& flags,
-               const typename CostEstimatorT::Ptr& costEstimator)
-                : ArbitratorBase::Option(behavior, flags), costEstimator_{costEstimator} {
+        Option(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior, const FlagsT& flags)
+                : ArbitratorBase::Option(behavior, flags) {
         }
 
-        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;
+        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);
         }
 
         /*!
@@ -88,23 +175,31 @@ 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_;
+        mutable util_caching::Cache<Time, double> lastEstimatedCost_;
     };
 
 
     explicit CostArbitrator(const std::string& name = "CostArbitrator",
+                            const typename BatchCostEstimatorT::Ptr& batchCostEstimator =
+                                std::make_shared<PlaceboCostEstimator<EnvironmentModelT, SubCommandT>>(),
                             typename VerifierT::Ptr verifier = std::make_shared<PlaceboVerifierT>())
-            : ArbitratorBase(name, verifier) {};
+            : ArbitratorBase(name, verifier), costEstimator_{batchCostEstimator} {};
+
+    explicit CostArbitrator(const std::string& name,
+                            const typename CostEstimatorT::Ptr& costEstimator,
+                            typename VerifierT::Ptr verifier = std::make_shared<PlaceboVerifierT>())
+            : ArbitratorBase(name, verifier),
+              costEstimator_(std::make_shared<PerOptionToBatchAdapterT>(costEstimator)) {
+    }
 
 
     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);
+                   const typename Option::FlagsT& flags) override {
+        typename Option::Ptr option = std::make_shared<Option>(behavior, flags);
         this->addOptionImpl(option);
     }
 
+
     /*!
      * \brief Returns a yaml representation of the arbitrator object with its current state
      *
@@ -124,17 +219,25 @@ 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();
+
+
+        std::vector<typename Option::Ptr> validOptions = collectValidOptions(options, time, environmentModel);
+
+        if (validOptions.size() < 2) {
+            // no need to estimate costs if there is nothing to sort
+            return typename ArbitratorBase::Options(validOptions.begin(), validOptions.end());
         }
 
-        // sort given options by using a multiset
-        std::multimap<double, typename ArbitratorBase::Option::Ptr> sortedOptionsMap;
+        return sortOptionsByCost(validOptions, time, environmentModel);
+    }
 
+    std::vector<typename Option::Ptr> collectValidOptions(const typename ArbitratorBase::Options& options,
+                                                          const Time& time,
+                                                          const EnvironmentModelT& environmentModel) const {
+
+        std::vector<typename Option::Ptr> validOptions;
         for (auto& optionBase : options) {
-            typename Option::Ptr option = std::dynamic_pointer_cast<Option>(optionBase);
+            auto option = std::dynamic_pointer_cast<Option>(optionBase);
 
             const bool isActive = this->isActive(option);
 
@@ -146,22 +249,52 @@ class CostArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommand
                 command = this->getAndVerifyCommand(option, time, environmentModel);
                 option->behavior()->loseControl(time, environmentModel);
             }
+            if (command) {
+                validOptions.push_back(option);
+            }
+        }
+        return validOptions;
+    }
+
+    typename ArbitratorBase::Options sortOptionsByCost(const std::vector<typename Option::Ptr>& options,
+                                                       const Time& time,
+                                                       const EnvironmentModelT& environmentModel) const {
+        std::vector<CandidateT> candidates;
+        candidates.reserve(options.size());
+        for (const auto& option : options) {
+            // The command has already been computed (and verified), so we can safely retrieve it from cache
+            const std::optional<SubCommandT> command = option->getCommand(time, environmentModel);
             if (!command) {
-                continue;
+                throw InvalidStateError("Could not retrieve cached command.");
             }
+            const bool isActive = this->isActive(option);
+            candidates.push_back(CandidateT{command.value(), isActive});
+        }
 
-            double cost = option->estimateCost(time, environmentModel, command.value(), isActive);
-            sortedOptionsMap.insert({cost, option});
+        std::vector<double> costs = costEstimator_->estimateCosts(time, environmentModel, candidates);
+        if (costs.size() != candidates.size()) {
+            throw InvalidCostError("CostEstimator returned mismatching number of costs.");
         }
 
-        // copy back to vector (these are pointers anyway, so copying is cheap)
+        // Sort options by cost
+        std::multimap<double, typename ArbitratorBase::Option::Ptr> sortedOptionsMap;
+        for (std::size_t i = 0; i < options.size(); ++i) {
+            options[i]->cacheLastEstimatedCost(time, costs[i]);
+            sortedOptionsMap.insert({costs[i], options[i]});
+        }
+
+        // Copy back to vector
         typename ArbitratorBase::Options sortedOptionsVector;
         sortedOptionsVector.reserve(options.size());
         for (const auto& sortedOption : sortedOptionsMap) {
             sortedOptionsVector.push_back(sortedOption.second);
         }
+
         return sortedOptionsVector;
     }
+
+
+    typename BatchCostEstimatorT::Ptr costEstimator_;
 };
 } // namespace arbitration_graphs
 

include/arbitration_graphs/exceptions.hpp

@@ -20,6 +20,14 @@ 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 InvalidStateError : public std::runtime_error {
+    using std::runtime_error::runtime_error;
+};
+
 class VerificationError : public std::runtime_error {
     using std::runtime_error::runtime_error;
 };