From ebbe9d54322315ca1546d120a009475a2ec992a0 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Mon, 27 Jul 2026 18:39:19 +0800 Subject: [PATCH] add clip --- .../deploy_go1_rlgym_pro_sdk.py | 46 ++++++++++++++++--- .../deploy_go1_rlgym_pro_sdk_fastcpp.py | 46 ++++++++++++++++--- .../deploy_go1_rlgym_pro_sdk_lab.py | 46 ++++++++++++++++--- .../deploy_go1_rlgym_pro_sdk_lab_fastcpp.py | 46 ++++++++++++++++--- 4 files changed, 160 insertions(+), 24 deletions(-) diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py index 3921c11..74bd9fb 100644 --- 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 diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py index 15f0e49..daa7dc0 100644 --- 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 diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py index 25c4280..4562a59 100644 --- 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 diff --git 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 index 4a27de5..93239ab 100644 --- 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