cyy/go1_pro_deploy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
cyy_mac
2026-07-26 23:16:57 +08:00
parent 94855db3ed
commit 69c9fece6c
4 changed files with 256 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
View File

@@ -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} "

74
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py
View File

@@ -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} "

74
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py
View File

@@ -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} "

74
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py
View File

@@ -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} "