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

优化板端速度，crc

Browse Source
This commit is contained in:
cyy_mac
2026-07-26 20:53:40 +08:00
parent 29de5eb8de
commit 300a6ff5bf
1 changed files with 26 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
View File

@@ -390,7 +390,7 @@ def send_position_cmd(client, state, targets, args):
client.send(cmd) client.send(cmd)
def ramp_to_default(client, args, state): def ramp_to_default(client, args, state, logger=None, step_base=0):
print("[INFO] Ramping to default pose...") print("[INFO] Ramping to default pose...")
current = motor_pos(state) current = motor_pos(state)
error = current - DEFAULT_DOF_POS error = current - DEFAULT_DOF_POS
@@ -402,6 +402,8 @@ def ramp_to_default(client, args, state):
ramp_steps = max(1, int(args.ramp_time * args.ramp_hz)) ramp_steps = max(1, int(args.ramp_time * args.ramp_hz))
dt = 1.0 / args.ramp_hz dt = 1.0 / args.ramp_hz
log_every = max(1, int(args.ramp_hz / 5.0))
next_t = time.perf_counter()
for i in range(ramp_steps): for i in range(ramp_steps):
if EXIT: if EXIT:
@@ -430,6 +432,8 @@ def ramp_to_default(client, args, state):
position_protect_limit=None, position_protect_limit=None,
) )
client.send(cmd) client.send(cmd)
if logger is not None and (i % log_every == 0 or i == ramp_steps - 1):
log_state(logger, step_base * 100000 + i, "RAMP", state, target=target)
if i % max(1, ramp_steps // 4) == 0: if i % max(1, ramp_steps // 4) == 0:
actual = motor_pos(state) actual = motor_pos(state)
print( print(
@@ -437,16 +441,29 @@ def ramp_to_default(client, args, state):
f"target_err={np.max(np.abs(target - DEFAULT_DOF_POS)):.3f} " f"target_err={np.max(np.abs(target - DEFAULT_DOF_POS)):.3f} "
f"actual_err={np.max(np.abs(actual - DEFAULT_DOF_POS)):.3f}" f"actual_err={np.max(np.abs(actual - DEFAULT_DOF_POS)):.3f}"
) )
time.sleep(dt) next_t += dt
sleep = next_t - time.perf_counter()
if sleep > 0:
time.sleep(sleep)
else:
next_t = time.perf_counter()
for _ in range(max(1, int(0.5 * args.ramp_hz))): hold_steps = max(1, int(0.5 * args.ramp_hz))
for i in range(hold_steps):
if EXIT: if EXIT:
return state return state
new_state = client.recv_latest() new_state = client.recv_latest()
if new_state is not None: if new_state is not None:
state = new_state state = new_state
send_hold_cmd(client, state, args) send_hold_cmd(client, state, args)
time.sleep(dt) if logger is not None and (i % log_every == 0 or i == hold_steps - 1):
log_state(logger, step_base * 100000 + ramp_steps + i, "RAMP_HOLD", state, target=DEFAULT_DOF_POS)
next_t += dt
sleep = next_t - time.perf_counter()
if sleep > 0:
time.sleep(sleep)
else:
next_t = time.perf_counter()
print("[INFO] Default pose reached.") print("[INFO] Default pose reached.")
return state return state
@@ -853,7 +870,7 @@ def run_deploy(args):
if r2_rose: if r2_rose:
print("\n[R2] IDLE -> CALIBRATE") print("\n[R2] IDLE -> CALIBRATE")
sm_state = State.CALIBRATE sm_state = State.CALIBRATE
state = ramp_to_default(client, args, state) state = ramp_to_default(client, args, state, logger=logger, step_base=step)
obs_builder.reset() obs_builder.reset()
cmd_filter.reset() cmd_filter.reset()
last_action[:] = 0.0 last_action[:] = 0.0
@@ -925,7 +942,7 @@ def run_deploy(args):
cmd_filter.reset() cmd_filter.reset()
last_action[:] = 0.0 last_action[:] = 0.0
prev_action[:] = 0.0 prev_action[:] = 0.0
state = ramp_to_default(client, args, state) state = ramp_to_default(client, args, state, logger=logger, step_base=step)
prev_target = DEFAULT_DOF_POS.copy() prev_target = DEFAULT_DOF_POS.copy()
rl_step = 0 rl_step = 0
continue continue
@@ -965,7 +982,7 @@ def run_deploy(args):
if r2_rose: if r2_rose:
print("\n[R2] FAULT -> CALIBRATE") print("\n[R2] FAULT -> CALIBRATE")
sm_state = State.CALIBRATE sm_state = State.CALIBRATE
state = ramp_to_default(client, args, state) state = ramp_to_default(client, args, state, logger=logger, step_base=step)
prev_target = DEFAULT_DOF_POS.copy() prev_target = DEFAULT_DOF_POS.copy()
sm_state = State.HOLD sm_state = State.HOLD
print("[STATE] HOLD") print("[STATE] HOLD")
@@ -1076,8 +1093,8 @@ def build_arg_parser():
parser.add_argument("--cmd-yaw", type=float, default=0.0) parser.add_argument("--cmd-yaw", type=float, default=0.0)
parser.add_argument("--rate-hz", type=float, default=50.0) parser.add_argument("--rate-hz", type=float, default=50.0)
parser.add_argument("--ramp-time", type=float, default=2.5) parser.add_argument("--ramp-time", type=float, default=5.0)
parser.add_argument("--ramp-hz", type=float, default=100.0) parser.add_argument("--ramp-hz", type=float, default=50.0)
parser.add_argument("--warmup-steps", type=int, default=50) parser.add_argument("--warmup-steps", type=int, default=50)
parser.add_argument("--max-steps", type=int, default=0) parser.add_argument("--max-steps", type=int, default=0)
parser.add_argument("--print-every", type=int, default=50) parser.add_argument("--print-every", type=int, default=50)