增加底层调试日志，监测电机是否使能

deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py

@@ -136,6 +136,30 @@ def motor_tau(state):
     return np.array([state.motorState[i].tauEst for i in range(NUM_ACTIONS)], dtype=np.float32)
 
 
+def motor_mode(state):
+    return np.array([state.motorState[i].mode for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_temperature(state):
+    return np.array([state.motorState[i].temperature for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_reserve(state):
+    return np.array([state.motorState[i].reserve for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_servo_fault(state):
+    modes = motor_mode(state)
+    bad = [
+        f"{JOINT_NAMES[i]}={int(modes[i])}"
+        for i in range(NUM_ACTIONS)
+        if int(modes[i]) != int(MotorMode.Servo)
+    ]
+    if bad:
+        return "motor feedback not servo: " + ", ".join(bad)
+    return None
+
+
 def validate_joint_order():
     sdk_names = list(JOINT_NAMES)
     if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -316,7 +340,10 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
-                rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
+                if np.issubdtype(v.dtype, np.integer):
+                    rec[k] = np.asarray(v, dtype=np.int32).reshape(-1).tolist()
+                else:
+                    rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
             elif isinstance(v, (np.float32, np.float64)):
                 rec[k] = float(v)
             elif isinstance(v, (np.int32, np.int64)):
@@ -597,6 +624,9 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
         dof_pos=motor_pos(state),
         dof_vel=motor_vel(state),
         tau_est=motor_tau(state),
+        motor_mode=motor_mode(state),
+        motor_temperature=motor_temperature(state),
+        motor_reserve=motor_reserve(state),
         commands_raw=np.zeros(3, dtype=np.float32) if cmd_raw is None else cmd_raw,
         commands=np.zeros(3, dtype=np.float32) if cmd is None else cmd,
         obs_single=np.zeros(NUM_OBS, dtype=np.float32) if obs_single is None else obs_single,
@@ -882,21 +912,33 @@ def run_deploy(args):
             elif sm_state == State.HOLD:
                 send_hold_cmd(client, state, args)
                 if r2_rose:
-                    print("\n[R2] HOLD -> OBS_TEST")
+                    servo_fault = motor_servo_fault(state)
-                    obs_builder.reset()
+                    if servo_fault:
-                    cmd_filter.reset()
+                        reason = servo_fault
-                    last_action[:] = 0.0
+                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
-                    prev_action[:] = 0.0
+                        sm_state = State.FAULT
-                    sm_state = State.OBS_TEST
+                        send_damping(client)
+                    else:
+                        print("\n[R2] HOLD -> OBS_TEST")
+                        obs_builder.reset()
+                        cmd_filter.reset()
+                        last_action[:] = 0.0
+                        prev_action[:] = 0.0
+                        sm_state = State.OBS_TEST
 
             elif sm_state == State.OBS_TEST:
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if not ok:
                     print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
                     sm_state = State.FAULT
+                    send_damping(client)
                 elif r2_rose:
                     print("\n[R2] OBS_TEST -> INFER_TEST")
                     obs_builder.reset()
@@ -909,7 +951,11 @@ 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)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -949,7 +995,11 @@ def run_deploy(args):
 
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -1009,6 +1059,10 @@ def run_deploy(args):
                 )
                 print(f"  RC: {fmt_rc(state)}")
                 print(f"  cmd={np.round(cmd, 3)} q={np.round(motor_pos(state), 2)}")
+                print(
+                    f"  motor_mode[FR]={motor_mode(state)[:3].tolist()} "
+                    f"temp[FR]={motor_temperature(state)[:3].tolist()}"
+                )
                 if sm_state in (State.INFER_TEST, State.RL, State.FAULT):
                     print(
                         f"  action_raw_max={np.max(np.abs(action_raw)):.3f} "

deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py

@@ -151,6 +151,30 @@ def motor_tau(state):
     return np.array([state.motorState[i].tauEst for i in range(NUM_ACTIONS)], dtype=np.float32)
 
 
+def motor_mode(state):
+    return np.array([state.motorState[i].mode for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_temperature(state):
+    return np.array([state.motorState[i].temperature for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_reserve(state):
+    return np.array([state.motorState[i].reserve for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_servo_fault(state):
+    modes = motor_mode(state)
+    bad = [
+        f"{JOINT_NAMES[i]}={int(modes[i])}"
+        for i in range(NUM_ACTIONS)
+        if int(modes[i]) != int(MotorMode.Servo)
+    ]
+    if bad:
+        return "motor feedback not servo: " + ", ".join(bad)
+    return None
+
+
 def validate_joint_order():
     sdk_names = list(JOINT_NAMES)
     if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -332,7 +356,10 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
-                rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
+                if np.issubdtype(v.dtype, np.integer):
+                    rec[k] = np.asarray(v, dtype=np.int32).reshape(-1).tolist()
+                else:
+                    rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
             elif isinstance(v, (np.float32, np.float64)):
                 rec[k] = float(v)
             elif isinstance(v, (np.int32, np.int64)):
@@ -627,6 +654,9 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
         dof_pos=motor_pos(state),
         dof_vel=motor_vel(state),
         tau_est=motor_tau(state),
+        motor_mode=motor_mode(state),
+        motor_temperature=motor_temperature(state),
+        motor_reserve=motor_reserve(state),
         commands_raw=np.zeros(3, dtype=np.float32) if cmd_raw is None else cmd_raw,
         commands=np.zeros(3, dtype=np.float32) if cmd is None else cmd,
         obs_single=np.zeros(NUM_OBS, dtype=np.float32) if obs_single is None else obs_single,
@@ -912,21 +942,33 @@ def run_deploy(args):
             elif sm_state == State.HOLD:
                 send_hold_cmd(client, state, args)
                 if r2_rose:
-                    print("\n[R2] HOLD -> OBS_TEST")
+                    servo_fault = motor_servo_fault(state)
-                    obs_builder.reset()
+                    if servo_fault:
-                    cmd_filter.reset()
+                        reason = servo_fault
-                    last_action[:] = 0.0
+                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
-                    prev_action[:] = 0.0
+                        sm_state = State.FAULT
-                    sm_state = State.OBS_TEST
+                        send_damping(client)
+                    else:
+                        print("\n[R2] HOLD -> OBS_TEST")
+                        obs_builder.reset()
+                        cmd_filter.reset()
+                        last_action[:] = 0.0
+                        prev_action[:] = 0.0
+                        sm_state = State.OBS_TEST
 
             elif sm_state == State.OBS_TEST:
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if not ok:
                     print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
                     sm_state = State.FAULT
+                    send_damping(client)
                 elif r2_rose:
                     print("\n[R2] OBS_TEST -> INFER_TEST")
                     obs_builder.reset()
@@ -939,7 +981,11 @@ 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)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -979,7 +1025,11 @@ def run_deploy(args):
 
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -1039,6 +1089,10 @@ def run_deploy(args):
                 )
                 print(f"  RC: {fmt_rc(state)}")
                 print(f"  cmd={np.round(cmd, 3)} q={np.round(motor_pos(state), 2)}")
+                print(
+                    f"  motor_mode[FR]={motor_mode(state)[:3].tolist()} "
+                    f"temp[FR]={motor_temperature(state)[:3].tolist()}"
+                )
                 if sm_state in (State.INFER_TEST, State.RL, State.FAULT):
                     print(
                         f"  action_raw_max={np.max(np.abs(action_raw)):.3f} "

deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py

@@ -137,6 +137,30 @@ def motor_tau(state):
     return np.array([state.motorState[i].tauEst for i in range(NUM_ACTIONS)], dtype=np.float32)
 
 
+def motor_mode(state):
+    return np.array([state.motorState[i].mode for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_temperature(state):
+    return np.array([state.motorState[i].temperature for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_reserve(state):
+    return np.array([state.motorState[i].reserve for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_servo_fault(state):
+    modes = motor_mode(state)
+    bad = [
+        f"{JOINT_NAMES[i]}={int(modes[i])}"
+        for i in range(NUM_ACTIONS)
+        if int(modes[i]) != int(MotorMode.Servo)
+    ]
+    if bad:
+        return "motor feedback not servo: " + ", ".join(bad)
+    return None
+
+
 def validate_joint_order():
     sdk_names = list(JOINT_NAMES)
     if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -317,7 +341,10 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
-                rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
+                if np.issubdtype(v.dtype, np.integer):
+                    rec[k] = np.asarray(v, dtype=np.int32).reshape(-1).tolist()
+                else:
+                    rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
             elif isinstance(v, (np.float32, np.float64)):
                 rec[k] = float(v)
             elif isinstance(v, (np.int32, np.int64)):
@@ -598,6 +625,9 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
         dof_pos=motor_pos(state),
         dof_vel=motor_vel(state),
         tau_est=motor_tau(state),
+        motor_mode=motor_mode(state),
+        motor_temperature=motor_temperature(state),
+        motor_reserve=motor_reserve(state),
         commands_raw=np.zeros(3, dtype=np.float32) if cmd_raw is None else cmd_raw,
         commands=np.zeros(3, dtype=np.float32) if cmd is None else cmd,
         obs_single=np.zeros(NUM_OBS, dtype=np.float32) if obs_single is None else obs_single,
@@ -883,21 +913,33 @@ def run_deploy(args):
             elif sm_state == State.HOLD:
                 send_hold_cmd(client, state, args)
                 if r2_rose:
-                    print("\n[R2] HOLD -> OBS_TEST")
+                    servo_fault = motor_servo_fault(state)
-                    obs_builder.reset()
+                    if servo_fault:
-                    cmd_filter.reset()
+                        reason = servo_fault
-                    last_action[:] = 0.0
+                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
-                    prev_action[:] = 0.0
+                        sm_state = State.FAULT
-                    sm_state = State.OBS_TEST
+                        send_damping(client)
+                    else:
+                        print("\n[R2] HOLD -> OBS_TEST")
+                        obs_builder.reset()
+                        cmd_filter.reset()
+                        last_action[:] = 0.0
+                        prev_action[:] = 0.0
+                        sm_state = State.OBS_TEST
 
             elif sm_state == State.OBS_TEST:
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if not ok:
                     print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
                     sm_state = State.FAULT
+                    send_damping(client)
                 elif r2_rose:
                     print("\n[R2] OBS_TEST -> INFER_TEST")
                     obs_builder.reset()
@@ -910,7 +952,11 @@ 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)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -950,7 +996,11 @@ def run_deploy(args):
 
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -1010,6 +1060,10 @@ def run_deploy(args):
                 )
                 print(f"  RC: {fmt_rc(state)}")
                 print(f"  cmd={np.round(cmd, 3)} q={np.round(motor_pos(state), 2)}")
+                print(
+                    f"  motor_mode[FR]={motor_mode(state)[:3].tolist()} "
+                    f"temp[FR]={motor_temperature(state)[:3].tolist()}"
+                )
                 if sm_state in (State.INFER_TEST, State.RL, State.FAULT):
                     print(
                         f"  action_raw_max={np.max(np.abs(action_raw)):.3f} "

deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py

@@ -152,6 +152,30 @@ def motor_tau(state):
     return np.array([state.motorState[i].tauEst for i in range(NUM_ACTIONS)], dtype=np.float32)
 
 
+def motor_mode(state):
+    return np.array([state.motorState[i].mode for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_temperature(state):
+    return np.array([state.motorState[i].temperature for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_reserve(state):
+    return np.array([state.motorState[i].reserve for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_servo_fault(state):
+    modes = motor_mode(state)
+    bad = [
+        f"{JOINT_NAMES[i]}={int(modes[i])}"
+        for i in range(NUM_ACTIONS)
+        if int(modes[i]) != int(MotorMode.Servo)
+    ]
+    if bad:
+        return "motor feedback not servo: " + ", ".join(bad)
+    return None
+
+
 def validate_joint_order():
     sdk_names = list(JOINT_NAMES)
     if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -333,7 +357,10 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
-                rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
+                if np.issubdtype(v.dtype, np.integer):
+                    rec[k] = np.asarray(v, dtype=np.int32).reshape(-1).tolist()
+                else:
+                    rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
             elif isinstance(v, (np.float32, np.float64)):
                 rec[k] = float(v)
             elif isinstance(v, (np.int32, np.int64)):
@@ -628,6 +655,9 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
         dof_pos=motor_pos(state),
         dof_vel=motor_vel(state),
         tau_est=motor_tau(state),
+        motor_mode=motor_mode(state),
+        motor_temperature=motor_temperature(state),
+        motor_reserve=motor_reserve(state),
         commands_raw=np.zeros(3, dtype=np.float32) if cmd_raw is None else cmd_raw,
         commands=np.zeros(3, dtype=np.float32) if cmd is None else cmd,
         obs_single=np.zeros(NUM_OBS, dtype=np.float32) if obs_single is None else obs_single,
@@ -913,21 +943,33 @@ def run_deploy(args):
             elif sm_state == State.HOLD:
                 send_hold_cmd(client, state, args)
                 if r2_rose:
-                    print("\n[R2] HOLD -> OBS_TEST")
+                    servo_fault = motor_servo_fault(state)
-                    obs_builder.reset()
+                    if servo_fault:
-                    cmd_filter.reset()
+                        reason = servo_fault
-                    last_action[:] = 0.0
+                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
-                    prev_action[:] = 0.0
+                        sm_state = State.FAULT
-                    sm_state = State.OBS_TEST
+                        send_damping(client)
+                    else:
+                        print("\n[R2] HOLD -> OBS_TEST")
+                        obs_builder.reset()
+                        cmd_filter.reset()
+                        last_action[:] = 0.0
+                        prev_action[:] = 0.0
+                        sm_state = State.OBS_TEST
 
             elif sm_state == State.OBS_TEST:
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if not ok:
                     print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
                     sm_state = State.FAULT
+                    send_damping(client)
                 elif r2_rose:
                     print("\n[R2] OBS_TEST -> INFER_TEST")
                     obs_builder.reset()
@@ -940,7 +982,11 @@ 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)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -980,7 +1026,11 @@ def run_deploy(args):
 
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                servo_fault = motor_servo_fault(state)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -1040,6 +1090,10 @@ def run_deploy(args):
                 )
                 print(f"  RC: {fmt_rc(state)}")
                 print(f"  cmd={np.round(cmd, 3)} q={np.round(motor_pos(state), 2)}")
+                print(
+                    f"  motor_mode[FR]={motor_mode(state)[:3].tolist()} "
+                    f"temp[FR]={motor_temperature(state)[:3].tolist()}"
+                )
                 if sm_state in (State.INFER_TEST, State.RL, State.FAULT):
                     print(
                         f"  action_raw_max={np.max(np.abs(action_raw)):.3f} "