From 34d0adb6657c7a1b23f09e0bacfc296059a7214e Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Sat, 21 Feb 2026 12:57:32 +0000
Subject: [PATCH 1/3] Initial plan


From 7257ce332e6efcbd4a770bb8e322738fffe20d93 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Sat, 21 Feb 2026 13:10:32 +0000
Subject: [PATCH 2/3] Add sequence arbitrator implementation, tests, and Python
 bindings

Co-authored-by: orzechow <5220714+orzechow@users.noreply.github.com>
---
 _codeql_detected_source_root                  |   1 +
 .../internal/sequence_arbitrator_io.hpp       |  71 ++++++
 .../sequence_arbitrator.hpp                   | 203 +++++++++++++++
 .../sequence_arbitrator.hpp                   |  58 +++++
 python_bindings/src/bindings.cpp              |   2 +
 test/sequence_arbitrator.cpp                  | 236 ++++++++++++++++++
 6 files changed, 571 insertions(+)
 create mode 120000 _codeql_detected_source_root
 create mode 100644 include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
 create mode 100644 include/arbitration_graphs/sequence_arbitrator.hpp
 create mode 100644 python_bindings/include/arbitration_graphs_py/sequence_arbitrator.hpp
 create mode 100644 test/sequence_arbitrator.cpp

diff --git a/_codeql_detected_source_root b/_codeql_detected_source_root
new file mode 120000
index 00000000..945c9b46
--- /dev/null
+++ b/_codeql_detected_source_root
@@ -0,0 +1 @@
+.
\ No newline at end of file
diff --git a/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
new file mode 100644
index 00000000..915f2b0d
--- /dev/null
+++ b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
@@ -0,0 +1,71 @@
+#pragma once
+
+#include "../sequence_arbitrator.hpp"
+
+
+namespace arbitration_graphs {
+
+
+///////////////////////////////////////
+//    SequenceArbitrator::Option    //
+///////////////////////////////////////
+
+template <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+std::ostream& SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::Option::toStream(
+    std::ostream& output,
+    const Time& time,
+    const EnvironmentModelT& environmentModel,
+    const int& optionIndex,
+    const std::string& prefix,
+    const std::string& suffix) const {
+    output << optionIndex + 1 << ". ";
+    ArbitratorBase::Option::toStream(output, time, environmentModel, optionIndex, prefix, suffix);
+    return output;
+}
+
+
+///////////////////////////////////////
+//        SequenceArbitrator        //
+///////////////////////////////////////
+
+template <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+std::ostream& SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toStream(
+    std::ostream& output,
+    const Time& time,
+    const EnvironmentModelT& environmentModel,
+    const std::string& prefix,
+    const std::string& suffix) const {
+
+    Behavior<EnvironmentModelT, CommandT>::toStream(output, time, environmentModel, prefix, suffix);
+
+    for (int i = 0; i < static_cast<int>(this->options().size()); ++i) {
+        const typename ArbitratorBase::Option::ConstPtr option = this->options().at(i);
+        const bool isCurrent = sequenceStarted_ && (i == currentIndex_);
+
+        if (isCurrent) {
+            output << suffix << '\n' << prefix << " -> ";
+        } else {
+            output << suffix << '\n' << prefix << "    ";
+        }
+        option->toStream(output, time, environmentModel, i, "    " + prefix, suffix);
+    }
+    return output;
+}
+
+template <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+YAML::Node SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toYaml(
+    const Time& time, const EnvironmentModelT& environmentModel) const {
+    YAML::Node node = Behavior<EnvironmentModelT, CommandT>::toYaml(time, environmentModel);
+
+    node["type"] = "SequenceArbitrator";
+    for (const typename ArbitratorBase::Option::ConstPtr& option : this->options()) {
+        node["options"].push_back(option->toYaml(time, environmentModel));
+    }
+    if (sequenceStarted_) {
+        node["activeBehavior"] = currentIndex_;
+    }
+
+    return node;
+}
+
+} // namespace arbitration_graphs
diff --git a/include/arbitration_graphs/sequence_arbitrator.hpp b/include/arbitration_graphs/sequence_arbitrator.hpp
new file mode 100644
index 00000000..413cb975
--- /dev/null
+++ b/include/arbitration_graphs/sequence_arbitrator.hpp
@@ -0,0 +1,203 @@
+#pragma once
+
+#include <memory>
+
+#include <yaml-cpp/yaml.h>
+
+#include "arbitrator.hpp"
+#include "exceptions.hpp"
+
+
+namespace arbitration_graphs {
+
+/*!
+ * \brief A sequence arbitrator that executes its sub-behaviors in a fixed order.
+ *
+ * Based on the definition by Lauer et al. (2010):
+ * - Invocation condition: The invocation condition of the first sub-behavior is true.
+ * - Commitment condition: The sequence has started and has not yet completed.
+ * - Algorithm: Executes sub-behaviors in the order they were added. Advances to the next
+ *              sub-behavior once the current sub-behavior's commitment condition becomes false.
+ *              The sequence is complete when the last sub-behavior's commitment condition
+ *              becomes false.
+ *
+ * \see Martin Lauer, Roland Hafner, Sascha Lange, and Martin Riedmiller,
+ *      "Cognitive concepts in autonomous soccer playing robots,"
+ *      Cognitive Systems Research, vol. 11, no. 3, pp. 287–309, 2010,
+ *      doi: https://doi.org/10.1016/j.cogsys.2009.12.003
+ */
+template <typename EnvironmentModelT, typename CommandT, typename SubCommandT = CommandT>
+class SequenceArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommandT> {
+public:
+    using ArbitratorBase = Arbitrator<EnvironmentModelT, CommandT, SubCommandT>;
+
+    using Ptr = std::shared_ptr<SequenceArbitrator>;
+    using ConstPtr = std::shared_ptr<const SequenceArbitrator>;
+
+    using PlaceboVerifierT = verification::PlaceboVerifier<EnvironmentModelT, SubCommandT>;
+    using VerifierT = verification::Verifier<EnvironmentModelT, SubCommandT>;
+
+    class Option : public ArbitratorBase::Option {
+    public:
+        using Ptr = std::shared_ptr<Option>;
+        using FlagsT = typename ArbitratorBase::Option::FlagsT;
+        using ConstPtr = std::shared_ptr<const Option>;
+
+        enum Flags { NoFlags = 0b0 };
+
+        Option(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior, const FlagsT& flags)
+                : ArbitratorBase::Option(behavior, flags) {
+        }
+
+        /*!
+         * \brief Writes a string representation of the behavior option and its current state to the output stream.
+         *
+         * \param output            Output stream to write into, will be returned also
+         * \param time              Expected execution time point of this behaviors command
+         * \param environmentModel  A read-only object containing the current state of the environment
+         * \param optionIndex       Position index of this option within options()
+         * \param prefix            A string that should be prepended to each line that is written to the output stream
+         * \param suffix            A string that should be appended to each line that is written to the output stream
+         * \return                  The same given input stream (signature similar to std::ostream& operator<<())
+         *
+         * \see Arbitrator::toStream()
+         */
+        std::ostream& toStream(std::ostream& output,
+                               const Time& time,
+                               const EnvironmentModelT& environmentModel,
+                               const int& optionIndex,
+                               const std::string& prefix = "",
+                               const std::string& suffix = "") const override;
+    };
+
+    explicit SequenceArbitrator(const std::string& name = "SequenceArbitrator",
+                                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) override {
+        typename Option::Ptr option = std::make_shared<Option>(behavior, flags);
+        this->addOptionImpl(option);
+        sequenceOptions_.push_back(option);
+    }
+
+    /*!
+     * \brief The invocation condition is true if the first sub-behavior's invocation condition is true.
+     */
+    bool checkInvocationCondition(const Time& time, const EnvironmentModelT& environmentModel) const override {
+        if (sequenceOptions_.empty()) {
+            return false;
+        }
+        return sequenceOptions_.front()->behavior()->checkInvocationCondition(time, environmentModel);
+    }
+
+    /*!
+     * \brief The commitment condition is true as long as the sequence has started and has not yet completed.
+     *
+     * The sequence is committed if the current sub-behavior's commitment condition is true,
+     * or if there are more sub-behaviors to execute after the current one.
+     */
+    bool checkCommitmentCondition(const Time& time, const EnvironmentModelT& environmentModel) const override {
+        if (!sequenceStarted_ || sequenceOptions_.empty()) {
+            return false;
+        }
+        return sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel) ||
+               currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size());
+    }
+
+    /*!
+     * \brief Starts execution of the sequence from the first sub-behavior.
+     */
+    void gainControl(const Time& time, const EnvironmentModelT& environmentModel) override {
+        currentIndex_ = 0;
+        sequenceStarted_ = true;
+        if (!sequenceOptions_.empty()) {
+            sequenceOptions_.front()->behavior()->gainControl(time, environmentModel);
+        }
+    }
+
+    /*!
+     * \brief Stops execution of the sequence and cleans up the current sub-behavior.
+     */
+    void loseControl(const Time& time, const EnvironmentModelT& environmentModel) override {
+        if (sequenceStarted_ && !sequenceOptions_.empty() &&
+            currentIndex_ < static_cast<int>(sequenceOptions_.size())) {
+            sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
+        }
+        currentIndex_ = 0;
+        sequenceStarted_ = false;
+    }
+
+    /*!
+     * \brief Executes sub-behaviors in sequence, advancing when the current one is done.
+     *
+     * Advances to the next sub-behavior when the current sub-behavior's commitment condition
+     * becomes false. Returns the command of the current (active) sub-behavior.
+     */
+    CommandT getCommand(const Time& time, const EnvironmentModelT& environmentModel) override {
+        if (!sequenceStarted_ || sequenceOptions_.empty()) {
+            throw InvocationConditionIsFalseError(
+                "SequenceArbitrator::getCommand() called without prior gainControl() or with no options!");
+        }
+
+        // Advance past sub-behaviors whose commitment condition has become false
+        while (currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size()) &&
+               !sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel)) {
+            sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
+            currentIndex_++;
+            sequenceOptions_.at(currentIndex_)->behavior()->gainControl(time, environmentModel);
+        }
+
+        // Get and verify command from the current sub-behavior
+        typename ArbitratorBase::Option::Ptr currentOption = sequenceOptions_.at(currentIndex_);
+        std::optional<SubCommandT> command = this->getAndVerifyCommand(currentOption, time, environmentModel);
+        if (command) {
+            return command.value();
+        }
+
+        throw NoApplicableOptionPassedVerificationError(
+            "Current sub-behavior in sequence failed verification!");
+    }
+
+    bool isActive() const override {
+        return sequenceStarted_;
+    }
+
+    /*!
+     * \brief Writes a string representation of the SequenceArbitrator with its current state to the output stream.
+     */
+    std::ostream& toStream(std::ostream& output,
+                           const Time& time,
+                           const EnvironmentModelT& environmentModel,
+                           const std::string& prefix = "",
+                           const std::string& suffix = "") const override;
+
+    /*!
+     * \brief Returns a yaml representation of the arbitrator object with its current state
+     *
+     * \param time  Expected execution time point of this behaviors command
+     * \param environmentModel  A read-only object containing the current state of the environment
+     * \return      Yaml representation of this behavior
+     */
+    YAML::Node toYaml(const Time& time, const EnvironmentModelT& environmentModel) const override;
+
+protected:
+    /*!
+     * \brief Not used by SequenceArbitrator (getCommand is fully overridden), but required as pure virtual.
+     */
+    typename ArbitratorBase::Options sortOptionsByGivenPolicy(
+        const typename ArbitratorBase::Options& options,
+        const Time& /*time*/,
+        const EnvironmentModelT& /*environmentModel*/) const override {
+        return options;
+    }
+
+private:
+    std::vector<typename Option::Ptr> sequenceOptions_;
+    int currentIndex_{0};
+    bool sequenceStarted_{false};
+};
+
+} // namespace arbitration_graphs
+
+#include "internal/sequence_arbitrator_io.hpp" // IWYU pragma: keep
diff --git a/python_bindings/include/arbitration_graphs_py/sequence_arbitrator.hpp b/python_bindings/include/arbitration_graphs_py/sequence_arbitrator.hpp
new file mode 100644
index 00000000..7a1fbcfd
--- /dev/null
+++ b/python_bindings/include/arbitration_graphs_py/sequence_arbitrator.hpp
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <pybind11/pybind11.h>
+#include <yaml-cpp/yaml.h>
+
+#include <arbitration_graphs/arbitrator.hpp>
+#include <arbitration_graphs/behavior.hpp>
+#include <arbitration_graphs/sequence_arbitrator.hpp>
+
+#include "command_wrapper.hpp"
+#include "environment_model_wrapper.hpp"
+#include "verification.hpp"
+#include "yaml_helper.hpp"
+
+namespace arbitration_graphs_py {
+
+namespace py = pybind11;
+namespace ag = arbitration_graphs;
+
+inline void bindSequenceArbitrator(py::module& module) {
+    using Time = ag::Time;
+
+    using ArbitratorT = ag::Arbitrator<EnvironmentModelWrapper, CommandWrapper>;
+    using ArbitratorOptionT = typename ArbitratorT::Option;
+
+    using BehaviorT = typename ArbitratorT::Behavior;
+
+    using SequenceArbitratorT = ag::SequenceArbitrator<EnvironmentModelWrapper, CommandWrapper>;
+
+    using OptionT = typename SequenceArbitratorT::Option;
+    using FlagsT = typename OptionT::FlagsT;
+
+    using VerifierT = ag::verification::Verifier<EnvironmentModelWrapper, CommandWrapper>;
+    using PlaceboVerifierT = ag::verification::PlaceboVerifier<EnvironmentModelWrapper, CommandWrapper>;
+
+    py::classh<SequenceArbitratorT, ArbitratorT> sequenceArbitrator(module, "SequenceArbitrator");
+    sequenceArbitrator
+        .def(py::init<const std::string&, const VerifierT::Ptr&>(),
+             py::arg("name") = "SequenceArbitrator",
+             py::arg("verifier") = PlaceboVerifierT())
+        .def("add_option", &SequenceArbitratorT::addOption, py::arg("behavior"), py::arg("flags"))
+        .def(
+            "to_yaml",
+            [](const SequenceArbitratorT& self, const Time& time, const EnvironmentModelWrapper& environmentModel) {
+                return yaml_helper::toYamlAsPythonObject(self, time, environmentModel);
+            },
+            py::arg("time"),
+            py::arg("environment_model"))
+        .def("__repr__",
+             [](const SequenceArbitratorT& self) { return "<SequenceArbitrator '" + self.name() + "'>"; });
+
+    py::classh<OptionT, ArbitratorOptionT> option(sequenceArbitrator, "Option");
+    option.def(py::init<const typename BehaviorT::Ptr&, const FlagsT&>(), py::arg("behavior"), py::arg("flags"));
+
+    py::enum_<typename OptionT::Flags>(option, "Flags").value("NO_FLAGS", OptionT::NoFlags);
+}
+
+} // namespace arbitration_graphs_py
diff --git a/python_bindings/src/bindings.cpp b/python_bindings/src/bindings.cpp
index 6fcec3d6..8b6a2d14 100644
--- a/python_bindings/src/bindings.cpp
+++ b/python_bindings/src/bindings.cpp
@@ -8,6 +8,7 @@
 #include "arbitration_graphs_py/exceptions.hpp"
 #include "arbitration_graphs_py/priority_arbitrator.hpp"
 #include "arbitration_graphs_py/random_arbitrator.hpp"
+#include "arbitration_graphs_py/sequence_arbitrator.hpp"
 #include "arbitration_graphs_py/verifier.hpp"
 
 #define STRINGIFY(x) #x
@@ -28,6 +29,7 @@ PYBIND11_MODULE(arbitration_graphs_py, mainModule) {
     bindCostArbitrator(mainModule);
     bindPriorityArbitrator(mainModule);
     bindRandomArbitrator(mainModule);
+    bindSequenceArbitrator(mainModule);
 
     // Add the __version__ attribute to the module
 #ifdef PROJECT_VERSION
diff --git a/test/sequence_arbitrator.cpp b/test/sequence_arbitrator.cpp
new file mode 100644
index 00000000..2420709d
--- /dev/null
+++ b/test/sequence_arbitrator.cpp
@@ -0,0 +1,236 @@
+#include <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "behavior.hpp"
+#include "sequence_arbitrator.hpp"
+
+#include "dummy_types.hpp"
+
+
+using namespace arbitration_graphs;
+using namespace arbitration_graphs_tests;
+
+
+class SequenceArbitratorTest : public ::testing::Test {
+protected:
+    using OptionFlags = SequenceArbitrator<DummyEnvironmentModel, DummyCommand>::Option::Flags;
+
+    // Behaviors: first two have invocation=false (unavailable), remaining have invocation=true
+    // commitment=false means the behavior completes immediately (no persistent commitment)
+    // commitment=true means the behavior keeps running until told to stop
+    DummyBehavior::Ptr behaviorA = std::make_shared<DummyBehavior>(true, false, "BehaviorA");
+    DummyBehavior::Ptr behaviorB = std::make_shared<DummyBehavior>(true, false, "BehaviorB");
+    DummyBehavior::Ptr behaviorC = std::make_shared<DummyBehavior>(true, true, "BehaviorC");
+
+    SequenceArbitrator<DummyEnvironmentModel, DummyCommand> testSequenceArbitrator;
+
+    DummyEnvironmentModel environmentModel;
+
+    Time time{Clock::now()};
+};
+
+
+TEST_F(SequenceArbitratorTest, InvocationCondition) {
+    // No options: IC should be false
+    EXPECT_FALSE(testSequenceArbitrator.checkInvocationCondition(time, environmentModel));
+    EXPECT_FALSE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // IC is based on the first sub-behavior's IC only
+    DummyBehavior::Ptr unavailableBehavior = std::make_shared<DummyBehavior>(false, false, "Unavailable");
+    testSequenceArbitrator.addOption(unavailableBehavior, OptionFlags::NoFlags);
+    EXPECT_FALSE(testSequenceArbitrator.checkInvocationCondition(time, environmentModel));
+    EXPECT_FALSE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+}
+
+TEST_F(SequenceArbitratorTest, BasicFunctionality) {
+    // IC is based on the first behavior's IC
+    testSequenceArbitrator.addOption(behaviorA, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorB, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorC, OptionFlags::NoFlags);
+
+    EXPECT_TRUE(testSequenceArbitrator.checkInvocationCondition(time, environmentModel));
+    EXPECT_FALSE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+
+    // Sequence has started: CC should be true (A is not committed but B and C remain)
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // First call: A has commitment=false → immediately advances to B
+    // B has commitment=false → immediately advances to C
+    // C has commitment=true → executes C
+    EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
+
+    // C has commitment=true → CC of sequence is still true
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Continue executing C
+    EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // When C's commitment becomes false, the sequence is complete
+    behaviorC->commitmentCondition = false;
+    EXPECT_FALSE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+}
+
+TEST_F(SequenceArbitratorTest, SequentialExecution) {
+    // All behaviors with commitment=true to control the flow manually
+    DummyBehavior::Ptr step1 = std::make_shared<DummyBehavior>(true, true, "Step1");
+    DummyBehavior::Ptr step2 = std::make_shared<DummyBehavior>(true, true, "Step2");
+    DummyBehavior::Ptr step3 = std::make_shared<DummyBehavior>(true, true, "Step3");
+
+    testSequenceArbitrator.addOption(step1, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(step2, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(step3, OptionFlags::NoFlags);
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+
+    // Execute step1 multiple times
+    EXPECT_EQ("Step1", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ("Step1", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Step1 is done → advance to step2
+    step1->commitmentCondition = false;
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+    EXPECT_EQ("Step2", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(1, step1->loseControlCounter);
+
+    // Execute step2 multiple times
+    EXPECT_EQ("Step2", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Step2 is done → advance to step3
+    step2->commitmentCondition = false;
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+    EXPECT_EQ("Step3", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(1, step2->loseControlCounter);
+
+    // Execute step3
+    EXPECT_EQ("Step3", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Step3 is done → sequence complete
+    step3->commitmentCondition = false;
+    EXPECT_FALSE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+}
+
+TEST_F(SequenceArbitratorTest, LoseControlResetsSequence) {
+    DummyBehavior::Ptr step1 = std::make_shared<DummyBehavior>(true, true, "Step1");
+    DummyBehavior::Ptr step2 = std::make_shared<DummyBehavior>(true, true, "Step2");
+
+    testSequenceArbitrator.addOption(step1, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(step2, OptionFlags::NoFlags);
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+    EXPECT_EQ("Step1", testSequenceArbitrator.getCommand(time, environmentModel));
+
+    // Advance to step2
+    step1->commitmentCondition = false;
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+    EXPECT_EQ("Step2", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(1, step1->loseControlCounter);
+
+    // loseControl resets the sequence
+    testSequenceArbitrator.loseControl(time, environmentModel);
+    EXPECT_EQ(1, step2->loseControlCounter);
+    EXPECT_FALSE(testSequenceArbitrator.isActive());
+
+    // Sequence can be restarted from the beginning
+    step1->commitmentCondition = true;
+    step2->commitmentCondition = true;
+    testSequenceArbitrator.gainControl(time, environmentModel);
+    EXPECT_EQ("Step1", testSequenceArbitrator.getCommand(time, environmentModel));
+}
+
+TEST_F(SequenceArbitratorTest, Printout) {
+    testSequenceArbitrator.addOption(behaviorA, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorB, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorC, OptionFlags::NoFlags);
+
+    // clang-format off
+    std::string expectedPrintout = InvocationTrueString + CommitmentFalseString + "SequenceArbitrator\n"
+                                    "    1. " + InvocationTrueString + CommitmentFalseString + "BehaviorA\n"
+                                    "    2. " + InvocationTrueString + CommitmentFalseString + "BehaviorB\n"
+                                    "    3. " + InvocationTrueString + CommitmentTrueString + "BehaviorC";
+    // clang-format on
+    std::string actualPrintout = testSequenceArbitrator.toString(time, environmentModel);
+    std::cout << actualPrintout << '\n';
+
+    EXPECT_EQ(expectedPrintout, actualPrintout);
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+    // A and B have commitment=false, so the sequence advances directly to C
+    EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
+
+    // clang-format off
+    expectedPrintout = InvocationTrueString + CommitmentTrueString + "SequenceArbitrator\n"
+                        "    1. " + InvocationTrueString + CommitmentFalseString + "BehaviorA\n"
+                        "    2. " + InvocationTrueString + CommitmentFalseString + "BehaviorB\n"
+                        " -> 3. " + InvocationTrueString + CommitmentTrueString + "BehaviorC";
+    // clang-format on
+    actualPrintout = testSequenceArbitrator.toString(time, environmentModel);
+    std::cout << actualPrintout << '\n';
+
+    EXPECT_EQ(expectedPrintout, actualPrintout);
+}
+
+TEST_F(SequenceArbitratorTest, ToYaml) {
+    testSequenceArbitrator.addOption(behaviorA, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorB, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(behaviorC, OptionFlags::NoFlags);
+
+    YAML::Node yaml = testSequenceArbitrator.toYaml(time, environmentModel);
+
+    EXPECT_EQ("SequenceArbitrator", yaml["type"].as<std::string>());
+    EXPECT_EQ("SequenceArbitrator", yaml["name"].as<std::string>());
+    EXPECT_EQ(true, yaml["invocationCondition"].as<bool>());
+    EXPECT_EQ(false, yaml["commitmentCondition"].as<bool>());
+
+    ASSERT_EQ(3, yaml["options"].size());
+    EXPECT_EQ("Option", yaml["options"][0]["type"].as<std::string>());
+    EXPECT_EQ("Option", yaml["options"][1]["type"].as<std::string>());
+    EXPECT_EQ("Option", yaml["options"][2]["type"].as<std::string>());
+    EXPECT_EQ("BehaviorA", yaml["options"][0]["behavior"]["name"].as<std::string>());
+    EXPECT_EQ("BehaviorB", yaml["options"][1]["behavior"]["name"].as<std::string>());
+    EXPECT_EQ("BehaviorC", yaml["options"][2]["behavior"]["name"].as<std::string>());
+
+    EXPECT_EQ(false, yaml["activeBehavior"].IsDefined());
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+    testSequenceArbitrator.getCommand(time, environmentModel);
+
+    yaml = testSequenceArbitrator.toYaml(time, environmentModel);
+
+    EXPECT_EQ(true, yaml["invocationCondition"].as<bool>());
+    EXPECT_EQ(true, yaml["commitmentCondition"].as<bool>());
+
+    ASSERT_EQ(true, yaml["activeBehavior"].IsDefined());
+    // A and B have commitment=false, so the sequence advances to C (index 2)
+    EXPECT_EQ(2, yaml["activeBehavior"].as<int>());
+}
+
+TEST(SequenceArbitrator, SubCommandTypeDiffersFromCommandType) {
+    Time time{Clock::now()};
+    DummyEnvironmentModel environmentModel;
+
+    using OptionFlags = SequenceArbitrator<DummyEnvironmentModel, DummyCommandInt, DummyCommand>::Option::Flags;
+
+    // DummyCommandInt is constructed from DummyCommand (string) by taking the string's length.
+    // The underscores in the names are intentional: they ensure the string lengths differ,
+    // so we can verify the correct behavior (step2) is executing.
+    DummyBehavior::Ptr step1 = std::make_shared<DummyBehavior>(true, false, "__Step1__");
+    DummyBehavior::Ptr step2 = std::make_shared<DummyBehavior>(true, true, "____Step2____");
+
+    SequenceArbitrator<DummyEnvironmentModel, DummyCommandInt, DummyCommand> testSequenceArbitrator;
+    testSequenceArbitrator.addOption(step1, OptionFlags::NoFlags);
+    testSequenceArbitrator.addOption(step2, OptionFlags::NoFlags);
+
+    testSequenceArbitrator.gainControl(time, environmentModel);
+
+    // step1 has commitment=false → advances to step2
+    std::string expected = "____Step2____";
+    EXPECT_EQ(expected.length(), testSequenceArbitrator.getCommand(time, environmentModel));
+}

From 98e2bd46cd4c508b142a1bccdd3176680a8404f2 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Sat, 21 Feb 2026 14:02:48 +0000
Subject: [PATCH 3/3] Fix SequenceArbitrator: lazy activation and one-step
 advancement per getCommand call

Co-authored-by: orzechow <5220714+orzechow@users.noreply.github.com>
---
 .../internal/sequence_arbitrator_io.hpp       |  4 +-
 .../sequence_arbitrator.hpp                   | 38 +++++++++-----
 test/sequence_arbitrator.cpp                  | 52 +++++++++++++++----
 3 files changed, 69 insertions(+), 25 deletions(-)

diff --git a/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
index 915f2b0d..7f1c0285 100644
--- a/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
+++ b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp
@@ -40,7 +40,7 @@ std::ostream& SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toSt
 
     for (int i = 0; i < static_cast<int>(this->options().size()); ++i) {
         const typename ArbitratorBase::Option::ConstPtr option = this->options().at(i);
-        const bool isCurrent = sequenceStarted_ && (i == currentIndex_);
+        const bool isCurrent = sequenceStarted_ && currentBehaviorActivated_ && (i == currentIndex_);
 
         if (isCurrent) {
             output << suffix << '\n' << prefix << " -> ";
@@ -61,7 +61,7 @@ YAML::Node SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toYaml(
     for (const typename ArbitratorBase::Option::ConstPtr& option : this->options()) {
         node["options"].push_back(option->toYaml(time, environmentModel));
     }
-    if (sequenceStarted_) {
+    if (sequenceStarted_ && currentBehaviorActivated_) {
         node["activeBehavior"] = currentIndex_;
     }
 
diff --git a/include/arbitration_graphs/sequence_arbitrator.hpp b/include/arbitration_graphs/sequence_arbitrator.hpp
index 413cb975..fce3e27b 100644
--- a/include/arbitration_graphs/sequence_arbitrator.hpp
+++ b/include/arbitration_graphs/sequence_arbitrator.hpp
@@ -106,33 +106,35 @@ class SequenceArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCom
     }
 
     /*!
-     * \brief Starts execution of the sequence from the first sub-behavior.
+     * \brief Starts execution of the sequence, resetting to the first sub-behavior.
+     *
+     * The first sub-behavior will be activated lazily on the first call to getCommand().
      */
-    void gainControl(const Time& time, const EnvironmentModelT& environmentModel) override {
+    void gainControl(const Time& /*time*/, const EnvironmentModelT& /*environmentModel*/) override {
         currentIndex_ = 0;
         sequenceStarted_ = true;
-        if (!sequenceOptions_.empty()) {
-            sequenceOptions_.front()->behavior()->gainControl(time, environmentModel);
-        }
+        currentBehaviorActivated_ = false;
     }
 
     /*!
-     * \brief Stops execution of the sequence and cleans up the current sub-behavior.
+     * \brief Stops execution of the sequence and cleans up the currently active sub-behavior.
      */
     void loseControl(const Time& time, const EnvironmentModelT& environmentModel) override {
-        if (sequenceStarted_ && !sequenceOptions_.empty() &&
-            currentIndex_ < static_cast<int>(sequenceOptions_.size())) {
+        if (sequenceStarted_ && currentBehaviorActivated_ && !sequenceOptions_.empty()) {
             sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
         }
         currentIndex_ = 0;
         sequenceStarted_ = false;
+        currentBehaviorActivated_ = false;
     }
 
     /*!
-     * \brief Executes sub-behaviors in sequence, advancing when the current one is done.
+     * \brief Executes sub-behaviors in sequence, advancing by one step when the current one is done.
      *
-     * Advances to the next sub-behavior when the current sub-behavior's commitment condition
-     * becomes false. Returns the command of the current (active) sub-behavior.
+     * On each call, the current sub-behavior is activated (if not already) and its command is returned.
+     * If the current sub-behavior's commitment condition is false on the next call and a subsequent
+     * sub-behavior exists, the sequence advances exactly one step: loseControl on the current,
+     * gainControl on the next.
      */
     CommandT getCommand(const Time& time, const EnvironmentModelT& environmentModel) override {
         if (!sequenceStarted_ || sequenceOptions_.empty()) {
@@ -140,12 +142,19 @@ class SequenceArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCom
                 "SequenceArbitrator::getCommand() called without prior gainControl() or with no options!");
         }
 
-        // Advance past sub-behaviors whose commitment condition has become false
-        while (currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size()) &&
-               !sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel)) {
+        // If the current sub-behavior has been executed before and its CC is now false, advance one step
+        if (currentBehaviorActivated_ &&
+            currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size()) &&
+            !sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel)) {
             sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
             currentIndex_++;
+            currentBehaviorActivated_ = false;
+        }
+
+        // Lazily activate the current sub-behavior on its first execution
+        if (!currentBehaviorActivated_) {
             sequenceOptions_.at(currentIndex_)->behavior()->gainControl(time, environmentModel);
+            currentBehaviorActivated_ = true;
         }
 
         // Get and verify command from the current sub-behavior
@@ -196,6 +205,7 @@ class SequenceArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCom
     std::vector<typename Option::Ptr> sequenceOptions_;
     int currentIndex_{0};
     bool sequenceStarted_{false};
+    bool currentBehaviorActivated_{false};
 };
 
 } // namespace arbitration_graphs
diff --git a/test/sequence_arbitrator.cpp b/test/sequence_arbitrator.cpp
index 2420709d..821b5b69 100644
--- a/test/sequence_arbitrator.cpp
+++ b/test/sequence_arbitrator.cpp
@@ -17,7 +17,7 @@ class SequenceArbitratorTest : public ::testing::Test {
 protected:
     using OptionFlags = SequenceArbitrator<DummyEnvironmentModel, DummyCommand>::Option::Flags;
 
-    // Behaviors: first two have invocation=false (unavailable), remaining have invocation=true
+    // Behaviors: first two have commitment=false (unavailable), remaining have invocation=true
     // commitment=false means the behavior completes immediately (no persistent commitment)
     // commitment=true means the behavior keeps running until told to stop
     DummyBehavior::Ptr behaviorA = std::make_shared<DummyBehavior>(true, false, "BehaviorA");
@@ -58,16 +58,32 @@ TEST_F(SequenceArbitratorTest, BasicFunctionality) {
     // Sequence has started: CC should be true (A is not committed but B and C remain)
     EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
 
-    // First call: A has commitment=false → immediately advances to B
-    // B has commitment=false → immediately advances to C
-    // C has commitment=true → executes C
+    // First call: gainControl A + getCommand A → "BehaviorA"
+    EXPECT_EQ("BehaviorA", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(0, behaviorA->loseControlCounter);
+
+    // A has CC=false but B remains → sequence is still committed
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Second call: loseControl A, gainControl B + getCommand B → "BehaviorB"
+    EXPECT_EQ("BehaviorB", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(1, behaviorA->loseControlCounter);
+    EXPECT_EQ(0, behaviorB->loseControlCounter);
+
+    // B has CC=false but C remains → sequence is still committed
+    EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
+
+    // Third call: loseControl B, gainControl C + getCommand C → "BehaviorC"
     EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(1, behaviorB->loseControlCounter);
+    EXPECT_EQ(0, behaviorC->loseControlCounter);
 
-    // C has commitment=true → CC of sequence is still true
+    // C has CC=true → sequence is still committed
     EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
 
     // Continue executing C
     EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
+    EXPECT_EQ(0, behaviorC->loseControlCounter);
     EXPECT_TRUE(testSequenceArbitrator.checkCommitmentCondition(time, environmentModel));
 
     // When C's commitment becomes false, the sequence is complete
@@ -162,7 +178,21 @@ TEST_F(SequenceArbitratorTest, Printout) {
     EXPECT_EQ(expectedPrintout, actualPrintout);
 
     testSequenceArbitrator.gainControl(time, environmentModel);
-    // A and B have commitment=false, so the sequence advances directly to C
+    EXPECT_EQ("BehaviorA", testSequenceArbitrator.getCommand(time, environmentModel));
+
+    // clang-format off
+    expectedPrintout = InvocationTrueString + CommitmentTrueString + "SequenceArbitrator\n"
+                        " -> 1. " + InvocationTrueString + CommitmentFalseString + "BehaviorA\n"
+                        "    2. " + InvocationTrueString + CommitmentFalseString + "BehaviorB\n"
+                        "    3. " + InvocationTrueString + CommitmentTrueString + "BehaviorC";
+    // clang-format on
+    actualPrintout = testSequenceArbitrator.toString(time, environmentModel);
+    std::cout << actualPrintout << '\n';
+
+    EXPECT_EQ(expectedPrintout, actualPrintout);
+
+    // Advance through B to C
+    EXPECT_EQ("BehaviorB", testSequenceArbitrator.getCommand(time, environmentModel));
     EXPECT_EQ("BehaviorC", testSequenceArbitrator.getCommand(time, environmentModel));
 
     // clang-format off
@@ -208,8 +238,8 @@ TEST_F(SequenceArbitratorTest, ToYaml) {
     EXPECT_EQ(true, yaml["commitmentCondition"].as<bool>());
 
     ASSERT_EQ(true, yaml["activeBehavior"].IsDefined());
-    // A and B have commitment=false, so the sequence advances to C (index 2)
-    EXPECT_EQ(2, yaml["activeBehavior"].as<int>());
+    // After the first getCommand, BehaviorA (index 0) is active
+    EXPECT_EQ(0, yaml["activeBehavior"].as<int>());
 }
 
 TEST(SequenceArbitrator, SubCommandTypeDiffersFromCommandType) {
@@ -230,7 +260,11 @@ TEST(SequenceArbitrator, SubCommandTypeDiffersFromCommandType) {
 
     testSequenceArbitrator.gainControl(time, environmentModel);
 
-    // step1 has commitment=false → advances to step2
+    // First call: gainControl step1 + getCommand step1
+    std::string step1Name = "__Step1__";
+    EXPECT_EQ(step1Name.length(), testSequenceArbitrator.getCommand(time, environmentModel));
+
+    // Second call: step1.CC=false → advance to step2 (loseControl step1, gainControl step2, getCommand step2)
     std::string expected = "____Step2____";
     EXPECT_EQ(expected.length(), testSequenceArbitrator.getCommand(time, environmentModel));
 }