cmvr-es/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp

#include "devices/arm/aubo_arm/aubo_arm.h"

#include "devices/arm/aubo_arm/aubo_motion_result.h"
#include "devices/arm/aubo_arm/aubo_motion_state.h"
#include "devices/arm/aubo_arm/aubo_safety_state.h"

#include <algorithm>
#include <cctype>
#include <chrono>
#include <condition_variable>
#include <cstring>
#include <exception>
#include <mutex>
#include <thread>
#include <tuple>
#include <utility>

#include "common/base/logging/logger.h"
#include "json/json.h"

#include "aubo_sdk/rpc.h"

namespace cmvr::device {
namespace {

bool cancellationRequested(
    const std::function<bool()>& cancellation_requested) noexcept
{
    if (!cancellation_requested) {
        return false;
    }
    try {
        return cancellation_requested();
    } catch (...) {
        // A broken cancellation source must never permit a queued motion to be
        // submitted after its ownership can no longer be established.
        return true;
    }
}

class MotionOwnerGuard final {
public:
    MotionOwnerGuard(
        aubo_internal::MotionState& state,
        std::atomic<bool>& busy,
        const aubo_internal::MotionToken token)
        : state_(state), busy_(busy), token_(token)
    {
    }

    ~MotionOwnerGuard() noexcept
    {
        try {
            if (requires_settlement_ && !settled_) {
                state_.failMotion(token_);
            } else {
                state_.finish(token_, finish_mode_);
            }
            busy_.store(state_.busy());
        } catch (...) {
            // A failed state lock must not terminate an RPC unwind. Preserve
            // the conservative externally visible state instead.
            busy_.store(true);
        }
    }

    MotionOwnerGuard(const MotionOwnerGuard&) = delete;
    MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete;

    void requireExplicitSettlement() noexcept
    {
        requires_settlement_ = true;
    }

    void settle() noexcept { settled_ = true; }

    void clearOnFinish() noexcept
    {
        finish_mode_ = aubo_internal::MotionFinishMode::Clear;
    }

    void retainKind() noexcept
    {
        finish_mode_ = aubo_internal::MotionFinishMode::Retain;
    }

private:
    aubo_internal::MotionState& state_;
    std::atomic<bool>& busy_;
    aubo_internal::MotionToken token_;
    aubo_internal::MotionFinishMode finish_mode_{
        aubo_internal::MotionFinishMode::RestorePrevious};
    bool requires_settlement_{false};
    bool settled_{false};
};

class StopStateGuard final {
public:
    StopStateGuard(
        aubo_internal::MotionState& state,
        std::atomic<bool>& busy)
        : state_(state), busy_(busy)
    {
    }

    ~StopStateGuard() noexcept
    {
        if (!completed_) {
            try {
                state_.failStop();
            } catch (...) {
                // Keep the facade fail-closed even if state cleanup fails.
            }
            busy_.store(true);
        }
    }

    StopStateGuard(const StopStateGuard&) = delete;
    StopStateGuard& operator=(const StopStateGuard&) = delete;

    bool complete()
    {
        if (!state_.completeStop()) {
            return false;
        }
        busy_.store(false);
        completed_ = true;
        return true;
    }

private:
    aubo_internal::MotionState& state_;
    std::atomic<bool>& busy_;
    bool completed_{false};
};

class SafetyRecoveryGuard final {
public:
    SafetyRecoveryGuard(
        std::shared_ptr<aubo_internal::SafetyState> state,
        const aubo_internal::RecoveryToken token)
        : state_(std::move(state)), token_(token)
    {
    }

    ~SafetyRecoveryGuard() noexcept
    {
        if (!completed_ && state_) {
            try {
                state_->failRecovery(token_);
            } catch (...) {
                // The safety latch itself remains set on every failure path.
            }
        }
    }

    SafetyRecoveryGuard(const SafetyRecoveryGuard&) = delete;
    SafetyRecoveryGuard& operator=(const SafetyRecoveryGuard&) = delete;

    bool complete(
        const bool robot_running,
        const bool controller_idle,
        const bool cancellation_confirmed)
    {
        if (!state_->completeRecovery(
                token_,
                robot_running,
                controller_idle,
                cancellation_confirmed)) {
            return false;
        }
        completed_ = true;
        return true;
    }

private:
    std::shared_ptr<aubo_internal::SafetyState> state_;
    aubo_internal::RecoveryToken token_;
    bool completed_{false};
};

const char* motionStartFailure(
    const aubo_internal::MotionStartStatus status) noexcept
{
    switch (status) {
    case aubo_internal::MotionStartStatus::Invalid:
        return "invalid motion type";
    case aubo_internal::MotionStartStatus::Busy:
        return "another motion is active";
    case aubo_internal::MotionStartStatus::Stopping:
        return "a stop operation is in progress";
    case aubo_internal::MotionStartStatus::Blocked:
        return "the previous stop did not complete; retry stopMotion or reconnect";
    case aubo_internal::MotionStartStatus::Started:
        break;
    }
    return "unknown motion state";
}

const char* motionKindName(const aubo_internal::MotionKind kind) noexcept
{
    switch (kind) {
    case aubo_internal::MotionKind::Joint:
        return "joint";
    case aubo_internal::MotionKind::Linear:
        return "linear";
    case aubo_internal::MotionKind::None:
        break;
    }
    return "unknown";
}

std::vector<std::string> defaultJointNames(const std::size_t dof)
{
    std::vector<std::string> names;
    names.reserve(dof);
    for (std::size_t i = 0; i < dof; ++i) {
        names.push_back("joint_" + std::to_string(i + 1));
    }
    return names;
}

std::string vendorBrandName(const config::VendorRobotArmBrand brand)
{
    switch (brand) {
    case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM:
        return "AuboARM";
    case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN:
    default:
        return "Unknown";
    }
}

using arcs::common_interface::RobotModeType;
using arcs::common_interface::RuntimeState;
using arcs::common_interface::SafetyModeType;
using arcs::aubo_sdk::RobotInterfacePtr;

RobotInterfacePtr getPrimaryRobotInterface(
    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
    const std::string& context,
    Result& result);

constexpr int kAuboServoMode = 3;
constexpr auto kSafetyPollInterval = std::chrono::milliseconds(50);
constexpr auto kSafetyReconnectInterval = std::chrono::milliseconds(250);
constexpr auto kSafetySampleMaxAge = std::chrono::milliseconds(500);

std::int64_t monotonicNowNs() noexcept
{
    return std::chrono::duration_cast<std::chrono::nanoseconds>(
        std::chrono::steady_clock::now().time_since_epoch())
        .count();
}

aubo_internal::SafetyCondition safetyConditionFromSdk(
    const SafetyModeType mode) noexcept
{
    using Condition = aubo_internal::SafetyCondition;
    switch (mode) {
    case SafetyModeType::Normal:
        return Condition::Normal;
    case SafetyModeType::ReducedMode:
        return Condition::Reduced;
    case SafetyModeType::Recovery:
        return Condition::Recovery;
    case SafetyModeType::Violation:
        return Condition::Violation;
    case SafetyModeType::ProtectiveStop:
        return Condition::ProtectiveStop;
    case SafetyModeType::SafeguardStop:
        return Condition::SafeguardStop;
    case SafetyModeType::SystemEmergencyStop:
        return Condition::SystemEmergencyStop;
    case SafetyModeType::RobotEmergencyStop:
        return Condition::RobotEmergencyStop;
    case SafetyModeType::Fault:
        return Condition::Fault;
    case SafetyModeType::Undefined:
        break;
    }
    return Condition::Unknown;
}

const char* safetyConditionName(
    const aubo_internal::SafetyCondition condition) noexcept
{
    using Condition = aubo_internal::SafetyCondition;
    switch (condition) {
    case Condition::Normal:
        return "Normal";
    case Condition::Reduced:
        return "Reduced";
    case Condition::Recovery:
        return "Recovery";
    case Condition::Violation:
        return "Violation";
    case Condition::ProtectiveStop:
        return "ProtectiveStop";
    case Condition::SafeguardStop:
        return "SafeguardStop";
    case Condition::SystemEmergencyStop:
        return "SystemEmergencyStop";
    case Condition::RobotEmergencyStop:
        return "RobotEmergencyStop";
    case Condition::Fault:
        return "Fault";
    case Condition::Unknown:
        break;
    }
    return "Unknown";
}

SafetyMode publicSafetyMode(
    const aubo_internal::SafetyCondition condition) noexcept
{
    using Condition = aubo_internal::SafetyCondition;
    switch (condition) {
    case Condition::Normal:
        return SafetyMode::Normal;
    case Condition::Reduced:
        return SafetyMode::Reduced;
    case Condition::ProtectiveStop:
        return SafetyMode::ProtectiveStop;
    case Condition::SafeguardStop:
        return SafetyMode::SafeguardStop;
    case Condition::SystemEmergencyStop:
        return SafetyMode::SystemEmergencyStop;
    case Condition::RobotEmergencyStop:
        return SafetyMode::EmergencyStop;
    case Condition::Violation:
    case Condition::Fault:
        return SafetyMode::Fault;
    case Condition::Recovery:
    case Condition::Unknown:
        break;
    }
    return SafetyMode::Unknown;
}

RobotMode publicRobotMode(const RobotModeType mode) noexcept
{
    switch (mode) {
    case RobotModeType::NoController:
    case RobotModeType::Disconnected:
        return RobotMode::Disconnected;
    case RobotModeType::PowerOff:
    case RobotModeType::PowerOffing:
        return RobotMode::PowerOff;
    case RobotModeType::Running:
        return RobotMode::Running;
    case RobotModeType::Error:
        return RobotMode::Fault;
    case RobotModeType::PowerOn:
    case RobotModeType::Idle:
    case RobotModeType::BrakeReleasing:
    case RobotModeType::BackDrive:
        return RobotMode::Idle;
    case RobotModeType::ConfirmSafety:
    case RobotModeType::Booting:
    case RobotModeType::Maintaince:
        break;
    }
    return RobotMode::Unknown;
}

struct AuboSafetyMonitor final {
    std::shared_ptr<aubo_internal::SafetyState> safety_state{
        std::make_shared<aubo_internal::SafetyState>()};
    std::shared_ptr<aubo_internal::MotionState> motion_state;
    std::atomic<int> safety_mode{
        static_cast<int>(SafetyModeType::Undefined)};
    std::atomic<int> robot_mode{
        static_cast<int>(RobotModeType::Disconnected)};
    std::atomic<int> runtime_state{
        static_cast<int>(RuntimeState::Stopped)};
    std::atomic<int> emergency_stop_source{-1};
    std::atomic<int> servo_mode_select{0};
    std::atomic<std::int64_t> last_sample_ns{0};
    std::atomic<bool> cancellation_confirmed{true};
    std::atomic<bool> runtime_abort_required{false};
    std::atomic<bool> servo_disable_required{false};
    std::atomic<bool> path_clear_required{false};
    std::atomic<bool> stop_requested{false};
    std::mutex wait_mutex;
    std::condition_variable wait_cv;
    std::mutex termination_mutex;
    std::recursive_mutex command_rpc_mutex;
    std::string arm_id;
};

std::shared_ptr<arcs::aubo_sdk::RpcClient> makeRpcClient()
{
    return std::shared_ptr<arcs::aubo_sdk::RpcClient>(
        ::createRpcClient(),
        [](arcs::aubo_sdk::RpcClient* client) {
            if (client) {
                ::destroyRpcClient(client);
            }
        });
}

bool monitorWait(
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const std::chrono::milliseconds duration)
{
    std::unique_lock lock(monitor->wait_mutex);
    return monitor->wait_cv.wait_for(
        lock,
        duration,
        [&monitor]() { return monitor->stop_requested.load(); });
}

void cancelForSafetyTransition(
    const std::shared_ptr<AuboSafetyMonitor>& monitor)
{
    monitor->cancellation_confirmed.store(false);
    if (monitor->runtime_state.load() !=
        static_cast<int>(RuntimeState::Stopped)) {
        monitor->runtime_abort_required.store(true);
    }
    if (monitor->servo_mode_select.load() != 0) {
        monitor->servo_disable_required.store(true);
    }
    monitor->motion_state->cancelActiveForSafety();
}

void publishSafetySample(
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const SafetyModeType safety_mode,
    const RobotModeType robot_mode,
    const RuntimeState runtime_state,
    const int emergency_stop_source,
    const int servo_mode_select)
{
    const auto previous = monitor->safety_state->snapshot();
    const int previous_runtime_state = monitor->runtime_state.load();
    const int previous_servo_mode = monitor->servo_mode_select.load();
    const auto condition = aubo_internal::effectiveSafetyCondition(
        safetyConditionFromSdk(safety_mode), emergency_stop_source);
    monitor->safety_state->observe(condition);
    const auto current = monitor->safety_state->snapshot();

    monitor->safety_mode.store(static_cast<int>(safety_mode));
    monitor->robot_mode.store(static_cast<int>(robot_mode));
    monitor->runtime_state.store(static_cast<int>(runtime_state));
    monitor->emergency_stop_source.store(emergency_stop_source);
    monitor->servo_mode_select.store(servo_mode_select);
    monitor->last_sample_ns.store(monotonicNowNs());

    if (previous.observed != condition ||
        (!previous.latched && current.latched)) {
        if (current.latched) {
            if (previous_runtime_state !=
                    static_cast<int>(RuntimeState::Stopped) ||
                runtime_state != RuntimeState::Stopped) {
                monitor->runtime_abort_required.store(true);
            }
            if (previous_servo_mode != 0 || servo_mode_select != 0) {
                monitor->servo_disable_required.store(true);
            }
            cancelForSafetyTransition(monitor);
        }
        if (current.latched) {
            CMVR_LOG(WARNING)
                << "[AuboArm] safety state changed, id=" << monitor->arm_id
                << ", state=" << safetyConditionName(condition)
                << ", latched=true";
        } else {
            CMVR_LOG(INFO)
                << "[AuboArm] safety state changed, id=" << monitor->arm_id
                << ", state=" << safetyConditionName(condition)
                << ", latched=false";
        }
    }
}

void refreshSafetySample(
    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const RobotInterfacePtr& robot_interface)
{
    auto robot_state = robot_interface->getRobotState();
    publishSafetySample(
        monitor,
        robot_state->getSafetyModeType(),
        robot_state->getRobotModeType(),
        rpc_client->getRuntimeMachine()->getRuntimeState(),
        robot_interface->getRobotConfig()
            ->getRobotEmergencyStopSource(),
        robot_interface->getMotionControl()->getServoModeSelect());
}

void publishSafetyUnavailable(
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const std::string& reason)
{
    const auto previous = monitor->safety_state->snapshot();
    monitor->safety_state->observe(
        aubo_internal::SafetyCondition::Unknown);
    monitor->safety_mode.store(
        static_cast<int>(SafetyModeType::Undefined));
    monitor->robot_mode.store(
        static_cast<int>(RobotModeType::Disconnected));
    monitor->emergency_stop_source.store(-1);
    monitor->last_sample_ns.store(0);
    if (previous.observed != aubo_internal::SafetyCondition::Unknown ||
        !previous.latched) {
        cancelForSafetyTransition(monitor);
        CMVR_LOG(WARNING)
            << "[AuboArm] safety monitor unavailable, id="
            << monitor->arm_id << ", reason=" << reason;
    }
}

bool safetySampleFresh(
    const std::shared_ptr<AuboSafetyMonitor>& monitor) noexcept
{
    const auto sample_ns = monitor->last_sample_ns.load();
    if (sample_ns <= 0) {
        return false;
    }
    const auto age_ns = monotonicNowNs() - sample_ns;
    return age_ns >= 0 &&
           age_ns <= std::chrono::duration_cast<std::chrono::nanoseconds>(
                         kSafetySampleMaxAge)
                         .count();
}

bool validateSafetyPermit(
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const aubo_internal::SafetyPermit permit)
{
    if (!safetySampleFresh(monitor)) {
        publishSafetyUnavailable(monitor, "sample is stale");
        return false;
    }
    return monitor->emergency_stop_source.load() == 0 &&
           monitor->robot_mode.load() ==
               static_cast<int>(RobotModeType::Running) &&
           monitor->safety_state->validate(permit);
}

bool enforceControllerTermination(
    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
    const std::shared_ptr<AuboSafetyMonitor>& monitor)
{
    std::unique_lock termination_lock(monitor->termination_mutex);
    monitor->motion_state->cancelActiveForSafety();
    const auto stop_request = monitor->motion_state->beginStop();
    if (!stop_request.started()) {
        return monitor->cancellation_confirmed.load();
    }

    const auto fail = [&monitor]() {
        monitor->motion_state->failStop();
        monitor->cancellation_confirmed.store(false);
        return false;
    };

    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "safety termination", interface_result);
        if (!interface_result.ok()) {
            return fail();
        }
        auto motion_control = robot_interface->getMotionControl();
        auto robot_state = robot_interface->getRobotState();
        auto runtime = rpc_client->getRuntimeMachine();

        constexpr auto kTerminationTimeout = std::chrono::seconds(2);
        constexpr int kStableSamples = 3;
        const auto deadline =
            std::chrono::steady_clock::now() + kTerminationTimeout;
        int stable_samples = 0;
        int iteration = 0;
        bool typed_stop_acknowledged =
            !stop_request.tracked_motion;
        while (!monitor->stop_requested.load() &&
               std::chrono::steady_clock::now() < deadline) {
            const int exec_id = motion_control->getExecId();
            const bool steady = robot_state->isSteady();
            int queue_size = motion_control->getQueueSize();
            int trajectory_queue_size =
                motion_control->getTrajectoryQueueSize();
            int servo_mode = motion_control->getServoModeSelect();
            auto runtime_state = runtime->getRuntimeState();

            if (runtime_state != RuntimeState::Stopped) {
                monitor->runtime_abort_required.store(true);
            }
            if (servo_mode != 0) {
                monitor->servo_disable_required.store(true);
            }
            if (queue_size != 0 || trajectory_queue_size != 0) {
                monitor->path_clear_required.store(true);
            }

            if (monitor->runtime_abort_required.load() &&
                iteration % 4 == 0 &&
                runtime->abort() == arcs::common_interface::AUBO_OK) {
                monitor->runtime_abort_required.store(false);
            }
            if (monitor->servo_disable_required.load() &&
                iteration % 4 == 0 &&
                motion_control->setServoModeSelect(0) ==
                    arcs::common_interface::AUBO_OK) {
                monitor->servo_disable_required.store(false);
            }

            const bool controller_moving = exec_id != -1 || !steady;
            if ((stop_request.tracked_motion || controller_moving) &&
                iteration % 4 == 0) {
                int stop_ret = arcs::common_interface::AUBO_OK;
                if (stop_request.kind == aubo_internal::MotionKind::Joint) {
                    stop_ret = motion_control->stopJoint(31.0);
                } else if (stop_request.kind ==
                           aubo_internal::MotionKind::Linear) {
                    stop_ret = motion_control->stopLine(10.0, 10.0);
                } else {
                    // RuntimeMachine::abort() is the only typed-independent
                    // SDK primitive documented to stop arbitrary operation.
                    monitor->runtime_abort_required.store(true);
                    stop_ret = runtime->abort();
                    if (stop_ret == arcs::common_interface::AUBO_OK) {
                        monitor->runtime_abort_required.store(false);
                    }
                }
                if (stop_request.kind !=
                        aubo_internal::MotionKind::None &&
                    stop_ret == arcs::common_interface::AUBO_OK) {
                    typed_stop_acknowledged = true;
                }
            }

            if (monitor->path_clear_required.load() &&
                iteration % 4 == 0 &&
                motion_control->clearPath() ==
                    arcs::common_interface::AUBO_OK) {
                monitor->path_clear_required.store(false);
            }

            queue_size = motion_control->getQueueSize();
            trajectory_queue_size =
                motion_control->getTrajectoryQueueSize();
            servo_mode = motion_control->getServoModeSelect();
            runtime_state = runtime->getRuntimeState();
            const bool owner_active = monitor->motion_state->ownerActive(
                stop_request.active_token);
            const bool idle =
                motion_control->getExecId() == -1 &&
                robot_state->isSteady() &&
                queue_size == 0 &&
                trajectory_queue_size == 0 && servo_mode == 0 &&
                runtime_state == RuntimeState::Stopped && !owner_active &&
                typed_stop_acknowledged &&
                !monitor->runtime_abort_required.load() &&
                !monitor->servo_disable_required.load() &&
                !monitor->path_clear_required.load() &&
                aubo_internal::isMotionSafe(
                    monitor->safety_state->snapshot().observed) &&
                monitor->emergency_stop_source.load() == 0;

            if (idle) {
                if (++stable_samples >= kStableSamples) {
                    if (!monitor->motion_state->completeStop()) {
                        return fail();
                    }
                    monitor->servo_mode_select.store(0);
                    monitor->runtime_state.store(
                        static_cast<int>(RuntimeState::Stopped));
                    monitor->cancellation_confirmed.store(true);
                    CMVR_LOG(INFO)
                        << "[AuboArm] safety termination confirmed, id="
                        << monitor->arm_id;
                    return true;
                }
            } else {
                stable_samples = 0;
            }

            ++iteration;
            if (monitorWait(monitor, kSafetyPollInterval)) {
                break;
            }
        }
    } catch (const std::exception& e) {
        CMVR_LOG(WARNING)
            << "[AuboArm] safety termination attempt failed, id="
            << monitor->arm_id << ", error=" << e.what();
    }
    return fail();
}

// Powering on to Idle keeps the brakes engaged. This pre-startup phase clears
// the controller queues without completing MotionState, so the retained
// Joint/Linear kind survives until a typed stop is acknowledged in Running.
bool prepareControllerForStartup(
    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
    const std::shared_ptr<AuboSafetyMonitor>& monitor)
{
    std::unique_lock termination_lock(monitor->termination_mutex);
    const auto cancellation =
        monitor->motion_state->cancelActiveForSafety();

    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "pre-startup safety cleanup", interface_result);
        if (!interface_result.ok()) {
            return false;
        }
        auto motion_control = robot_interface->getMotionControl();
        auto runtime = rpc_client->getRuntimeMachine();

        constexpr auto kCleanupTimeout = std::chrono::seconds(2);
        constexpr int kStableSamples = 3;
        const auto deadline =
            std::chrono::steady_clock::now() + kCleanupTimeout;
        int stable_samples = 0;
        int iteration = 0;
        while (!monitor->stop_requested.load() &&
               std::chrono::steady_clock::now() < deadline) {
            int queue_size = motion_control->getQueueSize();
            int trajectory_queue_size =
                motion_control->getTrajectoryQueueSize();
            int servo_mode = motion_control->getServoModeSelect();
            auto runtime_state = runtime->getRuntimeState();

            if (runtime_state != RuntimeState::Stopped) {
                monitor->runtime_abort_required.store(true);
            }
            if (servo_mode != 0) {
                monitor->servo_disable_required.store(true);
            }
            if (queue_size != 0 || trajectory_queue_size != 0) {
                monitor->path_clear_required.store(true);
            }

            if (iteration % 4 == 0) {
                if (monitor->runtime_abort_required.load() &&
                    runtime->abort() == arcs::common_interface::AUBO_OK) {
                    monitor->runtime_abort_required.store(false);
                }
                if (monitor->servo_disable_required.load() &&
                    motion_control->setServoModeSelect(0) ==
                        arcs::common_interface::AUBO_OK) {
                    monitor->servo_disable_required.store(false);
                }
                if (monitor->path_clear_required.load() &&
                    motion_control->clearPath() ==
                        arcs::common_interface::AUBO_OK) {
                    monitor->path_clear_required.store(false);
                }
            }

            queue_size = motion_control->getQueueSize();
            trajectory_queue_size =
                motion_control->getTrajectoryQueueSize();
            servo_mode = motion_control->getServoModeSelect();
            runtime_state = runtime->getRuntimeState();
            const bool owner_active =
                monitor->motion_state->ownerActive(
                    cancellation.active_token);
            const bool queues_cleared =
                motion_control->getExecId() == -1 &&
                queue_size == 0 && trajectory_queue_size == 0 &&
                servo_mode == 0 &&
                runtime_state == RuntimeState::Stopped &&
                !owner_active &&
                !monitor->runtime_abort_required.load() &&
                !monitor->servo_disable_required.load() &&
                !monitor->path_clear_required.load();
            if (queues_cleared) {
                if (++stable_samples >= kStableSamples) {
                    CMVR_LOG(INFO)
                        << "[AuboArm] pre-startup safety cleanup confirmed, id="
                        << monitor->arm_id;
                    return true;
                }
            } else {
                stable_samples = 0;
            }

            ++iteration;
            if (monitorWait(monitor, kSafetyPollInterval)) {
                break;
            }
        }
    } catch (const std::exception& e) {
        CMVR_LOG(WARNING)
            << "[AuboArm] pre-startup safety cleanup failed, id="
            << monitor->arm_id << ", error=" << e.what();
    }
    return false;
}

bool controllerStillQuiescent(
    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
    const RobotInterfacePtr& robot_interface)
{
    auto motion_control = robot_interface->getMotionControl();
    return motion_control->getExecId() == -1 &&
           motion_control->getQueueSize() == 0 &&
           motion_control->getTrajectoryQueueSize() == 0 &&
           robot_interface->getRobotState()->isSteady() &&
           motion_control->getServoModeSelect() == 0 &&
           rpc_client->getRuntimeMachine()->getRuntimeState() ==
               RuntimeState::Stopped;
}

void runSafetyMonitor(
    const std::shared_ptr<AuboSafetyMonitor>& monitor,
    const std::string& ip,
    const int port,
    const std::string& username,
    const std::string& password)
{
    while (!monitor->stop_requested.load()) {
        auto rpc_client = makeRpcClient();
        try {
            if (!rpc_client) {
                publishSafetyUnavailable(monitor, "create RPC client failed");
            } else {
                rpc_client->setRequestTimeout(250);
                const int connect_ret = rpc_client->connect(ip, port);
                const int login_ret = connect_ret == 0
                    ? rpc_client->login(username, password)
                    : connect_ret;
                if (connect_ret != 0 || login_ret != 0) {
                    publishSafetyUnavailable(
                        monitor,
                        "monitor RPC connect/login failed");
                } else {
                    Result interface_result;
                    auto robot_interface = getPrimaryRobotInterface(
                        rpc_client, "safety monitor", interface_result);
                    if (!interface_result.ok()) {
                        publishSafetyUnavailable(
                            monitor, interface_result.message);
                    } else {
                        while (!monitor->stop_requested.load()) {
                            refreshSafetySample(
                                rpc_client, monitor, robot_interface);

                            if (monitor->safety_state->snapshot().latched) {
                                if (monitor->cancellation_confirmed.load() &&
                                    !controllerStillQuiescent(
                                        rpc_client, robot_interface)) {
                                    CMVR_LOG(WARNING)
                                        << "[AuboArm] controller activity reappeared while safety was latched, id="
                                        << monitor->arm_id;
                                    cancelForSafetyTransition(monitor);
                                }
                                if (!monitor->cancellation_confirmed.load()) {
                                    (void)enforceControllerTermination(
                                        rpc_client, monitor);
                                }
                            }
                            if (monitorWait(
                                    monitor, kSafetyPollInterval)) {
                                break;
                            }
                        }
                    }
                }
            }
        } catch (const std::exception& e) {
            publishSafetyUnavailable(monitor, e.what());
        }

        if (rpc_client) {
            try {
                if (rpc_client->hasLogined()) {
                    rpc_client->logout();
                }
                if (rpc_client->hasConnected()) {
                    rpc_client->disconnect();
                }
            } catch (const std::exception& e) {
                CMVR_LOG(WARNING)
                    << "[AuboArm] safety monitor cleanup failed, id="
                    << monitor->arm_id << ", error=" << e.what();
            }
        }
        if (!monitor->stop_requested.load()) {
            publishSafetyUnavailable(monitor, "monitor RPC disconnected");
            (void)monitorWait(monitor, kSafetyReconnectInterval);
        }
    }
}

enum class CabinetIoOperation {
    GetDigitalInput,
    GetDigitalOutput,
    SetDigitalOutput,
};

std::string lowerString(std::string value)
{
    std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) {
        return static_cast<char>(std::tolower(c));
    });
    return value;
}

bool parseJsonCommand(const std::string& request_json,
                      Json::Value& root,
                      std::string& error)
{
    Json::CharReaderBuilder builder;
    std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
    return reader->parse(
        request_json.data(),
        request_json.data() + request_json.size(),
        &root,
        &error);
}

std::string compactJson(const Json::Value& value)
{
    Json::StreamWriterBuilder builder;
    builder[std::string("indentation")] = "";
    return Json::writeString(builder, value);
}

Json::Value& jsonMember(Json::Value& root, const char* name)
{
    return *root.demand(name, name + std::strlen(name));
}

const Json::Value* findJsonMember(const Json::Value& root, const char* name)
{
    return root.find(name, name + std::strlen(name));
}

bool requiredJsonString(const Json::Value& root,
                        const char* name,
                        std::string& value)
{
    const Json::Value* member = findJsonMember(root, name);
    if (!member || !member->isString() || member->asString().empty()) {
        return false;
    }
    value = member->asString();
    return true;
}

bool requiredJsonInt(const Json::Value& root, const char* name, int& value)
{
    const Json::Value* member = findJsonMember(root, name);
    if (!member || !member->isInt()) {
        return false;
    }
    value = member->asInt();
    return true;
}

bool requiredJsonBool(const Json::Value& root, const char* name, bool& value)
{
    const Json::Value* member = findJsonMember(root, name);
    if (!member || !member->isBool()) {
        return false;
    }
    value = member->asBool();
    return true;
}

bool parseCabinetIoOperation(const std::string& name, CabinetIoOperation& operation)
{
    const std::string normalized = lowerString(name);
    if (normalized == "get_di") {
        operation = CabinetIoOperation::GetDigitalInput;
    } else if (normalized == "get_do") {
        operation = CabinetIoOperation::GetDigitalOutput;
    } else if (normalized == "set_do") {
        operation = CabinetIoOperation::SetDigitalOutput;
    } else {
        return false;
    }
    return true;
}

std::string standardOutputRunstateName(
    const arcs::common_interface::StandardOutputRunState runstate)
{
    return arcs::common_interface::toString(runstate);
}

RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
                                           const std::string& context,
                                           Result& result)
{
    const auto robot_names = rpc_client->getRobotNames();
    if (robot_names.empty()) {
        result = Result::failure(ArmErrorCode::RobotNotReady,
                                 "[AuboArm] " + context + " failed: robot name list is empty");
        return nullptr;
    }

    auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
    if (!robot_interface) {
        result = Result::failure(ArmErrorCode::RobotNotReady,
                                 "[AuboArm] " + context + " failed: robot interface is null");
        return nullptr;
    }

    result = Result::success();
    return robot_interface;
}

bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
                      const RobotModeType& target_mode)
{
    const auto start_time = std::chrono::steady_clock::now();
    while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) {
        const auto current_mode = robot_interface->getRobotState()->getRobotModeType();
        if (current_mode == target_mode) {
            return true;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    return false;
}

template <typename IsCancelled>
aubo_internal::MotionWaitResult waitArrival(
    const RobotInterfacePtr& robot_interface,
    IsCancelled&& is_cancelled)
{
    int retry_count = 0;
    if (is_cancelled()) {
        return aubo_internal::MotionWaitResult::Cancelled;
    }
    int exec_id = robot_interface->getMotionControl()->getExecId();
    while (exec_id == -1 && retry_count++ < 5) {
        if (is_cancelled()) {
            return aubo_internal::MotionWaitResult::Cancelled;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        if (is_cancelled()) {
            return aubo_internal::MotionWaitResult::Cancelled;
        }
        exec_id = robot_interface->getMotionControl()->getExecId();
    }
    if (exec_id == -1) {
        return is_cancelled()
            ? aubo_internal::MotionWaitResult::Cancelled
            : aubo_internal::MotionWaitResult::Failed;
    }
    while (robot_interface->getMotionControl()->getExecId() != -1) {
        if (is_cancelled()) {
            return aubo_internal::MotionWaitResult::Cancelled;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    return is_cancelled()
        ? aubo_internal::MotionWaitResult::Cancelled
        : aubo_internal::MotionWaitResult::Completed;
}

bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode)
{
    for (int i = 0; i < 20; ++i) {
        if (robot_interface->getMotionControl()->getServoModeSelect() == mode) {
            return true;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    return false;
}

CartesianPose poseFromVector(const std::vector<double>& values)
{
    CartesianPose pose;
    if (values.size() >= 6) {
        pose.x = values[0];
        pose.y = values[1];
        pose.z = values[2];
        pose.rx = values[3];
        pose.ry = values[4];
        pose.rz = values[5];
    }
    return pose;
}

} // namespace

struct AuboArm::SdkState {
    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
    std::shared_ptr<aubo_internal::MotionState> motion_state{
        std::make_shared<aubo_internal::MotionState>()};
    std::shared_ptr<AuboSafetyMonitor> safety_monitor;
    std::thread safety_monitor_thread;

    ~SdkState()
    {
        if (safety_monitor) {
            safety_monitor->stop_requested.store(true);
            safety_monitor->wait_cv.notify_all();
        }
        if (safety_monitor_thread.joinable()) {
            safety_monitor_thread.join();
        }
    }
};

AuboArm::AuboArm(const config::RobotArmConfig& cfg)
    : cfg_(cfg)
{
    id_ = cfg.id();
    if (cfg.has_vendor()) {
        vendor_cfg_ = cfg.vendor();
    }

    ip_ = vendor_cfg_.ip();
    port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004;
    username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username();
    password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password();

    const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
    model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model();
    model_.manufacturer = vendorBrandName(vendor_cfg_.brand());
    model_.dof = dof;
    model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end());
    if (model_.joint_names.empty()) {
        model_.joint_names = defaultJointNames(dof);
    }
    if (model_.joint_names.size() != dof) {
        CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_;
        model_.joint_names = defaultJointNames(dof);
    }
}

AuboArm::~AuboArm()
{
    (void)disconnect();
}

bool AuboArm::init()
{
    if (ip_.empty()) {
        CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_;
        return false;
    }
    const auto result = connect(ip_, port_);
    if (!result.ok()) {
        CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message;
        return false;
    }
    return true;
}

bool AuboArm::stop()
{
    return stopMotion().ok();
}

bool AuboArm::executeJsonCommand(const std::string& request_json,
                                 std::string& response_json)
{
    Json::Value response(Json::objectValue);
    jsonMember(response, "success") = false;
    const auto fail = [&](const std::string& error_code,
                          const std::string& error_message) {
        jsonMember(response, "success") = false;
        jsonMember(response, "error_code") = error_code;
        jsonMember(response, "error_message") = error_message;
        response_json = compactJson(response);
        return false;
    };

    Json::Value root;
    std::string parse_error;
    if (!parseJsonCommand(request_json, root, parse_error)) {
        return fail("invalid_json", "invalid json: " + parse_error);
    }
    if (!root.isObject()) {
        return fail("invalid_json", "invalid json: root must be an object");
    }

    std::string command;
    if (!requiredJsonString(root, "command", command)) {
        return fail("invalid_argument",
                    "field 'command' is required and must be a non-empty string");
    }
    command = lowerString(command);
    if (command != "cabinet_io") {
        return fail("unsupported_command", "unsupported json command: " + command);
    }
    jsonMember(response, "command") = command;

    std::string operation_name;
    if (!requiredJsonString(root, "operation", operation_name)) {
        return fail("invalid_argument",
                    "field 'operation' is required and must be a non-empty string");
    }
    operation_name = lowerString(operation_name);
    CabinetIoOperation operation{};
    if (!parseCabinetIoOperation(operation_name, operation)) {
        return fail("invalid_operation",
                    "unsupported cabinet_io operation: " + operation_name);
    }
    jsonMember(response, "operation") = operation_name;

    int index = -1;
    if (!requiredJsonInt(root, "index", index) || index < 0) {
        return fail("invalid_argument",
                    "field 'index' is required and must be a non-negative JSON integer");
    }
    jsonMember(response, "index") = index;

    bool output_value = false;
    if (operation == CabinetIoOperation::SetDigitalOutput &&
        !requiredJsonBool(root, "value", output_value)) {
        return fail("invalid_argument",
                    "field 'value' is required for set_do and must be a JSON boolean");
    }

    std::lock_guard lock(mutex_);
    const auto ready = ensureConnected_("cabinet_io");
    if (!ready.ok()) {
        return fail("not_connected", ready.message);
    }

    try {
        Result interface_result;
        auto robot_interface =
            getPrimaryRobotInterface(sdk_->rpc_client, "cabinet_io", interface_result);
        if (!interface_result.ok() || !robot_interface) {
            return fail("robot_interface_unavailable", interface_result.message);
        }

        auto io = robot_interface->getIoControl();
        if (!io) {
            return fail("io_interface_unavailable",
                        "[AuboArm] cabinet_io failed: IO interface is null");
        }

        const bool is_input = operation == CabinetIoOperation::GetDigitalInput;
        const int count = is_input
            ? io->getStandardDigitalInputNum()
            : io->getStandardDigitalOutputNum();
        jsonMember(response, "count") = count;
        if (index >= count) {
            return fail(
                "index_out_of_range",
                "[AuboArm] cabinet_io index out of range: index=" +
                    std::to_string(index) + ", count=" + std::to_string(count));
        }

        if (operation == CabinetIoOperation::GetDigitalInput) {
            jsonMember(response, "value") = io->getStandardDigitalInput(index);
        } else {
            const auto runstate = io->getStandardDigitalOutputRunstate(index);
            jsonMember(response, "runstate") = standardOutputRunstateName(runstate);
            jsonMember(response, "runstate_code") = static_cast<int>(runstate);

            if (operation == CabinetIoOperation::GetDigitalOutput) {
                jsonMember(response, "value") =
                    io->getStandardDigitalOutput(index);
            } else {
                if (runstate != arcs::common_interface::StandardOutputRunState::None) {
                    return fail(
                        "output_managed_by_runstate",
                        "[AuboArm] cabinet_io set_do rejected: output is managed by "
                        "controller runstate; configure this channel as None before writing");
                }

                const int ret = io->setStandardDigitalOutput(index, output_value);
                jsonMember(response, "sdk_return_code") = ret;
                if (ret != 0) {
                    return fail(
                        "sdk_command_failed",
                        "[AuboArm] cabinet_io set_do failed: sdk ret=" +
                            std::to_string(ret));
                }
                jsonMember(response, "requested_value") = output_value;
            }
        }

        jsonMember(response, "success") = true;
        response_json = compactJson(response);
        return true;
    } catch (const arcs::common_interface::AuboException& e) {
        jsonMember(response, "sdk_return_code") = e.code();
        return fail("sdk_exception",
                    std::string("[AuboArm] cabinet_io failed: ") + e.what());
    } catch (const std::exception& e) {
        return fail("sdk_exception",
                    std::string("[AuboArm] cabinet_io failed: ") + e.what());
    }
}

ArmState AuboArm::getRobotState() const
{
    ArmState state;
    state.connected = connected_.load();
    state.moving = busy();
    state.robot_mode = getRobotMode();
    state.safety_mode = getSafetyMode();
    state.control_mode = getControlMode();
    state.powered_on = state.connected &&
                       state.robot_mode != RobotMode::Disconnected &&
                       state.robot_mode != RobotMode::PowerOff &&
                       state.robot_mode != RobotMode::Unknown;
    state.brake_released =
        state.robot_mode == RobotMode::Running &&
        state.safety_mode != SafetyMode::EmergencyStop &&
        state.safety_mode != SafetyMode::SystemEmergencyStop &&
        state.safety_mode != SafetyMode::SafeguardStop;
    state.program_running = false;
    {
        std::lock_guard lock(mutex_);
        if (sdk_ && sdk_->safety_monitor) {
            state.program_running =
                sdk_->safety_monitor->runtime_state.load() ==
                static_cast<int>(RuntimeState::Running);
        }
    }
    state.protective_stopped = isProtectiveStopped();
    state.emergency_stopped = isEmergencyStopped();
    state.fault = isFault();
    state.speed_scaling = speed_scaling_;
    state.actual_joint_state = getJointState();
    state.target_joint_state = state.actual_joint_state;
    return state;
}

JointGroupState AuboArm::getJointState() const
{
    JointGroupState state;
    state.position.assign(model_.dof, 0.0);
    state.velocity.assign(model_.dof, 0.0);
    state.effort.assign(model_.dof, 0.0);

    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
        return state;
    }
    try {
        const auto robot_names = sdk_->rpc_client->getRobotNames();
        if (robot_names.empty()) {
            CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty";
            return state;
        }
        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
        if (!robot_interface) {
            CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null";
            return state;
        }
        const auto robot_state = robot_interface->getRobotState();
        const auto positions = robot_state->getJointPositions();
        const auto velocities = robot_state->getJointSpeeds();
        const auto n = std::min<std::size_t>(model_.dof, positions.size());
        for (std::size_t i = 0; i < n; ++i) {
            state.position[i] = positions[i];
        }
        const auto vn = std::min<std::size_t>(model_.dof, velocities.size());
        for (std::size_t i = 0; i < vn; ++i) {
            state.velocity[i] = velocities[i];
        }
    } catch (const std::exception& e) {
        CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what();
    }
    return state;
}

CartesianPose AuboArm::getTcpPose(FrameType frame) const
{
    (void)frame;
    CartesianPose pose;

    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
        return pose;
    }
    try {
        const auto robot_names = sdk_->rpc_client->getRobotNames();
        if (robot_names.empty()) {
            CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot name list is empty";
            return pose;
        }
        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
        if (!robot_interface) {
            CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot interface is null";
            return pose;
        }
        const auto pose_values = robot_interface->getRobotState()->getTcpPose();
        if (pose_values.size() >= 6) {
            pose.x = pose_values[0];
            pose.y = pose_values[1];
            pose.z = pose_values[2];
            pose.rx = pose_values[3];
            pose.ry = pose_values[4];
            pose.rz = pose_values[5];
        }
    } catch (const std::exception& e) {
        CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: " << e.what();
    }

    return pose;
}

RobotMode AuboArm::getRobotMode() const
{
    if (!connected_.load()) {
        return RobotMode::Disconnected;
    }
    const auto safety_mode = getSafetyMode();
    if (safety_mode == SafetyMode::Fault) {
        return RobotMode::Fault;
    }
    if (safety_mode == SafetyMode::ProtectiveStop ||
        safety_mode == SafetyMode::SafeguardStop ||
        safety_mode == SafetyMode::EmergencyStop ||
        safety_mode == SafetyMode::SystemEmergencyStop) {
        return RobotMode::Stopped;
    }

    std::lock_guard lock(mutex_);
    if (!sdk_ || !sdk_->safety_monitor) {
        return RobotMode::Unknown;
    }
    return publicRobotMode(static_cast<RobotModeType>(
        sdk_->safety_monitor->robot_mode.load()));
}

SafetyMode AuboArm::getSafetyMode() const
{
    if (!connected_.load()) {
        return SafetyMode::Unknown;
    }
    std::lock_guard lock(mutex_);
    if (!sdk_ || !sdk_->safety_monitor) {
        return SafetyMode::Unknown;
    }
    const auto monitor = sdk_->safety_monitor;
    if (!safetySampleFresh(monitor)) {
        publishSafetyUnavailable(monitor, "sample is stale");
    }
    const auto snapshot = monitor->safety_state->snapshot();
    return publicSafetyMode(
        snapshot.latched ? snapshot.latched_reason : snapshot.observed);
}

ControlMode AuboArm::getControlMode() const
{
    if (!connected_.load()) {
        return ControlMode::None;
    }
    std::lock_guard lock(mutex_);
    if (sdk_ && sdk_->safety_monitor &&
        sdk_->safety_monitor->servo_mode_select.load() != 0) {
        return ControlMode::Servo;
    }
    return ControlMode::Position;
}

bool AuboArm::isProtectiveStopped() const
{
    const auto mode = getSafetyMode();
    return mode == SafetyMode::ProtectiveStop ||
           mode == SafetyMode::SafeguardStop;
}

bool AuboArm::isEmergencyStopped() const
{
    const auto mode = getSafetyMode();
    return emergency_stopped_.load() ||
           mode == SafetyMode::EmergencyStop ||
           mode == SafetyMode::SystemEmergencyStop;
}

bool AuboArm::isFault() const
{
    return getSafetyMode() == SafetyMode::Fault ||
           getRobotMode() == RobotMode::Fault;
}

bool AuboArm::busy() const
{
    std::lock_guard lock(mutex_);
    if (sdk_) {
        return sdk_->motion_state->busy();
    }
    return false;
}

Result AuboArm::torqueOn()
{
    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
    std::shared_ptr<AuboSafetyMonitor> monitor;
    {
        std::lock_guard lock(mutex_);
        const auto ready = ensureConnected_("torqueOn");
        if (!ready.ok()) {
            return ready;
        }
        rpc_client = sdk_->rpc_client;
        monitor = sdk_->safety_monitor;
    }

    try {
        if (!monitor) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] torqueOn failed: hardware safety monitor is unavailable");
        }
        std::unique_lock<std::recursive_mutex> command_rpc_lock(
            monitor->command_rpc_mutex);
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "torqueOn", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }

        refreshSafetySample(rpc_client, monitor, robot_interface);
        auto safety_snapshot = monitor->safety_state->snapshot();
        const std::uint64_t entry_safety_epoch = safety_snapshot.epoch;
        if (monitor->emergency_stop_source.load() != 0) {
            return Result::failure(
                ArmErrorCode::RobotInEmergencyStop,
                "[AuboArm] torqueOn rejected: hardware emergency-stop input is still active");
        }

        aubo_internal::RecoveryToken recovery_token;
        std::unique_ptr<SafetyRecoveryGuard> recovery_guard;
        bool recovering = safety_snapshot.latched;
        if (recovering &&
            !aubo_internal::isMotionSafe(safety_snapshot.observed)) {
            const auto condition = safety_snapshot.observed;
            if (aubo_internal::needsProtectiveUnlock(condition)) {
                return Result::failure(
                    ArmErrorCode::RobotInProtectiveStop,
                    "[AuboArm] torqueOn rejected: ProtectiveStop/Violation must be cleared with unlockProtectiveStop first");
            }
            if (condition ==
                aubo_internal::SafetyCondition::SafeguardStop) {
                return Result::failure(
                    ArmErrorCode::RobotInProtectiveStop,
                    "[AuboArm] torqueOn rejected: SafeguardStop requires the external safety IO to be cleared");
            }
            if (condition ==
                aubo_internal::SafetyCondition::Recovery) {
                return Result::failure(
                    ArmErrorCode::RobotNotReady,
                    "[AuboArm] torqueOn rejected: Recovery mode requires manually moving the arm inside its safety limits");
            }
            if (!aubo_internal::needsInterfaceBoardRestart(condition)) {
                return Result::failure(
                    ArmErrorCode::RobotNotReady,
                    "[AuboArm] torqueOn rejected: safety state is " +
                        std::string(safetyConditionName(condition)));
            }

            const int restart_ret =
                robot_interface->getRobotManage()->restartInterfaceBoard();
            if (restart_ret != arcs::common_interface::AUBO_OK) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" +
                        std::to_string(restart_ret));
            }

            const auto safety_deadline =
                std::chrono::steady_clock::now() +
                std::chrono::seconds(10);
            do {
                std::this_thread::sleep_for(
                    std::chrono::milliseconds(100));
                refreshSafetySample(
                    rpc_client, monitor, robot_interface);
                safety_snapshot = monitor->safety_state->snapshot();
                if (aubo_internal::isMotionSafe(
                        safety_snapshot.observed)) {
                    break;
                }
            } while (std::chrono::steady_clock::now() <
                     safety_deadline);
        }

        safety_snapshot = monitor->safety_state->snapshot();
        if (safety_snapshot.epoch != entry_safety_epoch) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] torqueOn recovery rejected: a new safety event occurred while resetting the controller; retry recovery explicitly");
        }
        if (!aubo_internal::isMotionSafe(safety_snapshot.observed)) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] torqueOn rejected: safety state is " +
                    std::string(safetyConditionName(
                        safety_snapshot.observed)));
        }

        if (recovering) {
            const auto token = monitor->safety_state->beginRecovery(
                entry_safety_epoch);
            if (!token.has_value()) {
                return Result::failure(
                    ArmErrorCode::RobotNotReady,
                    "[AuboArm] torqueOn recovery rejected: safety latch changed");
            }
            recovery_token = *token;
            recovery_guard = std::make_unique<SafetyRecoveryGuard>(
                monitor->safety_state, recovery_token);
        }

        double mass = 0.0;
        std::vector<double> cog(3, 0.0);
        std::vector<double> aom(3, 0.0);
        std::vector<double> inertia(6, 0.0);
        robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);

        auto current_mode =
            robot_interface->getRobotState()->getRobotModeType();
        if (current_mode != RobotModeType::Running &&
            current_mode != RobotModeType::Idle) {
            const int poweron_ret =
                robot_interface->getRobotManage()->poweron();
            if (poweron_ret != arcs::common_interface::AUBO_OK) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] torqueOn failed: poweron ret=" +
                        std::to_string(poweron_ret));
            }
            if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) {
                return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle");
            }
            current_mode = RobotModeType::Idle;
        }

        refreshSafetySample(rpc_client, monitor, robot_interface);
        auto before_brake_release =
            monitor->safety_state->snapshot();
        const std::uint64_t expected_epoch = recovering
            ? recovery_token.epoch
            : entry_safety_epoch;
        const bool recovery_token_current = !recovering ||
            (before_brake_release.recovery_in_progress &&
             before_brake_release.epoch == recovery_token.epoch);
        if (before_brake_release.epoch != expected_epoch ||
            !recovery_token_current || before_brake_release.latched != recovering ||
            !aubo_internal::isMotionSafe(
                before_brake_release.observed) ||
            monitor->emergency_stop_source.load() != 0) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] torqueOn rejected: safety state changed before brake release; the new event remains latched");
        }

        if (recovering) {
            cancelForSafetyTransition(monitor);
            const bool cleanup_ok = current_mode == RobotModeType::Running
                ? enforceControllerTermination(rpc_client, monitor)
                : prepareControllerForStartup(rpc_client, monitor);
            if (!cleanup_ok) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release");
            }
        }

        if (current_mode != RobotModeType::Running) {
            const int startup_ret =
                robot_interface->getRobotManage()->startup();
            if (startup_ret != arcs::common_interface::AUBO_OK) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] torqueOn failed: startup ret=" +
                        std::to_string(startup_ret));
            }
            if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) {
                return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running");
            }
        }

        refreshSafetySample(rpc_client, monitor, robot_interface);
        const auto after_startup = monitor->safety_state->snapshot();
        const bool post_recovery_token_current = !recovering ||
            (after_startup.recovery_in_progress &&
             after_startup.epoch == recovery_token.epoch);
        if (after_startup.epoch != expected_epoch ||
            !post_recovery_token_current || after_startup.latched != recovering ||
            !aubo_internal::isMotionSafe(after_startup.observed) ||
            monitor->emergency_stop_source.load() != 0 ||
            monitor->robot_mode.load() !=
                static_cast<int>(RobotModeType::Running)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched");
        }
        if (recovering) {
            cancelForSafetyTransition(monitor);
            if (!enforceControllerTermination(rpc_client, monitor)) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup");
            }
            refreshSafetySample(rpc_client, monitor, robot_interface);
            const bool robot_running =
                monitor->robot_mode.load() ==
                static_cast<int>(RobotModeType::Running);
            if (!recovery_guard->complete(
                    robot_running,
                    true,
                    monitor->cancellation_confirmed.load())) {
                return Result::failure(
                    ArmErrorCode::CommandRejected,
                    "[AuboArm] torqueOn recovery failed: safety state changed during recovery");
            }
        }
        emergency_stopped_.store(false);
        servo_mode_.store(false);
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
    }
}

Result AuboArm::torqueOff()
{
    const auto ready = ensureConnected_("torqueOff");
    if (!ready.ok()) {
        return ready;
    }

    try {
        const auto robot_names = sdk_->rpc_client->getRobotNames();
        if (robot_names.empty()) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
        }
        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
        if (!robot_interface) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
        }
        robot_interface->getRobotManage()->poweroff();
        if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) {
            return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what());
    }
}

Result AuboArm::calibrateZeroQ(const std::string& joint_name)
{
    (void)joint_name;
    return unsupported_("calibrateZeroQ");
}

Result AuboArm::emergencyStop()
{
    emergency_stopped_.store(true);
    {
        std::lock_guard lock(mutex_);
        if (sdk_ && sdk_->safety_monitor) {
            sdk_->safety_monitor->safety_state->observe(
                aubo_internal::SafetyCondition::RobotEmergencyStop);
            cancelForSafetyTransition(sdk_->safety_monitor);
        }
    }
    return stopMotion();
}

Result AuboArm::protectiveStop()
{
    {
        std::lock_guard lock(mutex_);
        if (sdk_ && sdk_->safety_monitor) {
            sdk_->safety_monitor->safety_state->observe(
                aubo_internal::SafetyCondition::ProtectiveStop);
            cancelForSafetyTransition(sdk_->safety_monitor);
        }
    }
    return stopMotion();
}

Result AuboArm::setSpeedScaling(const double scaling)
{
    if (scaling < 0.0 || scaling > 1.0) {
        return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]");
    }
    speed_scaling_ = scaling;
    return Result::success();
}

Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
{
    std::string error;
    if (!validDof_(target.position.size(), error)) {
        return Result::failure(ArmErrorCode::InvalidDof, error);
    }
    try {
        std::unique_lock submit_lock(mutex_);
        const auto locked_ready = ensureConnected_("moveJ");
        if (!locked_ready.ok()) {
            return locked_ready;
        }
        std::uint64_t safety_epoch = 0;
        const auto safety_ready = ensureMotionReady_(
            "moveJ", safety_epoch);
        if (!safety_ready.ok()) {
            return safety_ready;
        }
        const auto rpc_client = sdk_->rpc_client;
        const auto motion_state = sdk_->motion_state;
        const auto safety_monitor = sdk_->safety_monitor;
        const aubo_internal::SafetyPermit safety_permit{safety_epoch};
        const auto motion = motion_state->begin(
            aubo_internal::MotionKind::Joint);
        if (!motion.started()) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] moveJ rejected: " +
                    std::string(motionStartFailure(motion.status)) +
                    ", id=" + id_);
        }
        busy_.store(true);
        MotionOwnerGuard motion_owner{
            *motion_state, busy_, motion.token};

        const auto robot_names = rpc_client->getRobotNames();
        if (robot_names.empty()) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
        }
        auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
        if (!robot_interface) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
        }
        auto motion_control = robot_interface->getMotionControl();
        motion_control->setSpeedFraction(speed_scaling_);
        motion_owner.requireExplicitSettlement();
        if (cancellationRequested(options.cancellation_requested) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveJ cancelled before submission");
        }
        const int ret = motion_control->moveJoint(
            target.position,
            options.acceleration > 0.0 ? options.acceleration : 0.5,
            options.velocity > 0.0 ? options.velocity : 0.5,
            options.blend_radius,
            0);
        if (ret == arcs::common_interface::AUBO_OK) {
            submit_lock.unlock();
        } else {
            motion_owner.settle();
        }
        const auto outcome = aubo_internal::resolveMotionCommand(
            ret,
            arcs::common_interface::AUBO_OK,
            arcs::common_interface::AUBO_REQUEST_IGNORE,
            [&robot_interface,
             motion_state,
             safety_monitor,
             safety_permit,
             cancellation_requested = options.cancellation_requested,
             token = motion.token]() {
                return waitArrival(
                    robot_interface,
                    [motion_state,
                     safety_monitor,
                     safety_permit,
                     cancellation_requested,
                     token]() {
                        return cancellationRequested(
                                   cancellation_requested) ||
                               motion_state->cancelled(token) ||
                               !validateSafetyPermit(
                                   safety_monitor, safety_permit);
                    });
            });
        if (cancellationRequested(options.cancellation_requested)) {
            if (ret == arcs::common_interface::AUBO_OK) {
                if (outcome ==
                    aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
                    motion_owner.clearOnFinish();
                } else {
                    // The request was accepted but its completion is no longer
                    // owned by this caller. Preserve the typed motion state so
                    // the cancellation owner can issue stopJoint/stopLine.
                    motion_owner.retainKind();
                }
            }
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveJ cancelled by its caller");
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveJ cancelled by hardware safety event");
        }
        switch (outcome) {
        case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
            CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret="
                            << ret << " ("
                            << arcs::common_interface::returnValue2Str(ret) << ")";
            return Result::success();
        case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
            motion_owner.clearOnFinish();
            motion_owner.settle();
            return Result::success();
        case aubo_internal::MotionCommandOutcome::Cancelled:
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveJ cancelled by stopMotion or hardware safety event");
        case aubo_internal::MotionCommandOutcome::SubmitFailed:
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] moveJ failed: sdk ret=" + std::to_string(ret) +
                    " (" + arcs::common_interface::returnValue2Str(ret) + ")");
        case aubo_internal::MotionCommandOutcome::CompletionFailed:
            return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete");
        }
        return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ failed: unknown outcome");
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what());
    }
}

Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration)
{
    std::string error;
    if (!validDof_(velocity.velocity.size(), error)) {
        return Result::failure(ArmErrorCode::InvalidDof, error);
    }
    try {
        std::unique_lock submit_lock(mutex_);
        const auto locked_ready = ensureConnected_("speedJ");
        if (!locked_ready.ok()) {
            return locked_ready;
        }
        std::uint64_t safety_epoch = 0;
        const auto safety_ready = ensureMotionReady_(
            "speedJ", safety_epoch);
        if (!safety_ready.ok()) {
            return safety_ready;
        }
        const auto rpc_client = sdk_->rpc_client;
        const auto motion_state = sdk_->motion_state;
        const auto safety_monitor = sdk_->safety_monitor;
        const aubo_internal::SafetyPermit safety_permit{safety_epoch};
        const auto motion = motion_state->begin(
            aubo_internal::MotionKind::Joint, true);
        if (!motion.started()) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] speedJ rejected: " +
                    std::string(motionStartFailure(motion.status)) +
                    ", id=" + id_);
        }
        busy_.store(true);
        MotionOwnerGuard motion_owner{
            *motion_state, busy_, motion.token};

        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "speedJ", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }

        robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
        const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5;
        const double resolved_duration = duration > 0.0 ? duration : 100.0;
        motion_owner.requireExplicitSettlement();
        submit_lock.unlock();
        if (motion_state->cancelled(motion.token) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] speedJ cancelled before submission");
        }
        const int ret = robot_interface->getMotionControl()->speedJoint(
            velocity.velocity,
            resolved_acceleration,
            resolved_duration);
        if (motion_state->cancelled(motion.token) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] speedJ cancelled by stopMotion or hardware safety event");
        }
        if (ret != 0) {
            motion_owner.settle();
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] speedJ failed: ret=" + std::to_string(ret));
        }
        motion_owner.retainKind();
        motion_owner.settle();
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what());
    }
}

Result AuboArm::stopJ(double acceleration)
{
    return stopMotion_(MotionStopKind::Joint, acceleration);
}

Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
{
    (void)frame;
    try {
        std::unique_lock submit_lock(mutex_);
        const auto locked_ready = ensureConnected_("moveL");
        if (!locked_ready.ok()) {
            return locked_ready;
        }
        std::uint64_t safety_epoch = 0;
        const auto safety_ready = ensureMotionReady_(
            "moveL", safety_epoch);
        if (!safety_ready.ok()) {
            return safety_ready;
        }
        const auto rpc_client = sdk_->rpc_client;
        const auto motion_state = sdk_->motion_state;
        const auto safety_monitor = sdk_->safety_monitor;
        const aubo_internal::SafetyPermit safety_permit{safety_epoch};
        const auto motion = motion_state->begin(
            aubo_internal::MotionKind::Linear);
        if (!motion.started()) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] moveL rejected: " +
                    std::string(motionStartFailure(motion.status)) +
                    ", id=" + id_);
        }
        busy_.store(true);
        MotionOwnerGuard motion_owner{
            *motion_state, busy_, motion.token};

        const auto robot_names = rpc_client->getRobotNames();
        if (robot_names.empty()) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
        }
        auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
        if (!robot_interface) {
            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
        }
        auto motion_control = robot_interface->getMotionControl();
        motion_control->setSpeedFraction(speed_scaling_);
        std::vector<double> tcp_offset(6, 0.0);
        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
        std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
        motion_owner.requireExplicitSettlement();
        if (cancellationRequested(options.cancellation_requested) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveL cancelled before submission");
        }
        const int ret = motion_control->moveLine(
            pose,
            options.acceleration > 0.0 ? options.acceleration : 0.5,
            options.velocity > 0.0 ? options.velocity : 0.25,
            options.blend_radius,
            0);
        if (ret == arcs::common_interface::AUBO_OK) {
            submit_lock.unlock();
        } else {
            motion_owner.settle();
        }
        const auto outcome = aubo_internal::resolveMotionCommand(
            ret,
            arcs::common_interface::AUBO_OK,
            arcs::common_interface::AUBO_REQUEST_IGNORE,
            [&robot_interface,
             motion_state,
             safety_monitor,
             safety_permit,
             cancellation_requested = options.cancellation_requested,
             token = motion.token]() {
                return waitArrival(
                    robot_interface,
                    [motion_state,
                     safety_monitor,
                     safety_permit,
                     cancellation_requested,
                     token]() {
                        return cancellationRequested(
                                   cancellation_requested) ||
                               motion_state->cancelled(token) ||
                               !validateSafetyPermit(
                                   safety_monitor, safety_permit);
                    });
            });
        if (cancellationRequested(options.cancellation_requested)) {
            if (ret == arcs::common_interface::AUBO_OK) {
                if (outcome ==
                    aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
                    motion_owner.clearOnFinish();
                } else {
                    // Keep the accepted linear kind until a typed Stop confirms
                    // that the controller is idle.
                    motion_owner.retainKind();
                }
            }
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveL cancelled by its caller");
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveL cancelled by hardware safety event");
        }
        switch (outcome) {
        case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
            CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret="
                            << ret << " ("
                            << arcs::common_interface::returnValue2Str(ret) << ")";
            return Result::success();
        case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
            motion_owner.clearOnFinish();
            motion_owner.settle();
            return Result::success();
        case aubo_internal::MotionCommandOutcome::Cancelled:
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] moveL cancelled by stopMotion or hardware safety event");
        case aubo_internal::MotionCommandOutcome::SubmitFailed:
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] moveL failed: sdk ret=" + std::to_string(ret) +
                    " (" + arcs::common_interface::returnValue2Str(ret) + ")");
        case aubo_internal::MotionCommandOutcome::CompletionFailed:
            return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete");
        }
        return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL failed: unknown outcome");
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what());
    }
}

Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
{
    try {
        std::unique_lock submit_lock(mutex_);
        const auto locked_ready = ensureConnected_("speedL");
        if (!locked_ready.ok()) {
            return locked_ready;
        }
        std::uint64_t safety_epoch = 0;
        const auto safety_ready = ensureMotionReady_(
            "speedL", safety_epoch);
        if (!safety_ready.ok()) {
            return safety_ready;
        }
        const auto rpc_client = sdk_->rpc_client;
        const auto motion_state = sdk_->motion_state;
        const auto safety_monitor = sdk_->safety_monitor;
        const aubo_internal::SafetyPermit safety_permit{safety_epoch};
        const auto motion = motion_state->begin(
            aubo_internal::MotionKind::Linear, true);
        if (!motion.started()) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] speedL rejected: " +
                    std::string(motionStartFailure(motion.status)) +
                    ", id=" + id_);
        }
        busy_.store(true);
        MotionOwnerGuard motion_owner{
            *motion_state, busy_, motion.token};

        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "speedL", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }

        robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
        std::vector<double> tcp_offset(6, 0.0);
        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);

        std::vector<double> line_speed{velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0};
        std::vector<double> angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
        if (frame == FrameType::Tool) {
            auto tool_frame = robot_interface->getRobotState()->getTcpPose();
            if (tool_frame.size() < 6) {
                return Result::failure(ArmErrorCode::CommandFailed,
                                       "[AuboArm] speedL failed: tcp pose size is less than 6");
            }
            tool_frame[0] = 0.0;
            tool_frame[1] = 0.0;
            tool_frame[2] = 0.0;
            line_speed = rpc_client->getMath()->poseTrans(
                tool_frame, line_speed);
            angular_speed = rpc_client->getMath()->poseTrans(
                tool_frame, angular_speed);
        } else if (frame == FrameType::User) {
            return Result::failure(ArmErrorCode::UnsupportedCommand,
                                   "[AuboArm] speedL User frame requires a configured user coordinate frame");
        }

        std::vector<double> speed{
            line_speed[0],
            line_speed[1],
            line_speed[2],
            angular_speed[0],
            angular_speed[1],
            angular_speed[2],
        };

        const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2;
        const double resolved_duration = duration > 0.0 ? duration : 100.0;
        motion_owner.requireExplicitSettlement();
        submit_lock.unlock();
        if (motion_state->cancelled(motion.token) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] speedL cancelled before submission");
        }
        const int ret = robot_interface->getMotionControl()->speedLine(
            speed,
            resolved_acceleration,
            resolved_duration);
        if (motion_state->cancelled(motion.token) ||
            !validateSafetyPermit(safety_monitor, safety_permit)) {
            motion_owner.settle();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] speedL cancelled by stopMotion or hardware safety event");
        }
        if (ret != 0) {
            motion_owner.settle();
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] speedL failed: ret=" + std::to_string(ret));
        }
        motion_owner.retainKind();
        motion_owner.settle();
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what());
    }
}

Result AuboArm::stopL(std::optional<double> acceleration)
{
    return stopMotion_(
        MotionStopKind::Linear,
        acceleration.has_value() ? *acceleration : 0.0);
}

Result AuboArm::stopMotion()
{
    return stopMotion_(MotionStopKind::Automatic, 0.0);
}

Result AuboArm::stopMotion_(
    const MotionStopKind requested_kind,
    const double acceleration)
{
    if (!connected_.load()) {
        return Result::success();
    }

    try {
        std::unique_lock submit_lock(mutex_);
        if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
            return Result::success();
        }

        const auto motion_state = sdk_->motion_state;
        aubo_internal::MotionKind forced_kind =
            aubo_internal::MotionKind::None;
        if (requested_kind == MotionStopKind::Joint) {
            forced_kind = aubo_internal::MotionKind::Joint;
        } else if (requested_kind == MotionStopKind::Linear) {
            forced_kind = aubo_internal::MotionKind::Linear;
        }

        const auto stop_request =
            motion_state->beginStop(forced_kind);
        if (!stop_request.started()) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] stopMotion rejected: another stop operation is in progress");
        }
        busy_.store(true);
        StopStateGuard stop_state_guard{
            *motion_state, busy_};

        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            sdk_->rpc_client, "stopMotion", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }

        auto motion_control = robot_interface->getMotionControl();
        auto robot_state = robot_interface->getRobotState();
        int last_exec_id = motion_control->getExecId();
        bool last_steady = robot_state->isSteady();
        const bool requires_vendor_stop =
            stop_request.tracked_motion ||
            last_exec_id != -1 ||
            !last_steady;

        if (requires_vendor_stop &&
            stop_request.kind == aubo_internal::MotionKind::None) {
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] stopMotion failed: controller is moving but the "
                "active direct-motion type is unknown");
        }

        const auto issue_vendor_stop = [&]() -> Result {
            int ret = 0;
            if (stop_request.kind == aubo_internal::MotionKind::Joint) {
                const double resolved_acceleration =
                    acceleration > 0.0 ? acceleration : 31.0;
                ret = motion_control->stopJoint(resolved_acceleration);
            } else {
                const double resolved_acceleration =
                    acceleration > 0.0 ? acceleration : 10.0;
                ret = motion_control->stopLine(
                    resolved_acceleration, resolved_acceleration);
            }
            if (ret != arcs::common_interface::AUBO_OK) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] stopMotion failed: " +
                        std::string(motionKindName(stop_request.kind)) +
                        " stop sdk ret=" + std::to_string(ret) +
                        " (" +
                        arcs::common_interface::returnValue2Str(ret) +
                        ")");
            }
            return Result::success();
        };

        if (requires_vendor_stop) {
            const auto stop_result = issue_vendor_stop();
            if (!stop_result.ok()) {
                return stop_result;
            }
        }

        constexpr auto kStopTimeout = std::chrono::seconds(5);
        constexpr auto kPollInterval = std::chrono::milliseconds(50);
        constexpr int kStableSamples = 3;
        const auto deadline =
            std::chrono::steady_clock::now() + kStopTimeout;
        int stable_samples = 0;
        bool idle_since_stop = last_exec_id == -1 && last_steady;
        bool owner_active =
            motion_state->ownerActive(stop_request.active_token);
        while (std::chrono::steady_clock::now() < deadline) {
            last_exec_id = motion_control->getExecId();
            last_steady = robot_state->isSteady();
            owner_active =
                motion_state->ownerActive(stop_request.active_token);
            const bool physically_idle =
                last_exec_id == -1 && last_steady;
            if (!physically_idle && idle_since_stop) {
                if (stop_request.kind ==
                    aubo_internal::MotionKind::None) {
                    return Result::failure(
                        ArmErrorCode::CommandFailed,
                        "[AuboArm] stopMotion failed: motion started "
                        "after an idle observation but its type is unknown");
                }
                const auto stop_result = issue_vendor_stop();
                if (!stop_result.ok()) {
                    return stop_result;
                }
                idle_since_stop = false;
            }
            if (physically_idle && !owner_active) {
                if (++stable_samples >= kStableSamples) {
                    break;
                }
            } else {
                stable_samples = 0;
            }
            if (physically_idle) {
                idle_since_stop = true;
            }
            std::this_thread::sleep_for(kPollInterval);
        }
        if (stable_samples < kStableSamples) {
            return Result::failure(
                ArmErrorCode::Timeout,
                "[AuboArm] stopMotion failed: timeout waiting for " +
                    std::string(motionKindName(stop_request.kind)) +
                    " motion to stop, generation=" +
                    std::to_string(stop_request.active_token.generation) +
                    ", exec_id=" + std::to_string(last_exec_id) +
                    ", steady=" + (last_steady ? "true" : "false") +
                    ", owner_active=" +
                    (owner_active ? "true" : "false"));
        }
        if (!stop_state_guard.complete()) {
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] stopMotion failed: cancelled motion handler is still active");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
    }
}

Result AuboArm::startServoMode(const ServoOptions& options)
{
    const auto ready = ensureConnected_("startServoMode");
    if (!ready.ok()) {
        return ready;
    }
    std::uint64_t safety_epoch = 0;
    const auto safety_ready = ensureMotionReady_(
        "startServoMode", safety_epoch);
    if (!safety_ready.ok()) {
        return safety_ready;
    }
    const auto safety_monitor = sdk_->safety_monitor;
    const aubo_internal::SafetyPermit safety_permit{safety_epoch};

    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "startServoMode", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] startServoMode cancelled by hardware safety");
        }
        const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode);
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] startServoMode failed: ret=" + std::to_string(ret));
        }
        if (!waitServoModeSelect(robot_interface, kAuboServoMode)) {
            return Result::failure(ArmErrorCode::Timeout,
                                   "[AuboArm] startServoMode failed: timeout waiting for servo mode");
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            (void)robot_interface->getMotionControl()->setServoModeSelect(0);
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] startServoMode cancelled by hardware safety event");
        }
        servo_options_ = options;
        servo_mode_.store(true);
        safety_monitor->servo_mode_select.store(kAuboServoMode);
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] startServoMode failed: ") + e.what());
    }
}

Result AuboArm::servoJ(const JointPositionCommand& target)
{
    std::string error;
    if (!validDof_(target.position.size(), error)) {
        return Result::failure(ArmErrorCode::InvalidDof, error);
    }
    const auto ready = ensureConnected_("servoJ");
    if (!ready.ok()) {
        return ready;
    }
    std::uint64_t safety_epoch = 0;
    const auto safety_ready = ensureMotionReady_("servoJ", safety_epoch);
    if (!safety_ready.ok()) {
        return safety_ready;
    }
    const auto safety_monitor = sdk_->safety_monitor;
    const aubo_internal::SafetyPermit safety_permit{safety_epoch};

    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoJ", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) {
            const auto start_result = startServoMode(servo_options_);
            if (!start_result.ok()) {
                return start_result;
            }
        }
        const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] servoJ cancelled by hardware safety before submission");
        }
        const int ret = robot_interface->getMotionControl()->servoJoint(
            target.position,
            0.0,
            0.0,
            period,
            servo_options_.lookahead_time,
            servo_options_.gain);
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] servoJ failed: ret=" + std::to_string(ret));
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] servoJ cancelled by hardware safety event");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what());
    }
}

Result AuboArm::servoL(const CartesianPose& target, FrameType frame)
{
    const auto ready = ensureConnected_("servoL");
    if (!ready.ok()) {
        return ready;
    }
    std::uint64_t safety_epoch = 0;
    const auto safety_ready = ensureMotionReady_("servoL", safety_epoch);
    if (!safety_ready.ok()) {
        return safety_ready;
    }
    const auto safety_monitor = sdk_->safety_monitor;
    const aubo_internal::SafetyPermit safety_permit{safety_epoch};

    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoL", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) {
            const auto start_result = startServoMode(servo_options_);
            if (!start_result.ok()) {
                return start_result;
            }
        }

        std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
        if (frame == FrameType::Tool) {
            const auto current_pose = robot_interface->getRobotState()->getTcpPose();
            if (current_pose.size() < 6) {
                return Result::failure(ArmErrorCode::CommandFailed,
                                       "[AuboArm] servoL failed: tcp pose size is less than 6");
            }
            pose = sdk_->rpc_client->getMath()->poseTrans(current_pose, pose);
        } else if (frame == FrameType::User) {
            return Result::failure(ArmErrorCode::UnsupportedCommand,
                                   "[AuboArm] servoL User frame requires a configured user coordinate frame");
        }

        const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] servoL cancelled by hardware safety before submission");
        }
        const int ret = robot_interface->getMotionControl()->servoCartesian(
            pose,
            0.0,
            0.0,
            period,
            servo_options_.lookahead_time,
            servo_options_.gain);
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] servoL failed: ret=" + std::to_string(ret));
        }
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] servoL cancelled by hardware safety event");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what());
    }
}

Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity)
{
    const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
    return speedJ(velocity, 1.5, period);
}

Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame)
{
    const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
    return speedL(velocity, 1.2, period, frame);
}

Result AuboArm::stopServoMode()
{
    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
        servo_mode_.store(false);
        return Result::success();
    }
    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopServoMode", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        const int ret = robot_interface->getMotionControl()->setServoModeSelect(0);
        servo_mode_.store(false);
        if (sdk_->safety_monitor) {
            sdk_->safety_monitor->servo_mode_select.store(0);
        }
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] stopServoMode failed: ret=" + std::to_string(ret));
        }
        if (!waitServoModeSelect(robot_interface, 0)) {
            return Result::failure(ArmErrorCode::Timeout,
                                   "[AuboArm] stopServoMode failed: timeout waiting for servo mode disabled");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] stopServoMode failed: ") + e.what());
    }
}

Result AuboArm::connect(const std::string& ip, const int port)
{
    std::lock_guard lock(mutex_);
    if (connected_.load()) {
        return Result::success();
    }
    if (ip.empty()) {
        return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty");
    }

    try {
        const int resolved_port = port > 0 ? port : 30004;
        auto sdk_state = std::make_unique<SdkState>();
        sdk_state->rpc_client = makeRpcClient();
        if (!sdk_state->rpc_client) {
            return Result::failure(ArmErrorCode::ConnectionFailed,
                                   "[AuboArm] connect failed: create RPC client failed");
        }

        sdk_state->rpc_client->setRequestTimeout(1000);
        int ret = sdk_state->rpc_client->connect(ip, resolved_port);
        if (ret != 0) {
            return Result::failure(ArmErrorCode::ConnectionFailed,
                                   "[AuboArm] connect failed: rpc connect ret=" +
                                       std::to_string(ret) + ", ip=" + ip +
                                       ", port=" + std::to_string(resolved_port));
        }

        ret = sdk_state->rpc_client->login(username_, password_);
        if (ret != 0) {
            if (sdk_state->rpc_client->hasConnected()) {
                sdk_state->rpc_client->disconnect();
            }
            return Result::failure(ArmErrorCode::ConnectionFailed,
                                   "[AuboArm] connect failed: login ret=" + std::to_string(ret));
        }

        const auto robot_names = sdk_state->rpc_client->getRobotNames();
        if (robot_names.empty()) {
            if (sdk_state->rpc_client->hasLogined()) {
                sdk_state->rpc_client->logout();
            }
            if (sdk_state->rpc_client->hasConnected()) {
                sdk_state->rpc_client->disconnect();
            }
            return Result::failure(ArmErrorCode::ConnectionFailed,
                                   "[AuboArm] connect failed: robot name list is empty");
        }

        auto robot_interface = sdk_state->rpc_client->getRobotInterface(
            robot_names.front());
        if (!robot_interface) {
            sdk_state->rpc_client->logout();
            sdk_state->rpc_client->disconnect();
            return Result::failure(ArmErrorCode::ConnectionFailed,
                                   "[AuboArm] connect failed: robot interface is null");
        }

        sdk_state->safety_monitor =
            std::make_shared<AuboSafetyMonitor>();
        sdk_state->safety_monitor->motion_state =
            sdk_state->motion_state;
        sdk_state->safety_monitor->arm_id = id_;
        publishSafetySample(
            sdk_state->safety_monitor,
            robot_interface->getRobotState()->getSafetyModeType(),
            robot_interface->getRobotState()->getRobotModeType(),
            sdk_state->rpc_client->getRuntimeMachine()->getRuntimeState(),
            robot_interface->getRobotConfig()
                ->getRobotEmergencyStopSource(),
            robot_interface->getMotionControl()->getServoModeSelect());

        ip_ = ip;
        port_ = resolved_port;
        sdk_state->safety_monitor_thread = std::thread(
            runSafetyMonitor,
            sdk_state->safety_monitor,
            ip_,
            port_,
            username_,
            password_);
        sdk_ = std::move(sdk_state);
        connected_.store(true);
        return Result::success();
    } catch (const std::exception& e) {
        sdk_.reset();
        connected_.store(false);
        return Result::failure(ArmErrorCode::ConnectionFailed,
                               std::string("[AuboArm] connect failed: ") + e.what());
    }
}

Result AuboArm::disconnect()
{
    std::lock_guard lock(mutex_);
    try {
        connected_.store(false);
        if (sdk_ && sdk_->safety_monitor) {
            sdk_->safety_monitor->stop_requested.store(true);
            sdk_->safety_monitor->wait_cv.notify_all();
        }
        if (sdk_ && sdk_->safety_monitor_thread.joinable()) {
            sdk_->safety_monitor_thread.join();
        }
        const auto close_command_rpc = [this]() {
            if (sdk_ && sdk_->rpc_client) {
                if (sdk_->rpc_client->hasLogined()) {
                    sdk_->rpc_client->logout();
                }
                if (sdk_->rpc_client->hasConnected()) {
                    sdk_->rpc_client->disconnect();
                }
            }
        };
        if (sdk_ && sdk_->safety_monitor) {
            std::unique_lock<std::recursive_mutex> command_rpc_lock(
                sdk_->safety_monitor->command_rpc_mutex);
            close_command_rpc();
        } else {
            close_command_rpc();
        }
    } catch (const std::exception& e) {
        CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what();
    }
    sdk_.reset();
    busy_.store(false);
    servo_mode_.store(false);
    emergency_stopped_.store(false);
    return Result::success();
}

Result AuboArm::shutdown()
{
    (void)stopMotion();
    return disconnect();
}

Result AuboArm::clearFault()
{
    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
    std::shared_ptr<AuboSafetyMonitor> monitor;
    {
        std::lock_guard lock(mutex_);
        const auto ready = ensureConnected_("clearFault");
        if (!ready.ok()) {
            return ready;
        }
        rpc_client = sdk_->rpc_client;
        monitor = sdk_->safety_monitor;
    }

    try {
        if (!monitor) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] clearFault failed: hardware safety monitor is unavailable");
        }
        std::unique_lock<std::recursive_mutex> command_rpc_lock(
            monitor->command_rpc_mutex);
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "clearFault", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        refreshSafetySample(rpc_client, monitor, robot_interface);
        auto snapshot = monitor->safety_state->snapshot();
        const std::uint64_t expected_safety_epoch = snapshot.epoch;
        if (!snapshot.latched &&
            aubo_internal::isMotionSafe(snapshot.observed) &&
            monitor->robot_mode.load() !=
                static_cast<int>(RobotModeType::Error)) {
            return Result::success();
        }
        if (!snapshot.latched &&
            monitor->robot_mode.load() ==
                static_cast<int>(RobotModeType::Error)) {
            return Result::failure(
                ArmErrorCode::RobotInFault,
                "[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced");
        }
        if (monitor->emergency_stop_source.load() != 0) {
            return Result::failure(
                ArmErrorCode::RobotInEmergencyStop,
                "[AuboArm] clearFault rejected: hardware emergency-stop input is still active");
        }

        if (aubo_internal::needsProtectiveUnlock(snapshot.observed) ||
            aubo_internal::needsProtectiveUnlock(
                snapshot.latched_reason)) {
            command_rpc_lock.unlock();
            return unlockProtectiveStop_(expected_safety_epoch);
        }

        if (aubo_internal::isMotionSafe(snapshot.observed)) {
            if (monitor->robot_mode.load() ==
                static_cast<int>(RobotModeType::Running)) {
                command_rpc_lock.unlock();
                return completeSafetyRecovery_(
                    "clearFault", expected_safety_epoch);
            }
            return Result::failure(
                ArmErrorCode::RobotNotPowered,
                "[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery");
        }

        if (!aubo_internal::needsInterfaceBoardRestart(
                snapshot.observed)) {
            const std::string guidance =
                snapshot.observed ==
                        aubo_internal::SafetyCondition::SafeguardStop
                    ? "clear the external safety IO"
                    : (snapshot.observed ==
                               aubo_internal::SafetyCondition::Recovery
                           ? "manually move the arm inside its safety limits"
                           : "restore a valid controller safety state");
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] clearFault rejected for " +
                    std::string(safetyConditionName(snapshot.observed)) +
                    ": " + guidance);
        }

        const int ret =
            robot_interface->getRobotManage()->restartInterfaceBoard();
        if (ret != arcs::common_interface::AUBO_OK) {
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] clearFault failed: restartInterfaceBoard ret=" +
                    std::to_string(ret));
        }

        const auto deadline =
            std::chrono::steady_clock::now() + std::chrono::seconds(10);
        while (std::chrono::steady_clock::now() < deadline) {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            refreshSafetySample(rpc_client, monitor, robot_interface);
            snapshot = monitor->safety_state->snapshot();
            if (aubo_internal::isMotionSafe(snapshot.observed)) {
                break;
            }
        }
        if (!aubo_internal::isMotionSafe(snapshot.observed)) {
            return Result::failure(
                ArmErrorCode::Timeout,
                "[AuboArm] clearFault failed: timeout waiting for a safe controller state");
        }
        if (monitor->robot_mode.load() ==
            static_cast<int>(RobotModeType::Running)) {
            command_rpc_lock.unlock();
            return completeSafetyRecovery_(
                "clearFault", expected_safety_epoch);
        }
        return Result::failure(
            ArmErrorCode::RobotNotPowered,
            "[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode");
    } catch (const std::exception& e) {
        return Result::failure(
            ArmErrorCode::CommandFailed,
            std::string("[AuboArm] clearFault failed: ") + e.what());
    }
}

Result AuboArm::unlockProtectiveStop()
{
    return unlockProtectiveStop_(std::nullopt);
}

Result AuboArm::unlockProtectiveStop_(
    const std::optional<std::uint64_t> expected_safety_epoch)
{
    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
    std::shared_ptr<AuboSafetyMonitor> monitor;
    {
        std::lock_guard lock(mutex_);
        const auto ready = ensureConnected_("unlockProtectiveStop");
        if (!ready.ok()) {
            return ready;
        }
        rpc_client = sdk_->rpc_client;
        monitor = sdk_->safety_monitor;
    }

    try {
        if (!monitor) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] unlockProtectiveStop failed: hardware safety monitor is unavailable");
        }
        std::unique_lock<std::recursive_mutex> command_rpc_lock(
            monitor->command_rpc_mutex);
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, "unlockProtectiveStop", interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        refreshSafetySample(rpc_client, monitor, robot_interface);
        auto snapshot = monitor->safety_state->snapshot();
        const std::uint64_t recovery_epoch =
            expected_safety_epoch.value_or(snapshot.epoch);
        if (snapshot.epoch != recovery_epoch) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] unlockProtectiveStop rejected: a newer safety event superseded this recovery request");
        }
        if (!snapshot.latched &&
            aubo_internal::isMotionSafe(snapshot.observed)) {
            return Result::success();
        }
        if (monitor->emergency_stop_source.load() != 0) {
            return Result::failure(
                ArmErrorCode::RobotInEmergencyStop,
                "[AuboArm] unlockProtectiveStop rejected: hardware emergency-stop input is active");
        }

        if (aubo_internal::needsProtectiveUnlock(
                snapshot.observed)) {
            const int ret = robot_interface->getRobotManage()
                                ->setUnlockProtectiveStop();
            if (ret != arcs::common_interface::AUBO_OK) {
                return Result::failure(
                    ArmErrorCode::CommandFailed,
                    "[AuboArm] unlockProtectiveStop failed: sdk ret=" +
                        std::to_string(ret));
            }
        } else if (!aubo_internal::isMotionSafe(snapshot.observed)) {
            return Result::failure(
                ArmErrorCode::RobotInProtectiveStop,
                "[AuboArm] unlockProtectiveStop rejected: current safety state is " +
                    std::string(safetyConditionName(snapshot.observed)));
        }

        const auto deadline =
            std::chrono::steady_clock::now() + std::chrono::seconds(5);
        while (std::chrono::steady_clock::now() < deadline) {
            refreshSafetySample(rpc_client, monitor, robot_interface);
            snapshot = monitor->safety_state->snapshot();
            if (aubo_internal::isMotionSafe(snapshot.observed)) {
                break;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        if (!aubo_internal::isMotionSafe(snapshot.observed)) {
            return Result::failure(
                ArmErrorCode::Timeout,
                "[AuboArm] unlockProtectiveStop failed: safety mode did not return to Normal/Reduced");
        }
        if (monitor->robot_mode.load() !=
            static_cast<int>(RobotModeType::Running)) {
            return Result::failure(
                ArmErrorCode::RobotNotPowered,
                "[AuboArm] protective stop was unlocked, but torqueOn is required to complete safety recovery");
        }
        command_rpc_lock.unlock();
        return completeSafetyRecovery_(
            "unlockProtectiveStop", recovery_epoch);
    } catch (const std::exception& e) {
        return Result::failure(
            ArmErrorCode::CommandFailed,
            std::string("[AuboArm] unlockProtectiveStop failed: ") +
                e.what());
    }
}

Result AuboArm::loadProgram(const std::string& program_name)
{
    if (program_name.empty()) {
        return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] loadProgram failed: program name is empty");
    }
    const auto ready = ensureConnected_("loadProgram");
    if (!ready.ok()) {
        return ready;
    }
    try {
        const int ret = sdk_->rpc_client->getRuntimeMachine()->loadProgram(program_name);
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] loadProgram failed: ret=" + std::to_string(ret));
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] loadProgram failed: ") + e.what());
    }
}

Result AuboArm::playProgram()
{
    const auto ready = ensureConnected_("playProgram");
    if (!ready.ok()) {
        return ready;
    }
    std::uint64_t safety_epoch = 0;
    const auto safety_ready = ensureMotionReady_(
        "playProgram", safety_epoch);
    if (!safety_ready.ok()) {
        return safety_ready;
    }
    const auto safety_monitor = sdk_->safety_monitor;
    const aubo_internal::SafetyPermit safety_permit{safety_epoch};
    try {
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] playProgram cancelled by hardware safety before submission");
        }
        const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram();
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] playProgram failed: ret=" + std::to_string(ret));
        }
        safety_monitor->runtime_state.store(
            static_cast<int>(RuntimeState::Running));
        if (!validateSafetyPermit(safety_monitor, safety_permit)) {
            (void)sdk_->rpc_client->getRuntimeMachine()->abort();
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] playProgram cancelled by hardware safety event");
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] playProgram failed: ") + e.what());
    }
}

Result AuboArm::pauseProgram()
{
    const auto ready = ensureConnected_("pauseProgram");
    if (!ready.ok()) {
        return ready;
    }
    try {
        const int ret = sdk_->rpc_client->getRuntimeMachine()->pause();
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] pauseProgram failed: ret=" + std::to_string(ret));
        }
        if (sdk_->safety_monitor) {
            sdk_->safety_monitor->runtime_state.store(
                static_cast<int>(RuntimeState::Paused));
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] pauseProgram failed: ") + e.what());
    }
}

Result AuboArm::stopProgram()
{
    const auto ready = ensureConnected_("stopProgram");
    if (!ready.ok()) {
        return ready;
    }
    try {
        const int ret = sdk_->rpc_client->getRuntimeMachine()->abort();
        if (ret != 0) {
            return Result::failure(ArmErrorCode::CommandFailed,
                                   "[AuboArm] stopProgram failed: ret=" + std::to_string(ret));
        }
        if (sdk_->safety_monitor) {
            sdk_->safety_monitor->runtime_state.store(
                static_cast<int>(RuntimeState::Stopped));
            sdk_->safety_monitor->runtime_abort_required.store(false);
        }
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(ArmErrorCode::CommandFailed,
                               std::string("[AuboArm] stopProgram failed: ") + e.what());
    }
}

std::vector<double> AuboArm::ik(const std::string& base_link,
                                const std::string& ee_link,
                                const CartesianPose& pose)
{
    (void)base_link;
    (void)ee_link;
    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
        CMVR_LOG(ERROR) << "[AuboArm] ik failed: arm is not connected";
        return {};
    }
    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "ik", interface_result);
        if (!interface_result.ok()) {
            CMVR_LOG(ERROR) << interface_result.message;
            return {};
        }
        const auto qnear = getJointState().position;
        const std::vector<double> target_pose{pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz};
        const auto result = robot_interface->getRobotAlgorithm()->inverseKinematics(qnear, target_pose);
        const int ret = std::get<1>(result);
        if (ret != 0) {
            CMVR_LOG(ERROR) << "[AuboArm] ik failed: ret=" << ret;
            return {};
        }
        return std::get<0>(result);
    } catch (const std::exception& e) {
        CMVR_LOG(ERROR) << "[AuboArm] ik failed: " << e.what();
    }
    return {};
}

CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link)
{
    (void)base_link;
    (void)ee_link;
    return getTcpPose(FrameType::Base);
}

CartesianPose AuboArm::fk(bool is_tcp)
{
    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
        return {};
    }
    try {
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "fk", interface_result);
        if (!interface_result.ok()) {
            CMVR_LOG(ERROR) << interface_result.message;
            return {};
        }
        const auto q = getJointState().position;
        if (q.size() != model_.dof) {
            CMVR_LOG(ERROR) << "[AuboArm] fk failed: joint state dof mismatch";
            return {};
        }
        const auto result = is_tcp
            ? robot_interface->getRobotAlgorithm()->forwardKinematics(q)
            : robot_interface->getRobotAlgorithm()->forwardToolKinematics(q);
        const int ret = std::get<1>(result);
        if (ret != 0) {
            CMVR_LOG(ERROR) << "[AuboArm] fk failed: ret=" << ret;
            return {};
        }
        return poseFromVector(std::get<0>(result));
    } catch (const std::exception& e) {
        CMVR_LOG(ERROR) << "[AuboArm] fk failed: " << e.what();
    }
    return {};
}

Result AuboArm::unsupported_(const std::string& name) const
{
    const std::string message = "[AuboArm] " + name + " is not implemented";
    CMVR_LOG(ERROR) << message;
    return Result::failure(ArmErrorCode::UnsupportedCommand, message);
}

bool AuboArm::validDof_(const std::size_t size, std::string& error) const
{
    if (size != model_.dof) {
        error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) +
                ", actual=" + std::to_string(size);
        CMVR_LOG(ERROR) << error;
        return false;
    }
    return true;
}

Result AuboArm::completeSafetyRecovery_(
    const std::string& context,
    const std::uint64_t expected_safety_epoch)
{
    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
    std::shared_ptr<AuboSafetyMonitor> monitor;
    {
        std::lock_guard lock(mutex_);
        const auto ready = ensureConnected_(context);
        if (!ready.ok()) {
            return ready;
        }
        rpc_client = sdk_->rpc_client;
        monitor = sdk_->safety_monitor;
    }

    try {
        if (!monitor) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] " + context +
                    " failed: hardware safety monitor is unavailable");
        }
        std::unique_lock<std::recursive_mutex> command_rpc_lock(
            monitor->command_rpc_mutex);
        Result interface_result;
        auto robot_interface = getPrimaryRobotInterface(
            rpc_client, context, interface_result);
        if (!interface_result.ok()) {
            return interface_result;
        }
        refreshSafetySample(rpc_client, monitor, robot_interface);
        if (monitor->emergency_stop_source.load() != 0) {
            return Result::failure(
                ArmErrorCode::RobotInEmergencyStop,
                "[AuboArm] " + context +
                    " rejected: hardware emergency-stop input is active");
        }

        const auto snapshot = monitor->safety_state->snapshot();
        if (snapshot.epoch != expected_safety_epoch) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] " + context +
                    " rejected: a newer safety event superseded this recovery request");
        }
        if (!snapshot.latched) {
            return aubo_internal::isMotionSafe(snapshot.observed)
                ? Result::success()
                : Result::failure(
                      ArmErrorCode::RobotNotReady,
                      "[AuboArm] " + context +
                          " failed: safety state is " +
                          safetyConditionName(snapshot.observed));
        }
        if (!aubo_internal::isMotionSafe(snapshot.observed)) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] " + context +
                    " rejected: hardware safety state is " +
                    safetyConditionName(snapshot.observed));
        }
        if (monitor->robot_mode.load() !=
            static_cast<int>(RobotModeType::Running)) {
            return Result::failure(
                ArmErrorCode::RobotNotReady,
                "[AuboArm] " + context +
                    " rejected: robot must be Running before the safety latch can be cleared");
        }

        const auto recovery_token =
            monitor->safety_state->beginRecovery(
                expected_safety_epoch);
        if (!recovery_token.has_value()) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] " + context +
                    " rejected: another recovery is active or the safety state changed");
        }
        SafetyRecoveryGuard recovery_guard{
            monitor->safety_state, *recovery_token};
        cancelForSafetyTransition(monitor);
        if (!enforceControllerTermination(rpc_client, monitor)) {
            return Result::failure(
                ArmErrorCode::CommandFailed,
                "[AuboArm] " + context +
                    " failed: old motion/program could not be terminated and verified");
        }

        refreshSafetySample(rpc_client, monitor, robot_interface);
        const bool robot_running =
            monitor->robot_mode.load() ==
            static_cast<int>(RobotModeType::Running);
        if (!recovery_guard.complete(
                robot_running,
                true,
                monitor->cancellation_confirmed.load())) {
            return Result::failure(
                ArmErrorCode::CommandRejected,
                "[AuboArm] " + context +
                    " failed: safety state changed while recovery was being verified");
        }
        emergency_stopped_.store(false);
        servo_mode_.store(false);
        return Result::success();
    } catch (const std::exception& e) {
        return Result::failure(
            ArmErrorCode::CommandFailed,
            "[AuboArm] " + context +
                " failed during safety recovery: " + e.what());
    }
}

Result AuboArm::ensureConnected_(const std::string& context) const
{
    if (!connected_.load()) {
        return Result::failure(ArmErrorCode::NotConnected,
                               "[AuboArm] " + context + " failed: arm is not connected");
    }
    if (!sdk_ || !sdk_->rpc_client) {
        return Result::failure(ArmErrorCode::NotConnected,
                               "[AuboArm] " + context + " failed: SDK client is null");
    }
    return Result::success();
}

Result AuboArm::ensureMotionReady_(
    const std::string& context,
    std::uint64_t& safety_epoch) const
{
    const auto connected = ensureConnected_(context);
    if (!connected.ok()) {
        return connected;
    }
    if (!sdk_->safety_monitor) {
        return Result::failure(
            ArmErrorCode::RobotNotReady,
            "[AuboArm] " + context +
                " rejected: hardware safety monitor is unavailable");
    }

    const auto monitor = sdk_->safety_monitor;
    if (!safetySampleFresh(monitor)) {
        publishSafetyUnavailable(monitor, "sample is stale");
        return Result::failure(
            ArmErrorCode::RobotNotReady,
            "[AuboArm] " + context +
                " rejected: hardware safety state is unavailable or stale");
    }

    if (monitor->emergency_stop_source.load() != 0) {
        const auto previous = monitor->safety_state->snapshot();
        monitor->safety_state->observe(
            aubo_internal::SafetyCondition::RobotEmergencyStop);
        if (!previous.latched ||
            previous.observed !=
                aubo_internal::SafetyCondition::RobotEmergencyStop) {
            cancelForSafetyTransition(monitor);
        }
    }

    const auto snapshot = monitor->safety_state->snapshot();
    const auto condition = snapshot.latched
        ? snapshot.latched_reason
        : snapshot.observed;
    if (snapshot.latched ||
        !aubo_internal::isMotionSafe(snapshot.observed)) {
        ArmErrorCode code = ArmErrorCode::RobotNotReady;
        if (condition ==
                aubo_internal::SafetyCondition::RobotEmergencyStop ||
            condition ==
                aubo_internal::SafetyCondition::SystemEmergencyStop) {
            code = ArmErrorCode::RobotInEmergencyStop;
        } else if (
            condition == aubo_internal::SafetyCondition::ProtectiveStop ||
            condition == aubo_internal::SafetyCondition::SafeguardStop) {
            code = ArmErrorCode::RobotInProtectiveStop;
        } else if (
            condition == aubo_internal::SafetyCondition::Fault ||
            condition == aubo_internal::SafetyCondition::Violation) {
            code = ArmErrorCode::RobotInFault;
        }
        return Result::failure(
            code,
            "[AuboArm] " + context +
                " rejected: hardware safety latch is " +
                safetyConditionName(condition) +
                "; clear the hardware condition and perform explicit recovery");
    }

    if (monitor->robot_mode.load() !=
        static_cast<int>(RobotModeType::Running)) {
        return Result::failure(
            ArmErrorCode::RobotNotReady,
            "[AuboArm] " + context +
                " rejected: robot is not in Running mode");
    }

    const auto permit = monitor->safety_state->tryPermit();
    if (!permit.has_value()) {
        return Result::failure(
            ArmErrorCode::RobotNotReady,
            "[AuboArm] " + context +
                " rejected: no valid hardware safety permit");
    }
    safety_epoch = permit->epoch;
    return Result::success();
}

} // namespace cmvr::device