diff --git a/demo/include/demo/pacman_agent.hpp b/demo/include/demo/pacman_agent.hpp index 2c083c25..2524e3e5 100644 --- a/demo/include/demo/pacman_agent.hpp +++ b/demo/include/demo/pacman_agent.hpp @@ -46,12 +46,10 @@ class PacmanAgent { moveRandomlyBehavior_ = std::make_shared(parameters_.moveRandomlyBehavior); stayInPlaceBehavior_ = std::make_shared(); - eatDotsArbitrator_ = std::make_shared("EatDots", verifier_); - costEstimator_ = std::make_shared(parameters_.costEstimator); - eatDotsArbitrator_->addOption( - changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_); - eatDotsArbitrator_->addOption( - eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_); + CostEstimator::Ptr costEstimator = std::make_shared(parameters_.costEstimator); + eatDotsArbitrator_ = std::make_shared("EatDots", costEstimator, verifier_); + eatDotsArbitrator_->addOption(changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable); + eatDotsArbitrator_->addOption(eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable); rootArbitrator_ = std::make_shared("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_; }; diff --git a/docs/tasks/4_cost_arbitration.md b/docs/tasks/4_cost_arbitration.md index 5704f618..114d4ce2 100644 --- a/docs/tasks/4_cost_arbitration.md +++ b/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; ``` -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(parameters_.avoidGhostBehavior); @@ -116,16 +114,13 @@ explicit PacmanAgent(const entt::Game& game) : parameters_{}, environmentModel_{ eatClosestDotBehavior_ = std::make_shared(); moveRandomlyBehavior_ = std::make_shared(parameters_.moveRandomlyBehavior); - // This is now a cost arbitrator - eatDotsArbitrator_ = std::make_shared("EatDots"); // Construct the cost estimator - costEstimator_ = std::make_shared(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(parameters_.costEstimator); + // This is now a cost arbitrator using the cost estimator + eatDotsArbitrator_ = std::make_shared("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("Pac-Man"); rootArbitrator_->addOption(chaseGhostBehavior_, PriorityArbitrator::Option::Flags::Interruptable); diff --git a/docs/tasks/5_verification.md b/docs/tasks/5_verification.md index a79ea98b..9b2a08f1 100644 --- a/docs/tasks/5_verification.md +++ b/docs/tasks/5_verification.md @@ -116,12 +116,10 @@ explicit PacmanAgent(const entt::Game& game) : parameters_{}, environmentModel_{ stayInPlaceBehavior_ = std::make_shared(); // Pass the verifier instance to the cost arbitrator - eatDotsArbitrator_ = std::make_shared("EatDots", verifier_); - costEstimator_ = std::make_shared(parameters_.costEstimator); - eatDotsArbitrator_->addOption( - changeDotClusterBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_); - eatDotsArbitrator_->addOption( - eatClosestDotBehavior_, CostArbitrator::Option::Flags::Interruptable, costEstimator_); + CostEstimator::Ptr costEstimator = std::make_shared(parameters_.costEstimator); + eatDotsArbitrator_ = std::make_shared("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("Pac-Man", verifier_); diff --git a/include/arbitration_graphs/cost_arbitrator.hpp b/include/arbitration_graphs/cost_arbitrator.hpp index c182c5d4..6bee6e3b 100644 --- a/include/arbitration_graphs/cost_arbitrator.hpp +++ b/include/arbitration_graphs/cost_arbitrator.hpp @@ -1,15 +1,28 @@ #pragma once #include +#include #include +#include #include #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 struct CostEstimator { using Ptr = std::shared_ptr; @@ -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 +struct BatchCostEstimator { + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + struct Candidate { + SubCommandT command; + bool isActive; + }; + + virtual std::vector estimateCosts(const Time& time, + const EnvironmentModelT& environmentModel, + const std::vector& candidates) = 0; +}; + +template +class PerOptionToBatchAdapter : public BatchCostEstimator { +public: + using CandidateT = typename BatchCostEstimator::Candidate; + using CostEstimatorT = CostEstimator; + explicit PerOptionToBatchAdapter(typename CostEstimatorT::Ptr perOptionEstimator) + : perOptionEstimator_(std::move(perOptionEstimator)) { + } + std::vector estimateCosts(const Time& time, + const EnvironmentModelT& environmentModel, + const std::vector& candidates) override { + std::vector 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 +class PlaceboCostEstimator : public BatchCostEstimator { +public: + using CandidateT = typename BatchCostEstimator::Candidate; + + std::vector estimateCosts(const Time& /*time*/, + const EnvironmentModelT& /*environmentModel*/, + const std::vector& candidates) override { + std::vector costs(candidates.size()); + std::iota(costs.begin(), costs.end(), 0.0); + return costs; + } +}; + + template class CostArbitrator : public Arbitrator { public: @@ -29,7 +120,10 @@ class CostArbitrator : public Arbitrator; using ConstPtr = std::shared_ptr; + using BatchCostEstimatorT = BatchCostEstimator; + using CandidateT = typename BatchCostEstimatorT::Candidate; using CostEstimatorT = CostEstimator; + using PerOptionToBatchAdapterT = PerOptionToBatchAdapter; using PlaceboVerifierT = verification::PlaceboVerifier; using VerifierT = verification::Verifier; @@ -41,22 +135,15 @@ class CostArbitrator : public Arbitrator::Ptr& behavior, - const FlagsT& flags, - const typename CostEstimatorT::Ptr& costEstimator) - : ArbitratorBase::Option(behavior, flags), costEstimator_{costEstimator} { + Option(const typename Behavior::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 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 lastEstimatedCost_; + mutable util_caching::Cache lastEstimatedCost_; }; explicit CostArbitrator(const std::string& name = "CostArbitrator", + const typename BatchCostEstimatorT::Ptr& batchCostEstimator = + std::make_shared>(), typename VerifierT::Ptr verifier = std::make_shared()) - : 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()) + : ArbitratorBase(name, verifier), + costEstimator_(std::make_shared(costEstimator)) { + } void addOption(const typename Behavior::Ptr& behavior, - const typename Option::FlagsT& flags, - const typename CostEstimatorT::Ptr& costEstimator) { - typename Option::Ptr option = std::make_shared