diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 4c0da356..78eca49a 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -3321,6 +3321,19 @@ Result AuboArm::clearFault() } std::unique_lock command_rpc_lock( monitor->command_rpc_mutex); + const auto recover_with_torque_on = + [this, &command_rpc_lock]() -> Result { + if (command_rpc_lock.owns_lock()) { + command_rpc_lock.unlock(); + } + auto recovery = torqueOn(); + if (!recovery.ok()) { + recovery.message = + "[AuboArm] clearFault recovery failed: " + + recovery.message; + } + return recovery; + }; Result interface_result; auto robot_interface = getPrimaryRobotInterface( rpc_client, "clearFault", interface_result); @@ -3330,18 +3343,15 @@ Result AuboArm::clearFault() refreshSafetySample(rpc_client, monitor, robot_interface); auto snapshot = monitor->safety_state->snapshot(); const std::uint64_t expected_safety_epoch = snapshot.epoch; + const int entry_robot_mode = monitor->robot_mode.load(); if (!snapshot.latched && aubo_internal::isMotionSafe(snapshot.observed) && - monitor->robot_mode.load() != - static_cast(RobotModeType::Error)) { - return Result::success(); - } - if (!snapshot.latched && - monitor->robot_mode.load() == - static_cast(RobotModeType::Error)) { - return Result::failure( - ArmErrorCode::RobotInFault, - "[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced"); + entry_robot_mode != static_cast(RobotModeType::Error)) { + if (entry_robot_mode == + static_cast(RobotModeType::Running)) { + return Result::success(); + } + return recover_with_torque_on(); } if (monitor->emergency_stop_source.load() != 0) { return Result::failure( @@ -3353,22 +3363,38 @@ Result AuboArm::clearFault() aubo_internal::needsProtectiveUnlock( snapshot.latched_reason)) { command_rpc_lock.unlock(); - return unlockProtectiveStop_(expected_safety_epoch); + auto unlock_result = + unlockProtectiveStop_(expected_safety_epoch); + if (unlock_result.ok()) { + return monitor->robot_mode.load() == + static_cast(RobotModeType::Running) + ? Result::success() + : recover_with_torque_on(); + } + if (unlock_result.code == ArmErrorCode::RobotNotPowered) { + return recover_with_torque_on(); + } + return unlock_result; } - if (aubo_internal::isMotionSafe(snapshot.observed)) { + const bool controller_fault_requires_restart = + aubo_internal::isMotionSafe(snapshot.observed) && + monitor->robot_mode.load() == + static_cast(RobotModeType::Error); + + if (aubo_internal::isMotionSafe(snapshot.observed) && + !controller_fault_requires_restart) { if (monitor->robot_mode.load() == static_cast(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"); + return recover_with_torque_on(); } - if (!aubo_internal::needsInterfaceBoardRestart( + if (!controller_fault_requires_restart && + !aubo_internal::needsInterfaceBoardRestart( snapshot.observed)) { const std::string guidance = snapshot.observed == @@ -3415,9 +3441,7 @@ Result AuboArm::clearFault() 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"); + return recover_with_torque_on(); } catch (const std::exception& e) { return Result::failure( ArmErrorCode::CommandFailed,