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

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,6 +340,9 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
+                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)
@@ -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,6 +912,13 @@ 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}")
+                        sm_state = State.FAULT
+                        send_damping(client)
+                    else:
                         print("\n[R2] HOLD -> OBS_TEST")
                         obs_builder.reset()
                         cmd_filter.reset()
@@ -893,10 +930,15 @@ 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:
                     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,6 +951,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -949,6 +995,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -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,6 +356,9 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
+                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)
@@ -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,6 +942,13 @@ 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}")
+                        sm_state = State.FAULT
+                        send_damping(client)
+                    else:
                         print("\n[R2] HOLD -> OBS_TEST")
                         obs_builder.reset()
                         cmd_filter.reset()
@@ -923,10 +960,15 @@ 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:
                     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,6 +981,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -979,6 +1025,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -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,6 +341,9 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
+                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)
@@ -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,6 +913,13 @@ 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}")
+                        sm_state = State.FAULT
+                        send_damping(client)
+                    else:
                         print("\n[R2] HOLD -> OBS_TEST")
                         obs_builder.reset()
                         cmd_filter.reset()
@@ -894,10 +931,15 @@ 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:
                     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,6 +952,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -950,6 +996,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -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,6 +357,9 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
+                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)
@@ -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,6 +943,13 @@ 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}")
+                        sm_state = State.FAULT
+                        send_damping(client)
+                    else:
                         print("\n[R2] HOLD -> OBS_TEST")
                         obs_builder.reset()
                         cmd_filter.reset()
@@ -924,10 +961,15 @@ 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:
                     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,6 +982,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -980,6 +1026,10 @@ 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:
                     ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
@@ -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} "