add clip

2026-07-27 18:39:19 +08:00
parent b957c19eed
commit ebbe9d5432
4 changed files with 160 additions and 24 deletions
--- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
+++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
@@ -160,6 +160,25 @@ def motor_servo_fault(state):
    return None


+class MotorServoGuard:
+    def __init__(self, fault_frames):
+        self.fault_frames = max(1, int(fault_frames))
+        self.consecutive_bad = 0
+
+    def reset(self):
+        self.consecutive_bad = 0
+
+    def update(self, state):
+        reason = motor_servo_fault(state)
+        if reason is None:
+            self.reset()
+            return None
+        self.consecutive_bad += 1
+        if self.consecutive_bad >= self.fault_frames:
+            return f"{reason} ({self.consecutive_bad} consecutive frames)"
+        return None
+
+
 def validate_joint_order():
    sdk_names = list(JOINT_NAMES)
    if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -525,8 +544,14 @@ def action_ok(action_raw, args):
    if not np.all(np.isfinite(action_raw)):
        return False, "action is non-finite"
    max_abs = float(np.max(np.abs(action_raw)))
+    hard_limit = float(args.action_hard_trip_limit)
+    if hard_limit > 0 and max_abs > hard_limit:
+        return False, f"action abs {max_abs:.2f} > hard trip {hard_limit:.2f}"
    if max_abs > args.action_trip_limit:
-        return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.2f}"
+        return True, (
+            f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; "
+            f"clipped to {args.action_clip:.2f}"
+        )
    return True, "ok"


@@ -846,6 +871,7 @@ def run_deploy(args):
        edge.update(state)
        obs_builder = ObsHistoryBuilder()
        cmd_filter = CommandFilter(args)
+        servo_guard = MotorServoGuard(args.motor_mode_fault_frames)

        sm_state = State.IDLE
        last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
@@ -880,8 +906,15 @@ def run_deploy(args):
                prev_action[:] = 0.0
                prev_target = DEFAULT_DOF_POS.copy()
                rl_step = 0
+                servo_guard.reset()
                send_damping(client)

+            if sm_state in (State.HOLD, State.OBS_TEST, State.INFER_TEST, State.RL):
+                servo_fault = servo_guard.update(state)
+            else:
+                servo_guard.reset()
+                servo_fault = None
+
            cmd_raw = get_command(state, args)
            if sm_state in (State.OBS_TEST, State.INFER_TEST, State.RL):
                cmd = cmd_filter.update(cmd_raw)
@@ -912,7 +945,6 @@ def run_deploy(args):
            elif sm_state == State.HOLD:
                send_hold_cmd(client, state, args)
                if r2_rose:
-                    servo_fault = motor_servo_fault(state)
                    if servo_fault:
                        reason = servo_fault
                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
@@ -930,7 +962,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -951,7 +982,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -995,7 +1025,6 @@ def run_deploy(args):

                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1157,7 +1186,10 @@ def build_arg_parser():
    parser.add_argument("--max-pitch-deg", type=float, default=35.0)
    parser.add_argument("--max-dof-vel", type=float, default=30.0)
    parser.add_argument("--max-gyro", type=float, default=15.0)
-    parser.add_argument("--action-trip-limit", type=float, default=12.0)
+    parser.add_argument("--action-trip-limit", type=float, default=12.0,
+                        help="Soft raw-action warning threshold; output is still clipped to --action-clip")
+    parser.add_argument("--action-hard-trip-limit", type=float, default=16.0,
+                        help="Hard raw-action fault threshold; <=0 disables the hard trip")
    parser.add_argument("--action-clip", type=float, default=4.0)
    parser.add_argument("--action-ema-alpha", type=float, default=0.0)
    parser.add_argument("--max-target-step", type=float, default=0.08)
@@ -1165,6 +1197,8 @@ def build_arg_parser():

    parser.add_argument("--log-dir", default=str(HERE / "logs"), help="JSONL log directory; empty disables logging")
    parser.add_argument("--log-flush-every", type=int, default=50)
+    parser.add_argument("--motor-mode-fault-frames", type=int, default=3,
+                        help="Consecutive non-servo feedback frames required before a motor-mode fault")
    return parser


--- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py
+++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py
@@ -174,6 +174,25 @@ def motor_servo_fault(state):
    return None


+class MotorServoGuard:
+    def __init__(self, fault_frames):
+        self.fault_frames = max(1, int(fault_frames))
+        self.consecutive_bad = 0
+
+    def reset(self):
+        self.consecutive_bad = 0
+
+    def update(self, state):
+        reason = motor_servo_fault(state)
+        if reason is None:
+            self.reset()
+            return None
+        self.consecutive_bad += 1
+        if self.consecutive_bad >= self.fault_frames:
+            return f"{reason} ({self.consecutive_bad} consecutive frames)"
+        return None
+
+
 def validate_joint_order():
    sdk_names = list(JOINT_NAMES)
    if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -516,8 +535,14 @@ def action_ok(action_raw, args):
    if not np.all(np.isfinite(action_raw)):
        return False, "action is non-finite"
    max_abs = float(np.max(np.abs(action_raw)))
+    hard_limit = float(args.action_hard_trip_limit)
+    if hard_limit > 0 and max_abs > hard_limit:
+        return False, f"action abs {max_abs:.2f} > hard trip {hard_limit:.2f}"
    if max_abs > args.action_trip_limit:
-        return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.2f}"
+        return True, (
+            f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; "
+            f"clipped to {args.action_clip:.2f}"
+        )
    return True, "ok"


@@ -843,6 +868,7 @@ def run_deploy(args):
        edge.update(state)
        obs_builder = ObsHistoryBuilder()
        cmd_filter = CommandFilter(args)
+        servo_guard = MotorServoGuard(args.motor_mode_fault_frames)

        sm_state = State.IDLE
        last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
@@ -892,8 +918,15 @@ def run_deploy(args):
                prev_action[:] = 0.0
                prev_target = DEFAULT_DOF_POS.copy()
                rl_step = 0
+                servo_guard.reset()
                send_damping(client)

+            if sm_state in (State.HOLD, State.OBS_TEST, State.INFER_TEST, State.RL):
+                servo_fault = servo_guard.update(state)
+            else:
+                servo_guard.reset()
+                servo_fault = None
+
            cmd_raw = get_command(state, args)
            if sm_state in (State.OBS_TEST, State.INFER_TEST, State.RL):
                cmd = cmd_filter.update(cmd_raw)
@@ -924,7 +957,6 @@ def run_deploy(args):
            elif sm_state == State.HOLD:
                send_hold_cmd(client, state, args)
                if r2_rose:
-                    servo_fault = motor_servo_fault(state)
                    if servo_fault:
                        reason = servo_fault
                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
@@ -942,7 +974,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -963,7 +994,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1010,7 +1040,6 @@ def run_deploy(args):

                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1179,7 +1208,10 @@ def build_arg_parser():
    parser.add_argument("--max-pitch-deg", type=float, default=35.0)
    parser.add_argument("--max-dof-vel", type=float, default=30.0)
    parser.add_argument("--max-gyro", type=float, default=15.0)
-    parser.add_argument("--action-trip-limit", type=float, default=12.0)
+    parser.add_argument("--action-trip-limit", type=float, default=12.0,
+                        help="Soft raw-action warning threshold; output is still clipped to --action-clip")
+    parser.add_argument("--action-hard-trip-limit", type=float, default=16.0,
+                        help="Hard raw-action fault threshold; <=0 disables the hard trip")
    parser.add_argument("--action-clip", type=float, default=4.0)
    parser.add_argument("--action-ema-alpha", type=float, default=0.0)
    parser.add_argument("--max-target-step", type=float, default=0.08)
@@ -1187,6 +1219,8 @@ def build_arg_parser():

    parser.add_argument("--log-dir", default=str(HERE / "logs"), help="JSONL log directory; empty disables logging")
    parser.add_argument("--log-flush-every", type=int, default=50)
+    parser.add_argument("--motor-mode-fault-frames", type=int, default=3,
+                        help="Consecutive non-servo feedback frames required before a motor-mode fault")
    parser.add_argument("--log-timing", action="store_true",
                        help="Log per-loop timing fields for deploy-mode latency diagnosis")
    return parser
--- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py
+++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py
@@ -161,6 +161,25 @@ def motor_servo_fault(state):
    return None


+class MotorServoGuard:
+    def __init__(self, fault_frames):
+        self.fault_frames = max(1, int(fault_frames))
+        self.consecutive_bad = 0
+
+    def reset(self):
+        self.consecutive_bad = 0
+
+    def update(self, state):
+        reason = motor_servo_fault(state)
+        if reason is None:
+            self.reset()
+            return None
+        self.consecutive_bad += 1
+        if self.consecutive_bad >= self.fault_frames:
+            return f"{reason} ({self.consecutive_bad} consecutive frames)"
+        return None
+
+
 def validate_joint_order():
    sdk_names = list(JOINT_NAMES)
    if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -526,8 +545,14 @@ def action_ok(action_raw, args):
    if not np.all(np.isfinite(action_raw)):
        return False, "action is non-finite"
    max_abs = float(np.max(np.abs(action_raw)))
+    hard_limit = float(args.action_hard_trip_limit)
+    if hard_limit > 0 and max_abs > hard_limit:
+        return False, f"action abs {max_abs:.2f} > hard trip {hard_limit:.2f}"
    if max_abs > args.action_trip_limit:
-        return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.2f}"
+        return True, (
+            f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; "
+            f"clipped to {args.action_clip:.2f}"
+        )
    return True, "ok"


@@ -847,6 +872,7 @@ def run_deploy(args):
        edge.update(state)
        obs_builder = ObsHistoryBuilder()
        cmd_filter = CommandFilter(args)
+        servo_guard = MotorServoGuard(args.motor_mode_fault_frames)

        sm_state = State.IDLE
        last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
@@ -881,8 +907,15 @@ def run_deploy(args):
                prev_action[:] = 0.0
                prev_target = DEFAULT_DOF_POS.copy()
                rl_step = 0
+                servo_guard.reset()
                send_damping(client)

+            if sm_state in (State.HOLD, State.OBS_TEST, State.INFER_TEST, State.RL):
+                servo_fault = servo_guard.update(state)
+            else:
+                servo_guard.reset()
+                servo_fault = None
+
            cmd_raw = get_command(state, args)
            if sm_state in (State.OBS_TEST, State.INFER_TEST, State.RL):
                cmd = cmd_filter.update(cmd_raw)
@@ -913,7 +946,6 @@ def run_deploy(args):
            elif sm_state == State.HOLD:
                send_hold_cmd(client, state, args)
                if r2_rose:
-                    servo_fault = motor_servo_fault(state)
                    if servo_fault:
                        reason = servo_fault
                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
@@ -931,7 +963,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -952,7 +983,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -996,7 +1026,6 @@ def run_deploy(args):

                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1158,7 +1187,10 @@ def build_arg_parser():
    parser.add_argument("--max-pitch-deg", type=float, default=35.0)
    parser.add_argument("--max-dof-vel", type=float, default=30.0)
    parser.add_argument("--max-gyro", type=float, default=15.0)
-    parser.add_argument("--action-trip-limit", type=float, default=12.0)
+    parser.add_argument("--action-trip-limit", type=float, default=12.0,
+                        help="Soft raw-action warning threshold; output is still clipped to --action-clip")
+    parser.add_argument("--action-hard-trip-limit", type=float, default=16.0,
+                        help="Hard raw-action fault threshold; <=0 disables the hard trip")
    parser.add_argument("--action-clip", type=float, default=4.0)
    parser.add_argument("--action-ema-alpha", type=float, default=0.0)
    parser.add_argument("--max-target-step", type=float, default=0.08)
@@ -1166,6 +1198,8 @@ def build_arg_parser():

    parser.add_argument("--log-dir", default=str(HERE / "logs"), help="JSONL log directory; empty disables logging")
    parser.add_argument("--log-flush-every", type=int, default=50)
+    parser.add_argument("--motor-mode-fault-frames", type=int, default=3,
+                        help="Consecutive non-servo feedback frames required before a motor-mode fault")
    return parser


--- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py
+++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py
@@ -175,6 +175,25 @@ def motor_servo_fault(state):
    return None


+class MotorServoGuard:
+    def __init__(self, fault_frames):
+        self.fault_frames = max(1, int(fault_frames))
+        self.consecutive_bad = 0
+
+    def reset(self):
+        self.consecutive_bad = 0
+
+    def update(self, state):
+        reason = motor_servo_fault(state)
+        if reason is None:
+            self.reset()
+            return None
+        self.consecutive_bad += 1
+        if self.consecutive_bad >= self.fault_frames:
+            return f"{reason} ({self.consecutive_bad} consecutive frames)"
+        return None
+
+
 def validate_joint_order():
    sdk_names = list(JOINT_NAMES)
    if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -517,8 +536,14 @@ def action_ok(action_raw, args):
    if not np.all(np.isfinite(action_raw)):
        return False, "action is non-finite"
    max_abs = float(np.max(np.abs(action_raw)))
+    hard_limit = float(args.action_hard_trip_limit)
+    if hard_limit > 0 and max_abs > hard_limit:
+        return False, f"action abs {max_abs:.2f} > hard trip {hard_limit:.2f}"
    if max_abs > args.action_trip_limit:
-        return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.2f}"
+        return True, (
+            f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; "
+            f"clipped to {args.action_clip:.2f}"
+        )
    return True, "ok"


@@ -844,6 +869,7 @@ def run_deploy(args):
        edge.update(state)
        obs_builder = ObsHistoryBuilder()
        cmd_filter = CommandFilter(args)
+        servo_guard = MotorServoGuard(args.motor_mode_fault_frames)

        sm_state = State.IDLE
        last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
@@ -893,8 +919,15 @@ def run_deploy(args):
                prev_action[:] = 0.0
                prev_target = DEFAULT_DOF_POS.copy()
                rl_step = 0
+                servo_guard.reset()
                send_damping(client)

+            if sm_state in (State.HOLD, State.OBS_TEST, State.INFER_TEST, State.RL):
+                servo_fault = servo_guard.update(state)
+            else:
+                servo_guard.reset()
+                servo_fault = None
+
            cmd_raw = get_command(state, args)
            if sm_state in (State.OBS_TEST, State.INFER_TEST, State.RL):
                cmd = cmd_filter.update(cmd_raw)
@@ -925,7 +958,6 @@ def run_deploy(args):
            elif sm_state == State.HOLD:
                send_hold_cmd(client, state, args)
                if r2_rose:
-                    servo_fault = motor_servo_fault(state)
                    if servo_fault:
                        reason = servo_fault
                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
@@ -943,7 +975,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -964,7 +995,6 @@ def run_deploy(args):
                send_hold_cmd(client, state, args)
                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1011,7 +1041,6 @@ def run_deploy(args):

                obs_single = obs_builder.build_single(state, cmd, last_action)
                onnx_input = obs_builder.build_onnx_input(obs_single)
-                servo_fault = motor_servo_fault(state)
                if servo_fault:
                    ok, reason = False, servo_fault
                else:
@@ -1180,7 +1209,10 @@ def build_arg_parser():
    parser.add_argument("--max-pitch-deg", type=float, default=35.0)
    parser.add_argument("--max-dof-vel", type=float, default=30.0)
    parser.add_argument("--max-gyro", type=float, default=15.0)
-    parser.add_argument("--action-trip-limit", type=float, default=12.0)
+    parser.add_argument("--action-trip-limit", type=float, default=12.0,
+                        help="Soft raw-action warning threshold; output is still clipped to --action-clip")
+    parser.add_argument("--action-hard-trip-limit", type=float, default=16.0,
+                        help="Hard raw-action fault threshold; <=0 disables the hard trip")
    parser.add_argument("--action-clip", type=float, default=4.0)
    parser.add_argument("--action-ema-alpha", type=float, default=0.0)
    parser.add_argument("--max-target-step", type=float, default=0.08)
@@ -1188,6 +1220,8 @@ def build_arg_parser():

    parser.add_argument("--log-dir", default=str(HERE / "logs"), help="JSONL log directory; empty disables logging")
    parser.add_argument("--log-flush-every", type=int, default=50)
+    parser.add_argument("--motor-mode-fault-frames", type=int, default=3,
+                        help="Consecutive non-servo feedback frames required before a motor-mode fault")
    parser.add_argument("--log-timing", action="store_true",
                        help="Log per-loop timing fields for deploy-mode latency diagnosis")
    return parser