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 834e3a0..3921c11 100644 --- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py +++ b/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") - obs_builder.reset() - cmd_filter.reset() - last_action[:] = 0.0 - prev_action[:] = 0.0 - sm_state = State.OBS_TEST + 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() + 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} " 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 f5343d9..58fad4c 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 @@ -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") - obs_builder.reset() - cmd_filter.reset() - last_action[:] = 0.0 - prev_action[:] = 0.0 - sm_state = State.OBS_TEST + 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() + 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} " 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 7c07d35..25c4280 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 @@ -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") - obs_builder.reset() - cmd_filter.reset() - last_action[:] = 0.0 - prev_action[:] = 0.0 - sm_state = State.OBS_TEST + 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() + 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} " 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 a8283ec..c4eccd0 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 @@ -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") - obs_builder.reset() - cmd_filter.reset() - last_action[:] = 0.0 - prev_action[:] = 0.0 - sm_state = State.OBS_TEST + 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() + 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} "