add clip

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
 
 

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

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
 
 

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