feat: implement SEER Robokit clear fault

add system operational state recovery
2026-08-18 15:04:04 +08:00 · 2026-08-18 15:03:17 +08:00
16 changed files with 1025 additions and 64 deletions
--- a/cmvr-es/devices/agv/seer_robokit/README.md
+++ b/cmvr-es/devices/agv/seer_robokit/README.md
@ -106,7 +106,7 @@ cmake --install build
 | `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` |
 | `19205` | 底盘控制 | `2000`、`2010`、`2022` |
 | `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` |
-| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` |
+| `19207` | 控制权、清错、地图上传下载 | `4005`、`4009`、`4010`、`4011` |
 | `19210` | 开始/停止建图 | `6100`、`6101` |
 | `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 |

@ -126,7 +126,7 @@ gRPC 客户端另做一套租约逻辑。
 | `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 |
 | `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 |
 | `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 |
-| `clearFault` | 未实现 | 返回 `UnsupportedCommand` |
+| `clearFault` | `4009` | 抢权后发送无请求体命令，清除可恢复故障 |
 | `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 |
 | `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 |
 | `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 |
--- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h
+++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h
@ -25,6 +25,7 @@ constexpr std::uint16_t kRobotTaskTranslate = 3055;
 constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
 constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
 constexpr std::uint16_t kRobotConfigLock = 4005;
+constexpr std::uint16_t kRobotConfigClearFault = 4009;
 constexpr std::uint16_t kRobotConfigUploadMap = 4010;
 constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
 constexpr std::uint16_t kRobotOtherStartMapping = 6100;
--- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
+++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
@ -161,9 +161,13 @@ AgvResult SeerRobokitAgv::emergencyStopTrackedNavigation_(

 AgvResult SeerRobokitAgv::clearFault()
 {
-    return AgvResult::failure(
-        AgvErrorCode::UnsupportedCommand,
-        "SEER Robokit clearFault command is not implemented");
+    Json::Value response;
+    auto result = sendControlledCommand_(
+        sock_config_,
+        kRobotConfigClearFault,
+        Json::Value(Json::objectValue),
+        &response);
+    return result.ok() ? resultFromResponse_(response) : result;
 }

 AgvResult SeerRobokitAgv::navigateToPose(
--- a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp
+++ b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp
@ -150,6 +150,7 @@ constexpr std::uint16_t kRobotTaskGoTarget = 3051;
 constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
 constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
 constexpr std::uint16_t kRobotConfigLock = 4005;
+constexpr std::uint16_t kRobotConfigClearFault = 4009;
 constexpr std::uint16_t kRobotConfigUploadMap = 4010;
 constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
 constexpr std::uint16_t kRobotOtherStartMapping = 6100;
@ -784,6 +785,9 @@ TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquire
        [this]() {
        return agv_->emergencyStop();
    });
+    expectControlled(kRobotConfigClearFault, [this]() {
+        return agv_->clearFault();
+    });
    expectControlled(kRobotControlMotion, [this]() {
        return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2});
    });
@ -3114,15 +3118,37 @@ TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggrega
    EXPECT_EQ(records[2].command, kRobotTaskCancel);
 }

-TEST_F(SeerRobokitControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority)
+TEST_F(SeerRobokitControlAuthorityTest, ClearFaultSends4009WithoutPayloadAfterAcquiringAuthority)
 {
    controller_.clearRecords();

    const auto result = agv_->clearFault();

+    ASSERT_TRUE(result.ok()) << result.message;
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 2U);
+    EXPECT_EQ(records[0].command, kRobotConfigLock);
+    EXPECT_EQ(records[1].command, kRobotConfigClearFault);
+    EXPECT_TRUE(records[1].payload.empty());
+}
+
+TEST_F(SeerRobokitControlAuthorityTest, ClearFaultPreservesControllerFailure)
+{
+    controller_.setResponseCode(kRobotConfigClearFault, 44009);
+    controller_.clearRecords();
+
+    const auto result = agv_->clearFault();
+
    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand);
-    EXPECT_TRUE(controller_.records().empty());
+    EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(result.message.find("ret_code=44009"), std::string::npos);
+    EXPECT_NE(
+        result.message.find("simulated command failure"),
+        std::string::npos);
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 2U);
+    EXPECT_EQ(records[0].command, kRobotConfigLock);
+    EXPECT_EQ(records[1].command, kRobotConfigClearFault);
 }

 TEST_F(SeerRobokitControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority)
--- a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp
+++ b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp
@ -55,6 +55,8 @@ using safety::TriState;
 using Probe = std::function<DeviceSafetySnapshot(
    std::uint64_t, std::uint64_t)>;
 using Operation = std::function<ParticipantResult()>;
+using RecoveryOperation =
+    std::function<RecoveryCheckResult(const RecoveryContext&)>;

 TriState tri(const bool value) noexcept
 {
@ -66,6 +68,59 @@ bool agvStopResultAccepted(const AgvResult& result) noexcept
    return result.ok() || result.code == AgvErrorCode::UnsupportedCommand;
 }

+bool armQuiescentForSafety(const ArmState& state) noexcept
+{
+    return state.protective_stopped || state.emergency_stopped ||
+           (!state.moving && !state.program_running);
+}
+
+SafetyReason armRecoveryReason(const ArmErrorCode code) noexcept
+{
+    switch (code) {
+    case ArmErrorCode::NotConnected:
+    case ArmErrorCode::ConnectionFailed:
+        return SafetyReason::DeviceDisconnected;
+    case ArmErrorCode::Timeout:
+        return SafetyReason::ParticipantTimeout;
+    case ArmErrorCode::RobotInEmergencyStop:
+        return SafetyReason::EmergencyStopActive;
+    case ArmErrorCode::RobotInProtectiveStop:
+        return SafetyReason::ProtectiveStopActive;
+    case ArmErrorCode::RobotInFault:
+        return SafetyReason::DeviceFault;
+    case ArmErrorCode::RobotNotReady:
+    case ArmErrorCode::RobotNotPowered:
+        return SafetyReason::DeviceNotReady;
+    case ArmErrorCode::UnsupportedCommand:
+        return SafetyReason::UnsupportedCommand;
+    case ArmErrorCode::OK:
+        return SafetyReason::None;
+    default:
+        return SafetyReason::DeviceFault;
+    }
+}
+
+SafetyReason agvRecoveryReason(const AgvErrorCode code) noexcept
+{
+    switch (code) {
+    case AgvErrorCode::NotConnected:
+    case AgvErrorCode::ConnectionFailed:
+        return SafetyReason::DeviceDisconnected;
+    case AgvErrorCode::Timeout:
+        return SafetyReason::ParticipantTimeout;
+    case AgvErrorCode::EmergencyStopped:
+        return SafetyReason::EmergencyStopActive;
+    case AgvErrorCode::Fault:
+        return SafetyReason::DeviceFault;
+    case AgvErrorCode::UnsupportedCommand:
+        return SafetyReason::UnsupportedCommand;
+    case AgvErrorCode::OK:
+        return SafetyReason::None;
+    default:
+        return SafetyReason::DeviceFault;
+    }
+}
+
 SafetyBlocker blocker(
    const SafetyReason reason,
    const std::string& source)
@ -133,9 +188,13 @@ DeviceSafetyDescriptor defaultDescriptor(const AbstractDevice& device)

 class LegacyDeviceSafetyEndpoint final : public DeviceSafetyEndpoint {
 public:
-    LegacyDeviceSafetyEndpoint(DeviceSafetyDescriptor descriptor, Probe probe)
+    LegacyDeviceSafetyEndpoint(
+        DeviceSafetyDescriptor descriptor,
+        Probe probe,
+        RecoveryOperation operational_recovery)
        : descriptor_(std::move(descriptor)),
          probe_(std::move(probe)),
+          operational_recovery_(std::move(operational_recovery)),
          poll_interval_(std::clamp(
              descriptor_.maximum_snapshot_age / 2,
              std::chrono::milliseconds(50),
@ -272,6 +331,18 @@ public:
        return {true, SafetyReason::None, {}};
    }

+    RecoveryCheckResult restoreOperationalState(
+        const RecoveryContext& context) override
+    {
+        if (!operational_recovery_) {
+            return {
+                false,
+                SafetyReason::UnsupportedCommand,
+                "device does not support operational recovery"};
+        }
+        return operational_recovery_(context);
+    }
+
 private:
    DeviceSafetySnapshot sample_()
    {
@ -334,6 +405,7 @@ private:

    DeviceSafetyDescriptor descriptor_;
    Probe probe_;
+    RecoveryOperation operational_recovery_;
    const std::chrono::milliseconds poll_interval_;
    std::atomic<std::uint64_t> generation_{1};
    std::atomic<std::uint64_t> sequence_{0};
@ -523,8 +595,9 @@ Probe probeFor(const std::shared_ptr<AbstractDevice>& device)
                state.connected && state.powered_on &&
                !state.fault && !state.emergency_stopped &&
                !state.protective_stopped);
-            snapshot.quiescent = tri(!state.moving && !state.program_running);
-            snapshot.motion_active = tri(state.moving || state.program_running);
+            const bool quiescent = armQuiescentForSafety(state);
+            snapshot.quiescent = tri(quiescent);
+            snapshot.motion_active = tri(!quiescent);
            snapshot.actuator_enabled = tri(state.powered_on);
            snapshot.emergency_stop_active = tri(state.emergency_stopped);
            snapshot.protective_stop_active = tri(state.protective_stopped);
@ -571,7 +644,7 @@ Probe probeFor(const std::shared_ptr<AbstractDevice>& device)
            snapshot.motion_active = tri(state.moving || active_navigation);
            snapshot.actuator_enabled = TriState::Unknown;
            snapshot.emergency_stop_active = tri(state.emergency_stopped);
-            snapshot.protective_stop_active = TriState::Unknown;
+            snapshot.protective_stop_active = TriState::False;
            snapshot.fault_active = tri(state.fault);
            snapshot.condition = !state.connected
                ? SafetyCondition::Unknown
@ -599,8 +672,6 @@ Probe probeFor(const std::shared_ptr<AbstractDevice>& device)
                : TriState::Unknown;
            snapshot.motion_active = TriState::Unknown;
            snapshot.actuator_enabled = TriState::Unknown;
-            snapshot.emergency_stop_active = TriState::Unknown;
-            snapshot.protective_stop_active = TriState::Unknown;
            snapshot.fault_active = tri(
                state == AbstractDexHand::Status::FAULT);
            snapshot.condition = !connected
@ -715,8 +786,6 @@ Probe probeFor(const std::shared_ptr<AbstractDevice>& device)
            snapshot.operational_ready = tri(!state.error);
            snapshot.quiescent = TriState::Unknown;
            snapshot.motion_active = TriState::Unknown;
-            snapshot.emergency_stop_active = TriState::Unknown;
-            snapshot.protective_stop_active = TriState::Unknown;
            snapshot.fault_active = tri(state.error);
            snapshot.condition = state.error
                ? SafetyCondition::Unsafe
@ -759,6 +828,42 @@ Probe probeFor(const std::shared_ptr<AbstractDevice>& device)
    };
 }

+RecoveryOperation operationalRecoveryFor(
+    const std::shared_ptr<AbstractDevice>& device)
+{
+    if (const auto arm = std::dynamic_pointer_cast<RobotArm>(device)) {
+        return [arm](const RecoveryContext& context) {
+            if (SafetyClock::now() >= context.deadline) {
+                return RecoveryCheckResult{
+                    false,
+                    SafetyReason::ParticipantTimeout,
+                    "operational recovery deadline expired before clearFault"};
+            }
+            const auto result = arm->clearFault();
+            return RecoveryCheckResult{
+                result.ok(),
+                armRecoveryReason(result.code),
+                result.ok() ? std::string{} : result.message};
+        };
+    }
+    if (const auto agv = std::dynamic_pointer_cast<AbstractAGV>(device)) {
+        return [agv](const RecoveryContext& context) {
+            if (SafetyClock::now() >= context.deadline) {
+                return RecoveryCheckResult{
+                    false,
+                    SafetyReason::ParticipantTimeout,
+                    "operational recovery deadline expired before clearFault"};
+            }
+            const auto result = agv->clearFault();
+            return RecoveryCheckResult{
+                result.ok(),
+                agvRecoveryReason(result.code),
+                result.ok() ? std::string{} : result.message};
+        };
+    }
+    return {};
+}
+
 std::shared_ptr<SafetyParticipant> participantFor(
    const std::shared_ptr<AbstractDevice>& device,
    const DeviceSafetyDescriptor& safety_descriptor,
@ -781,7 +886,7 @@ std::shared_ptr<SafetyParticipant> participantFor(
        };
        verify = [arm] {
            const auto state = arm->getRobotState();
-            const bool stopped = !state.moving && !state.program_running;
+            const bool stopped = armQuiescentForSafety(state);
            return ParticipantResult{
                stopped,
                stopped ? SafetyReason::None
@ -998,7 +1103,9 @@ safety::DeviceSafetyRegistration makeDeviceSafetyRegistration(
    } else {
        registration.descriptor = defaultDescriptor(*device);
        registration.endpoint = std::make_shared<LegacyDeviceSafetyEndpoint>(
-            registration.descriptor, probeFor(device));
+            registration.descriptor,
+            probeFor(device),
+            operationalRecoveryFor(device));
    }

    if (configured_maximum_age > std::chrono::milliseconds::zero()) {
--- a/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h
+++ b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h
@ -26,6 +26,17 @@ public:
    }
    virtual HardwareCheckResult validateBeforeDispatch(
        const AdmissionPermit& permit) = 0;
+    // Explicit operator-authorized recovery which may power or enable the
+    // device. RecoverSafetyState never calls this method; it is reserved for
+    // RestoreOperationalState transactions.
+    virtual RecoveryCheckResult restoreOperationalState(
+        const RecoveryContext&)
+    {
+        return {
+            false,
+            SafetyReason::UnsupportedCommand,
+            "device does not support operational recovery"};
+    }
    virtual RecoveryCheckResult reconcileAdmissionState(
        const RecoveryContext& context) = 0;
 };
--- a/cmvr-es/manager/safety_manager/include/safety_manager.h
+++ b/cmvr-es/manager/safety_manager/include/safety_manager.h
@ -118,11 +118,13 @@ struct RecoveryRequest {
    bool all_devices{false};
    std::uint64_t expected_safety_epoch{0};
    bool verify_only{true};
+    bool restore_operational_state{false};
    std::string reason;
    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
-    // Called only for a latch-clearing transaction, after hardware facts have
-    // been verified and before any software barrier is reconciled or released.
-    // A false result leaves admission latched.
+    // Authorizes the state-changing part of recovery. Operational restore
+    // calls this before invoking device recovery; software-only recovery calls
+    // it after hardware verification and before releasing admission barriers.
+    // A false result leaves admission latched and prevents device recovery.
    std::function<bool()> authorize_clear;
 };

--- a/cmvr-es/manager/safety_manager/src/safety_manager.cpp
+++ b/cmvr-es/manager/safety_manager/src/safety_manager.cpp
@ -1759,7 +1759,10 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
    RecoveryResult invalid;
    invalid.recovery_id = request.recovery_id;
    if (request.recovery_id.empty() || request.reason.empty() ||
-        (!request.all_devices && request.device_ids.empty())) {
+        (!request.all_devices && request.device_ids.empty()) ||
+        (request.restore_operational_state &&
+         (request.verify_only || !request.all_devices ||
+          !request.authorize_clear))) {
        invalid.result = RecoveryResultCode::Failed;
        invalid.targets.push_back({
            "system",
@ -1767,7 +1770,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
            request.reason.empty()
                ? SafetyReason::RecoveryReasonRequired
                : SafetyReason::InvalidArgument,
-            "recovery_id, reason, and an explicit non-empty scope are required"});
+            request.restore_operational_state
+                ? "operational restore requires all_devices, latch clearing, and explicit authorization"
+                : "recovery_id, reason, and an explicit non-empty scope are required"});
        return invalid;
    }
    std::unordered_set<std::string> unique_ids;
@ -1787,7 +1792,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
    std::sort(fingerprint_ids.begin(), fingerprint_ids.end());
    std::ostringstream fingerprint;
    fingerprint << request.expected_safety_epoch << ':'
-                << request.verify_only << ':' << request.all_devices << ':'
+                << request.verify_only << ':'
+                << request.restore_operational_state << ':'
+                << request.all_devices << ':'
                << request.reason;
    for (const auto& id : fingerprint_ids) {
        fingerprint << ':' << id;
@ -1912,7 +1919,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
                request.recovery_id,
                request.verify_only
                    ? "recovery verification started"
-                    : "software latch recovery started");
+                    : request.restore_operational_state
+                          ? "operational state restoration started"
+                          : "software latch recovery started");
        }
    }

@ -1972,6 +1981,101 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
        result.current_safety_epoch,
        deadline,
        request.verify_only};
+    bool clear_authorization_checked = false;
+    bool clear_authorized = true;
+    if (request.restore_operational_state && request.authorize_clear) {
+        clear_authorization_checked = true;
+        try {
+            clear_authorized = request.authorize_clear();
+        } catch (...) {
+            clear_authorized = false;
+        }
+    }
+    if (request.restore_operational_state && !clear_authorized &&
+        targets.empty()) {
+        all_verified = false;
+        result.targets.push_back({
+            "system",
+            false,
+            SafetyReason::RecoveryAuditFailed,
+            "persistent recovery audit did not authorize operational restore"});
+    }
+
+    const auto validate_snapshot = [](
+                                       const SafetySnapshotView& view,
+                                       const SafetyPolicyFamily policy,
+                                       const bool require_operational_ready) {
+        if (!view.fresh) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::SafetyStateStale,
+                "refreshed safety sample is stale"};
+        }
+        if (view.snapshot.condition == SafetyCondition::Unknown) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::SafetyStateMissing,
+                "hardware safety condition remains unknown"};
+        }
+        if (view.snapshot.connected != TriState::True) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::DeviceDisconnected,
+                "device connection is not confirmed"};
+        }
+        if (policy == SafetyPolicyFamily::Control) {
+            if (view.snapshot.emergency_stop_active != TriState::False) {
+                return RecoveryCheckResult{
+                    false,
+                    SafetyReason::EmergencyStopActive,
+                    "emergency stop is active or unknown"};
+            }
+            if (view.snapshot.protective_stop_active != TriState::False) {
+                return RecoveryCheckResult{
+                    false,
+                    SafetyReason::ProtectiveStopActive,
+                    "protective stop is active or unknown"};
+            }
+            if (view.snapshot.fault_active != TriState::False) {
+                return RecoveryCheckResult{
+                    false,
+                    SafetyReason::DeviceFault,
+                    "device fault remains active or unknown"};
+            }
+        } else if (view.snapshot.fault_active == TriState::True) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::DeviceFault,
+                "device fault remains active"};
+        }
+        if (view.snapshot.condition == SafetyCondition::Unsafe) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::HardwareUnsafe,
+                "hardware safety condition remains unsafe"};
+        }
+        if (policy == SafetyPolicyFamily::Control &&
+            view.snapshot.quiescent != TriState::True) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::DeviceStillMoving,
+                "control device has not confirmed a quiescent state"};
+        }
+        if (require_operational_ready &&
+            (view.snapshot.condition != SafetyCondition::Nominal ||
+             view.snapshot.operational_ready != TriState::True)) {
+            return RecoveryCheckResult{
+                false,
+                SafetyReason::DeviceNotReady,
+                "device did not confirm a nominal operational state"};
+        }
+        return RecoveryCheckResult{true, SafetyReason::None, {}};
+    };
+
+    if (request.restore_operational_state) {
+        std::lock_guard lock(impl_->mutex);
+        impl_->active_operation_phase = "restore_devices";
+    }
    for (const auto& target : targets) {
        SafetyTargetResult target_result;
        target_result.target_id = target.id;
@ -1990,37 +2094,81 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
            target_result.reason = SafetyReason::SafetyStateStale;
            target_result.detail =
                "active safety refresh did not publish a newer sample";
-        } else if (!view.fresh) {
-            target_result.reason = SafetyReason::SafetyStateStale;
-            target_result.detail = "refreshed safety sample is stale";
-        } else if (view.snapshot.condition == SafetyCondition::Unknown) {
-            target_result.reason = SafetyReason::SafetyStateMissing;
-            target_result.detail = "hardware safety condition remains unknown";
-        } else if (view.snapshot.condition == SafetyCondition::Unsafe) {
-            target_result.reason = SafetyReason::HardwareUnsafe;
-            target_result.detail = "hardware safety condition remains unsafe";
-        } else if (view.snapshot.connected != TriState::True) {
-            target_result.reason = SafetyReason::DeviceDisconnected;
-            target_result.detail = "device connection is not confirmed";
-        } else if (view.snapshot.emergency_stop_active != TriState::False) {
-            target_result.reason = SafetyReason::EmergencyStopActive;
-            target_result.detail =
-                "emergency stop must be released and observed as false";
-        } else if (view.snapshot.protective_stop_active != TriState::False) {
-            target_result.reason = SafetyReason::ProtectiveStopActive;
-            target_result.detail =
-                "protective stop must be resolved by the typed hardware flow";
-        } else if (view.snapshot.fault_active != TriState::False) {
-            target_result.reason = SafetyReason::DeviceFault;
-            target_result.detail = "device fault remains active or unknown";
-        } else if (target.policy == SafetyPolicyFamily::Control &&
-                   view.snapshot.quiescent != TriState::True) {
-            target_result.reason = SafetyReason::DeviceStillMoving;
-            target_result.detail =
-                "control device has not confirmed a quiescent state";
        } else {
-            target_result.success = true;
-            target_result.reason = SafetyReason::None;
+            RecoveryCheckResult checked;
+            if (!request.restore_operational_state) {
+                checked = validate_snapshot(view, target.policy, false);
+            } else if (!clear_authorized) {
+                checked = {
+                    false,
+                    SafetyReason::RecoveryAuditFailed,
+                    "persistent recovery audit did not authorize operational restore"};
+            } else if (!view.fresh) {
+                checked = {
+                    false,
+                    SafetyReason::SafetyStateStale,
+                    "refreshed safety sample is stale"};
+            } else if (view.snapshot.condition == SafetyCondition::Unknown) {
+                checked = {
+                    false,
+                    SafetyReason::SafetyStateMissing,
+                    "hardware safety condition remains unknown"};
+            } else if (view.snapshot.connected != TriState::True) {
+                checked = {
+                    false,
+                    SafetyReason::DeviceDisconnected,
+                    "device connection is not confirmed"};
+            } else if (target.policy == SafetyPolicyFamily::Control &&
+                       view.snapshot.emergency_stop_active !=
+                           TriState::False) {
+                checked = {
+                    false,
+                    SafetyReason::EmergencyStopActive,
+                    "emergency stop must be released before operational recovery"};
+            } else if (target.policy == SafetyPolicyFamily::Control &&
+                       view.snapshot.quiescent != TriState::True) {
+                checked = {
+                    false,
+                    SafetyReason::DeviceStillMoving,
+                    "control device has not confirmed a quiescent state"};
+            } else {
+                checked = validate_snapshot(view, target.policy, true);
+                if (!checked.reconciled) {
+                    try {
+                        checked = target.endpoint->restoreOperationalState(
+                            recovery_context);
+                    } catch (const std::exception& error) {
+                        checked = {
+                            false, SafetyReason::InternalError, error.what()};
+                    } catch (...) {
+                        checked = {
+                            false,
+                            SafetyReason::InternalError,
+                            "device operational recovery threw an unknown exception"};
+                    }
+                    if (checked.reconciled) {
+                        const auto restored_sequence =
+                            view.snapshot.sample_sequence;
+                        target.endpoint->requestSafetyRefresh();
+                        if (!impl_->snapshots.waitForNewerSample(
+                                target.id,
+                                restored_sequence,
+                                deadline,
+                                view)) {
+                            checked = {
+                                false,
+                                SafetyReason::SafetyStateStale,
+                                "operational recovery did not publish a newer safety sample"};
+                        } else {
+                            checked = validate_snapshot(
+                                view, target.policy, true);
+                        }
+                    }
+                }
+            }
+            target_result.success = checked.reconciled;
+            target_result.reason = checked.reason;
+            target_result.detail = std::move(checked.detail);
        }

        if (!target_result.success) {
@ -2030,7 +2178,10 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
    }

    std::vector<std::string> retained_candidate_ids;
-    if (!request.verify_only) {
+    const bool operational_hardware_verified = all_verified;
+    if (!request.verify_only &&
+        (!request.restore_operational_state ||
+         operational_hardware_verified)) {
        const bool has_device_clear_candidate = std::any_of(
            result.targets.begin(), result.targets.end(),
            [](const auto& target) { return target.success; });
@ -2059,8 +2210,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
        const bool has_clear_candidate =
            has_device_clear_candidate ||
            !retained_candidate_ids.empty();
-        bool clear_authorized = true;
-        if (has_clear_candidate && request.authorize_clear) {
+        if (has_clear_candidate && request.authorize_clear &&
+            !clear_authorization_checked) {
+            clear_authorization_checked = true;
            try {
                clear_authorized = request.authorize_clear();
            } catch (...) {
@ -2270,7 +2422,8 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request)
            if (device == impl_->devices.end()) {
                continue;
            }
-            if (target.success && !request.verify_only) {
+            if (target.success && !request.verify_only &&
+                (!request.restore_operational_state || all_verified)) {
                device->second.quarantined = false;
                device->second.software_blockers.clear();
            }
--- a/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp
+++ b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp
@ -10,10 +10,10 @@
 namespace cmvr::safety {
 namespace {

-DeviceSafetyDescriptor controlDescriptor()
+DeviceSafetyDescriptor controlDescriptor(std::string device_id = "arm")
 {
    DeviceSafetyDescriptor descriptor;
-    descriptor.device_id = "arm";
+    descriptor.device_id = std::move(device_id);
    descriptor.kind = device::DeviceKind::Arm;
    descriptor.default_policy = SafetyPolicyFamily::Control;
    descriptor.maximum_snapshot_age = std::chrono::seconds(1);
@ -75,6 +75,23 @@ public:
        return recovery_check;
    }

+    RecoveryCheckResult restoreOperationalState(
+        const RecoveryContext&) override
+    {
+        ++operational_restores;
+        if (operational_restore_check.reconciled &&
+            restore_to_nominal_on_success) {
+            condition = SafetyCondition::Nominal;
+            connected = TriState::True;
+            ready = TriState::True;
+            quiescent = TriState::True;
+            emergency_stop = TriState::False;
+            protective_stop = TriState::False;
+            fault = TriState::False;
+        }
+        return operational_restore_check;
+    }
+
    DeviceSafetyDescriptor descriptor_;
    SafetySnapshotPublisher publisher_;
    SafetyCondition condition{SafetyCondition::Nominal};
@ -86,11 +103,15 @@ public:
    TriState fault{TriState::False};
    HardwareCheckResult final_check{true, SafetyReason::None, {}};
    RecoveryCheckResult recovery_check{true, SafetyReason::None, {}};
+    RecoveryCheckResult operational_restore_check{
+        true, SafetyReason::None, {}};
    std::uint64_t generation{1};
    std::uint64_t sequence{0};
    bool publish_on_refresh{true};
    std::atomic<int> hardware_checks{0};
    std::atomic<int> recoveries{0};
+    std::atomic<int> operational_restores{0};
+    bool restore_to_nominal_on_success{true};
 };

 class FakeParticipant final : public SafetyParticipant {
@ -466,6 +487,260 @@ TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop)
    EXPECT_EQ(endpoint->recoveries.load(), 0);
 }

+TEST(SafetyManagerTest,
+     OperationalRestoreClearsRecoverableHardwareAndSoftwareState)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Unsafe;
+    endpoint->ready = TriState::False;
+    endpoint->protective_stop = TriState::True;
+    endpoint->quiescent = TriState::True;
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
+    const auto before = coordinator.snapshot();
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-operational";
+    request.all_devices = true;
+    request.expected_safety_epoch = before.safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator confirmed the work cell is clear";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return true; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::Recovered);
+    EXPECT_EQ(restored.system_state, SystemAdmissionState::Open);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_TRUE(restored.targets.front().success)
+        << restored.targets.front().detail;
+    EXPECT_EQ(endpoint->operational_restores.load(), 1);
+    EXPECT_EQ(endpoint->recoveries.load(), 1);
+    EXPECT_EQ(
+        coordinator.snapshot().devices.front().admission_state,
+        DeviceAdmissionState::Open);
+}
+
+TEST(SafetyManagerTest, OperationalRestoreFailsClosedOnActiveEmergencyStop)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Unsafe;
+    endpoint->ready = TriState::False;
+    endpoint->emergency_stop = TriState::True;
+    endpoint->quiescent = TriState::True;
+    endpoint->operational_restore_check = {
+        false,
+        SafetyReason::EmergencyStopActive,
+        "hardware emergency-stop input is still active"};
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
+    const auto before = coordinator.snapshot();
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-active-estop";
+    request.all_devices = true;
+    request.expected_safety_epoch = before.safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return true; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
+    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_FALSE(restored.targets.front().success);
+    EXPECT_EQ(
+        restored.targets.front().reason,
+        SafetyReason::EmergencyStopActive);
+    EXPECT_EQ(endpoint->operational_restores.load(), 0);
+    EXPECT_EQ(endpoint->recoveries.load(), 0);
+}
+
+TEST(SafetyManagerTest, OperationalRestoreDoesNotRecoverAnActiveDevice)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Unsafe;
+    endpoint->ready = TriState::False;
+    endpoint->quiescent = TriState::False;
+    endpoint->fault = TriState::True;
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-active-device";
+    request.all_devices = true;
+    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return true; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_EQ(
+        restored.targets.front().reason,
+        SafetyReason::DeviceStillMoving);
+    EXPECT_EQ(endpoint->operational_restores.load(), 0);
+    EXPECT_EQ(endpoint->recoveries.load(), 0);
+}
+
+TEST(SafetyManagerTest, OperationalRestoreAuditFailurePreventsDeviceRecovery)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Unsafe;
+    endpoint->ready = TriState::False;
+    endpoint->protective_stop = TriState::True;
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-audit-failure";
+    request.all_devices = true;
+    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return false; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_EQ(
+        restored.targets.front().reason,
+        SafetyReason::RecoveryAuditFailed);
+    EXPECT_EQ(endpoint->operational_restores.load(), 0);
+    EXPECT_EQ(endpoint->recoveries.load(), 0);
+}
+
+TEST(SafetyManagerTest, OperationalRestoreRequiresExplicitAuthorization)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.markStartupComplete();
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-without-authorization";
+    request.all_devices = true;
+    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::Failed);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_EQ(
+        restored.targets.front().reason,
+        SafetyReason::InvalidArgument);
+    EXPECT_EQ(endpoint->operational_restores.load(), 0);
+}
+
+TEST(SafetyManagerTest,
+     OperationalRestoreAuditFailureWithNoDevicesRemainsLatched)
+{
+    SafetyManager coordinator;
+    coordinator.markStartupComplete();
+
+    RecoveryRequest request;
+    request.recovery_id = "empty-restore-audit-failure";
+    request.all_devices = true;
+    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return false; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
+    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
+    ASSERT_EQ(restored.targets.size(), 1U);
+    EXPECT_EQ(restored.targets.front().target_id, "system");
+    EXPECT_EQ(
+        restored.targets.front().reason,
+        SafetyReason::RecoveryAuditFailed);
+}
+
+TEST(SafetyManagerTest,
+     OperationalRestoreDoesNotPartiallyReleaseSoftwareAdmission)
+{
+    SafetyManager coordinator;
+    auto first = std::make_shared<FakeEndpoint>(
+        controlDescriptor("arm-first"));
+    first->condition = SafetyCondition::Unsafe;
+    first->ready = TriState::False;
+    first->protective_stop = TriState::True;
+    auto second = std::make_shared<FakeEndpoint>(
+        controlDescriptor("arm-second"));
+    second->condition = SafetyCondition::Unsafe;
+    second->ready = TriState::False;
+    second->fault = TriState::True;
+    second->operational_restore_check = {
+        false, SafetyReason::DeviceFault, "fault reset failed"};
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor("arm-first"), first, {}}));
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor("arm-second"), second, {}}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice(
+        "arm-first", SafetyReason::OutcomeUnknown);
+    coordinator.quarantineDevice(
+        "arm-second", SafetyReason::OutcomeUnknown);
+
+    RecoveryRequest request;
+    request.recovery_id = "restore-partial-failure";
+    request.all_devices = true;
+    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
+    request.verify_only = false;
+    request.restore_operational_state = true;
+    request.reason = "operator requested recovery";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return true; };
+
+    const auto restored = coordinator.recover(request);
+
+    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
+    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
+    EXPECT_EQ(first->operational_restores.load(), 1);
+    EXPECT_EQ(second->operational_restores.load(), 1);
+    EXPECT_EQ(first->recoveries.load(), 0);
+    EXPECT_EQ(second->recoveries.load(), 0);
+    const auto snapshot = coordinator.snapshot();
+    ASSERT_EQ(snapshot.devices.size(), 2U);
+    for (const auto& device : snapshot.devices) {
+        EXPECT_EQ(
+            device.admission_state,
+            DeviceAdmissionState::Quarantined)
+            << device.descriptor.device_id;
+    }
+}
+
 TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch)
 {
    SafetyManager coordinator;
--- a/cmvr-es/service/grpc/server/include/grpc_system_service.h
+++ b/cmvr-es/service/grpc/server/include/grpc_system_service.h
@ -50,6 +50,7 @@ namespace cmvr::service
        grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override;
        grpc::Status GetSafetyState(grpc::ServerContext* context, const cmvr::api::GetSafetyStateCommand_Request* request, cmvr::api::GetSafetyStateCommand_Feedback* response) override;
        grpc::Status RecoverSafetyState(grpc::ServerContext* context, const cmvr::api::RecoverSafetyStateCommand_Request* request, cmvr::api::RecoverSafetyStateCommand_Feedback* response) override;
+        grpc::Status RestoreOperationalState(grpc::ServerContext* context, const cmvr::api::RestoreOperationalStateCommand_Request* request, cmvr::api::RestoreOperationalStateCommand_Feedback* response) override;
    private:
        device::DeviceManager& dmgr_;
        const std::chrono::milliseconds stop_timeout_;
--- a/cmvr-es/service/grpc/server/src/grpc_security.cpp
+++ b/cmvr-es/service/grpc/server/src/grpc_security.cpp
@ -419,6 +419,9 @@ const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry()
        add("SystemService", "RecoverSafetyState", GrpcAccessClass::Recover,
            CommandIntent::RecoverAdmission, SafetyPolicyFamily::Control,
            true, true);
+        add("SystemService", "RestoreOperationalState",
+            GrpcAccessClass::Recover, CommandIntent::RecoverAdmission,
+            SafetyPolicyFamily::Control, true, true);

        add_many("ArmService",
                 {"getJointState", "getPose", "getPoseMatrix",
--- a/cmvr-es/service/grpc/server/src/grpc_system_service.cpp
+++ b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp
@ -1575,6 +1575,166 @@ grpc::Status gRPCSystemServiceImpl::RecoverSafetyState(
    return grpc::Status::OK;
 }

+grpc::Status gRPCSystemServiceImpl::RestoreOperationalState(
+    grpc::ServerContext* context,
+    const cmvr::api::RestoreOperationalStateCommand_Request* request,
+    cmvr::api::RestoreOperationalStateCommand_Feedback* response)
+{
+    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
+        security_gateway_, context,
+        "/cmvr.api.SystemService/RestoreOperationalState");
+    if (!request || !response) {
+        return grpc::Status(
+            grpc::StatusCode::INVALID_ARGUMENT,
+            "RestoreOperationalState request and response are required");
+    }
+    if (request->recovery_id().empty() || request->reason().empty()) {
+        const std::string detail =
+            "recovery_id and reason are required";
+        setSafetyHeaderFailure(
+            response->mutable_header(),
+            request->reason().empty()
+                ? cmvr::safety::SafetyReason::RecoveryReasonRequired
+                : cmvr::safety::SafetyReason::InvalidArgument,
+            detail);
+        return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, detail);
+    }
+    if (!recovery_audit_sink_) {
+        const std::string detail =
+            "persistent recovery audit is not configured";
+        setSafetyHeaderFailure(
+            response->mutable_header(),
+            cmvr::safety::SafetyReason::RecoveryAuditFailed,
+            detail);
+        return grpc::Status(
+            grpc::StatusCode::FAILED_PRECONDITION,
+            "RECOVERY_AUDIT_FAILED");
+    }
+
+    const auto accepted_snapshot = dmgr_.safetyManager().snapshot();
+    RecoveryAuditRecord audit;
+    audit.occurred_at_unix_ms = unixTimeMs();
+    audit.stage = "accepted";
+    audit.correlation_id = cmvr_grpc_call_guard.context().correlation_id;
+    audit.principal_id = cmvr_grpc_call_guard.context().principal.id;
+    audit.peer = cmvr_grpc_call_guard.context().peer;
+    audit.recovery_id = request->recovery_id();
+    audit.reason = request->reason();
+    audit.mode = "restore_operational_state";
+    audit.all_devices = true;
+    audit.expected_safety_epoch = accepted_snapshot.safety_epoch;
+    audit.result = "pending";
+    std::string audit_error;
+    if (!recovery_audit_sink_->append(audit, &audit_error)) {
+        const auto detail = audit_error.empty()
+            ? std::string("persistent recovery audit write failed")
+            : audit_error;
+        setSafetyHeaderFailure(
+            response->mutable_header(),
+            cmvr::safety::SafetyReason::RecoveryAuditFailed,
+            detail);
+        return grpc::Status(
+            grpc::StatusCode::FAILED_PRECONDITION,
+            "RECOVERY_AUDIT_FAILED");
+    }
+
+    auto deadline = cmvr_grpc_call_guard.context().deadline;
+    const auto configured_deadline =
+        cmvr::safety::SafetyClock::now() +
+        dmgr_.safetyManager().config().recovery_timeout;
+    if (deadline == cmvr::safety::SafetyClock::time_point::max() ||
+        configured_deadline < deadline) {
+        deadline = configured_deadline;
+    }
+    if (request->timeout_ms() != 0) {
+        deadline = std::min(
+            deadline,
+            cmvr::safety::SafetyClock::now() +
+                std::chrono::milliseconds(request->timeout_ms()));
+    }
+
+    auto commit_audit = audit;
+    commit_audit.stage = "restore_commit";
+    commit_audit.result = "authorized";
+    const auto sink = recovery_audit_sink_;
+
+    cmvr::safety::RecoveryRequest coordinator_request;
+    coordinator_request.recovery_id = request->recovery_id();
+    coordinator_request.all_devices = true;
+    coordinator_request.expected_safety_epoch =
+        accepted_snapshot.safety_epoch;
+    coordinator_request.verify_only = false;
+    coordinator_request.restore_operational_state = true;
+    coordinator_request.reason = request->reason();
+    coordinator_request.deadline = deadline;
+    coordinator_request.authorize_clear =
+        [sink, commit_audit = std::move(commit_audit)]() mutable {
+            commit_audit.occurred_at_unix_ms = unixTimeMs();
+            std::string error;
+            const bool persisted = sink->append(commit_audit, &error);
+            if (!persisted) {
+                CMVR_LOG(ERROR)
+                    << "[gRPCSystemServiceImpl] Operational restore audit "
+                       "failed: "
+                    << error;
+            }
+            return persisted;
+        };
+
+    const auto result =
+        dmgr_.safetyManager().recover(coordinator_request);
+    response->set_recovery_id(result.recovery_id);
+    response->set_result(toApiRecoveryResult(result.result));
+    response->set_previous_safety_epoch(result.previous_safety_epoch);
+    response->set_current_safety_epoch(result.current_safety_epoch);
+    response->set_system_state(
+        toApiSystemAdmissionState(result.system_state));
+    for (const auto& target : result.targets) {
+        populateSafetyTargetResult(target, *response->add_targets());
+    }
+
+    const bool success = recoveryCompletedAsRequested(result.result);
+    auto* header = response->mutable_header();
+    header->set_success(success);
+    header->set_command_id(request->recovery_id());
+    header->set_service_instance_id(
+        dmgr_.safetyManager().serviceInstanceId());
+    header->set_safety_epoch(result.current_safety_epoch);
+    header->set_execution_state(
+        success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED
+                : cmvr::api::COMMAND_EXECUTION_STATE_FAILED);
+    if (success) {
+        header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE);
+    } else {
+        const auto failed = std::find_if(
+            result.targets.begin(), result.targets.end(),
+            [](const auto& target) { return !target.success; });
+        if (failed != result.targets.end()) {
+            header->set_reason_code(toApiSafetyReason(failed->reason));
+            header->set_error_message(failed->detail);
+        } else {
+            header->set_reason_code(
+                cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR);
+            header->set_error_message(
+                "operational restore failed without a target result");
+        }
+    }
+    setCurrentTimestamp(header->mutable_timestamp());
+
+    audit.occurred_at_unix_ms = unixTimeMs();
+    audit.stage = "completed";
+    audit.previous_safety_epoch = result.previous_safety_epoch;
+    audit.current_safety_epoch = result.current_safety_epoch;
+    audit.result = cmvr::safety::toString(result.result);
+    if (!recovery_audit_sink_->append(audit, &audit_error)) {
+        CMVR_LOG(ERROR)
+            << "[gRPCSystemServiceImpl] Operational restore completion audit "
+               "failed: "
+            << audit_error;
+    }
+    return grpc::Status::OK;
+}
+
 grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response)
 {
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
--- a/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp
+++ b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp
@ -646,7 +646,13 @@ public:
        return model;
    }
    std::size_t getDof() const override { return kDof; }
-    device::ArmState getRobotState() const override { return {}; }
+    device::ArmState getRobotState() const override
+    {
+        std::lock_guard lock(mutex_);
+        auto state = reported_state_;
+        state.moving = state.moving || active_motions_ != 0;
+        return state;
+    }
    device::JointGroupState getJointState() const override { return {}; }
    device::CartesianPose getTcpPose(
        device::FrameType = device::FrameType::Base) const override
@ -793,7 +799,19 @@ public:
        shutdown_calls_.fetch_add(1, std::memory_order_relaxed);
        return device::Result::success();
    }
-    device::Result clearFault() override { return device::Result::success(); }
+    device::Result clearFault() override
+    {
+        std::lock_guard lock(mutex_);
+        ++clear_fault_calls_;
+        reported_state_.connected = true;
+        reported_state_.powered_on = true;
+        reported_state_.moving = false;
+        reported_state_.program_running = false;
+        reported_state_.fault = false;
+        reported_state_.emergency_stopped = false;
+        reported_state_.protective_stopped = false;
+        return device::Result::success();
+    }
    device::Result unlockProtectiveStop() override
    {
        return device::Result::success();
@ -949,6 +967,18 @@ public:
        return shutdown_calls_.load(std::memory_order_relaxed);
    }

+    int clearFaultCalls() const
+    {
+        std::lock_guard lock(mutex_);
+        return clear_fault_calls_;
+    }
+
+    void setReportedState(const device::ArmState& state)
+    {
+        std::lock_guard lock(mutex_);
+        reported_state_ = state;
+    }
+
    void blockHealthSnapshot()
    {
        std::lock_guard lock(health_mutex_);
@ -1096,6 +1126,8 @@ private:
    int health_snapshot_calls_{0};
    bool block_health_snapshot_{false};
    bool health_snapshot_entered_{false};
+    device::ArmState reported_state_{};
+    int clear_fault_calls_{0};
 };

 class ActionTestAgv final : public device::AbstractAGV {
@ -1342,6 +1374,18 @@ const api::SafetyOperationTargetResult* findSafetyTarget(
    return nullptr;
 }

+const api::SafetyOperationTargetResult* findSafetyTarget(
+    const api::RestoreOperationalStateCommand_Feedback& response,
+    const std::string& id)
+{
+    for (const auto& target : response.targets()) {
+        if (target.target_id() == id) {
+            return &target;
+        }
+    }
+    return nullptr;
+}
+
 class GrpcSystemServiceTest : public ::testing::Test {
 protected:
    void SetUp() override
@ -1635,6 +1679,25 @@ TEST_F(GrpcSystemServiceTest, RecoveryIsDisabledByDefault)
    EXPECT_EQ(status.error_message(), "RECOVERY_RPC_DISABLED");
 }

+TEST_F(GrpcSystemServiceTest, OperationalRestoreIsDisabledByDefault)
+{
+    config::DeviceManagerConfig config;
+    (void)device::DeviceManager::getInstance(config);
+    service_ = std::make_unique<gRPCSystemServiceImpl>();
+
+    api::RestoreOperationalStateCommand_Request request;
+    request.set_recovery_id("restore-disabled");
+    request.set_reason("operator requested recovery");
+    api::RestoreOperationalStateCommand_Feedback response;
+    grpc::ServerContext context;
+
+    const auto status = service_->RestoreOperationalState(
+        &context, &request, &response);
+
+    EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(status.error_message(), "RECOVERY_RPC_DISABLED");
+}
+
 TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator)
 {
    config::DeviceManagerConfig config;
@ -1682,6 +1745,64 @@ TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator)
        epoch_before_failure);
 }

+TEST_F(GrpcSystemServiceTest,
+       RestoreOperationalStateRecoversArmAndClearsAdmission)
+{
+    config::DeviceManagerConfig config;
+    auto& manager = device::DeviceManager::getInstance(config);
+    action_trace_ = std::make_shared<ActionTrace>();
+    action_arm_ = std::make_shared<ActionTestArm>(
+        "restore-arm", action_trace_);
+    device::ArmState protected_state;
+    protected_state.connected = true;
+    protected_state.powered_on = true;
+    protected_state.moving = true;
+    protected_state.program_running = true;
+    protected_state.protective_stopped = true;
+    action_arm_->setReportedState(protected_state);
+    manager.registerDevice(action_arm_);
+    manager.safetyManager().quarantineDevice(
+        action_arm_->id(), safety::SafetyReason::OutcomeUnknown);
+
+    auto audit = std::make_shared<MemoryRecoveryAuditSink>();
+    service_ = std::make_unique<gRPCSystemServiceImpl>(
+        std::chrono::seconds(1), makeAllowAllRecoveryGateway(), audit);
+
+    api::RestoreOperationalStateCommand_Request request;
+    request.set_recovery_id("restore-whole-system");
+    request.set_reason("operator confirmed the work cell is clear");
+    request.set_timeout_ms(1000);
+    api::RestoreOperationalStateCommand_Feedback response;
+    grpc::ServerContext context;
+
+    const auto status = service_->RestoreOperationalState(
+        &context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    ASSERT_TRUE(response.header().success())
+        << response.header().error_message();
+    EXPECT_EQ(
+        response.result(), api::SAFETY_OPERATION_RESULT_RECOVERED);
+    EXPECT_EQ(
+        response.system_state(), api::SYSTEM_ADMISSION_STATE_OPEN);
+    EXPECT_EQ(action_arm_->clearFaultCalls(), 1);
+    const auto* arm_target = findSafetyTarget(response, action_arm_->id());
+    ASSERT_NE(arm_target, nullptr);
+    EXPECT_EQ(
+        arm_target->result(), api::SAFETY_OPERATION_RESULT_SUCCEEDED);
+    ASSERT_EQ(audit->records.size(), 3U);
+    EXPECT_EQ(audit->records[0].stage, "accepted");
+    EXPECT_EQ(audit->records[1].stage, "restore_commit");
+    EXPECT_EQ(audit->records[2].stage, "completed");
+
+    const auto snapshot = manager.safetyManager().snapshot();
+    EXPECT_EQ(snapshot.system_state, safety::SystemAdmissionState::Open);
+    ASSERT_EQ(snapshot.devices.size(), 1U);
+    EXPECT_EQ(
+        snapshot.devices.front().admission_state,
+        safety::DeviceAdmissionState::Open);
+}
+
 TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind)
 {
    struct ExpectedMapping {
@ -2956,6 +3077,62 @@ TEST_F(GrpcSystemServiceTest,
    EXPECT_FALSE(lease.acquired);
 }

+TEST_F(GrpcSystemServiceTest,
+       StopAllAcceptsProtectedArmAsQuiescent)
+{
+    initializeActionDevices();
+    device::ArmState protected_state;
+    protected_state.connected = true;
+    protected_state.powered_on = true;
+    protected_state.moving = true;
+    protected_state.program_running = true;
+    protected_state.protective_stopped = true;
+    action_arm_->setReportedState(protected_state);
+
+    api::StopAllCommand_Request request;
+    api::StopAllCommand_Feedback response;
+    grpc::ServerContext context;
+    const auto status = service_->StopAll(
+        &context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_TRUE(response.header().success())
+        << response.header().error_message();
+    const auto* arm_target = findSafetyTarget(response, action_arm_->id());
+    ASSERT_NE(arm_target, nullptr);
+    EXPECT_EQ(
+        arm_target->result(), api::SAFETY_OPERATION_RESULT_SUCCEEDED);
+}
+
+TEST_F(GrpcSystemServiceTest,
+       StopAllDoesNotTreatGenericArmFaultAsQuiescent)
+{
+    initializeActionDevices();
+    device::ArmState fault_state;
+    fault_state.connected = true;
+    fault_state.powered_on = true;
+    fault_state.moving = true;
+    fault_state.program_running = true;
+    fault_state.fault = true;
+    action_arm_->setReportedState(fault_state);
+
+    api::StopAllCommand_Request request;
+    api::StopAllCommand_Feedback response;
+    grpc::ServerContext context;
+    const auto status = service_->StopAll(
+        &context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_FALSE(response.header().success());
+    const auto* arm_target = findSafetyTarget(response, action_arm_->id());
+    ASSERT_NE(arm_target, nullptr);
+    EXPECT_EQ(
+        arm_target->result(), api::SAFETY_OPERATION_RESULT_FAILED);
+    EXPECT_EQ(
+        arm_target->reason_code(),
+        api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING);
+}
+
 TEST_F(GrpcSystemServiceTest,
       StopAllStopsActiveMediaWithoutStoppingDeviceLifecycles)
 {
--- a/docs/device_safety_control_plane_architecture.md
+++ b/docs/device_safety_control_plane_architecture.md
@ -535,6 +535,27 @@ struct AdmissionPermit {
  不能把它登记成 ResetFault；
 - ResetFault 成功只触发安全快照刷新，不直接清除 central quarantine。

+### 8.2.1 整机运行态恢复
+
+`SystemService.RestoreOperationalState` 是面向上层平台的整机恢复事务，与
+`RecoverSafetyState` 的软件准入恢复语义分离：
+
+- 请求必须携带唯一 `recovery_id`、非空 `reason`，并通过与 Recover 相同的
+  `RecoveryExposure` 和持久审计策略；
+- 事务固定覆盖全部已注册设备，并与 StopAll、其他恢复事务通过 process operation mutex
+  串行；事务期间系统处于 Recovering，拒绝新的普通运动命令；
+- 硬件恢复前必须主动取得新鲜快照，确认设备已连接；Control 设备还必须确认物理急停已释放
+  且已经静止。条件未知、急停仍有效、设备仍运动或审计授权失败时，不调用设备恢复能力；
+- 对尚未达到 Nominal/operational-ready 的支持设备调用
+  `DeviceSafetyEndpoint::restoreOperationalState()`。Legacy Arm 和 AGV adapter 分别调用设备
+  `clearFault()`；该操作允许按设备实现执行上电、使能和故障复位，但不得恢复旧轨迹、导航、
+  teleop session、control lease 或 ActionQueue；
+- 每个硬件恢复动作完成后必须再次主动刷新，确认连接、新鲜度、急停、保护停、fault、静止和
+  operational-ready。任一设备失败时系统保持 Latched，并返回逐设备失败原因；中央 quarantine、
+  retained barrier 和全局 gate 不做部分释放；
+- 只有全部必需设备验证成功后，才协调释放软件 latch 和 retained barrier，将系统切回 Open。
+  调用成功只表示整机已使能、空闲并可接受新的 gRPC 命令，不会续跑中断前的任务。
+
 ### 8.3 流式控制

 Teleoperation、Motor cyclic stream 和未来连续控制不为每个 setpoint 写 unary ledger：
@ -1092,7 +1113,8 @@ public:
 `CommandIntent` 和最低 role。未知方法在 Authenticated Profile 中 fail-closed；Compatibility
 Profile 可以只为已有 RPC 保留当前行为，但仍必须产生 `unclassified_method` 告警并在阶段 2 前清零。

-`RecoverSafetyState` 额外使用独立的 `RecoveryExposure`：
+`RecoverSafetyState` 和 `RestoreOperationalState` 额外使用独立的
+`RecoveryExposure`：

 | RecoveryExposure | 行为 |
 | --- | --- |
--- a/protos/cmvr/api/safety_command.proto
+++ b/protos/cmvr/api/safety_command.proto
@ -184,3 +184,21 @@ message RecoverSafetyStateCommand {
        string recovery_id = 7;
    }
 }
+
+message RestoreOperationalStateCommand {
+    message Request {
+        string recovery_id = 1;
+        string reason = 2;
+        uint32 timeout_ms = 3;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        SafetyOperationResult result = 2;
+        uint64 previous_safety_epoch = 3;
+        uint64 current_safety_epoch = 4;
+        SystemAdmissionState system_state = 5;
+        repeated SafetyOperationTargetResult targets = 6;
+        string recovery_id = 7;
+    }
+}
--- a/protos/cmvr/api/system_service.proto
+++ b/protos/cmvr/api/system_service.proto
@ -19,4 +19,5 @@ service SystemService {

    rpc GetSafetyState(GetSafetyStateCommand.Request) returns (GetSafetyStateCommand.Feedback) {}
    rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) returns (RecoverSafetyStateCommand.Feedback) {}
+    rpc RestoreOperationalState(RestoreOperationalStateCommand.Request) returns (RestoreOperationalStateCommand.Feedback) {}
 }
Author	SHA1	Message	Date
xtkuang	f611ad1db8	feat: implement SEER Robokit clear fault	2026-08-18 15:04:04 +08:00
xtkuang	8ac1bff099	add system operational state recovery	2026-08-18 15:03:17 +08:00