From 300a6ff5bfbbe0823e3ae05f795a0fc689c9eefb Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Sun, 26 Jul 2026 20:53:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9D=BF=E7=AB=AF=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=EF=BC=8Ccrc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../deploy_go1_rlgym_pro_sdk.py | 35 ++++++++++++++----- 1 file changed, 26 insertions(+), 9 deletions(-) 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 bf654ff..834e3a0 100644 --- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py +++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py @@ -390,7 +390,7 @@ def send_position_cmd(client, state, targets, args): 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...") current = motor_pos(state) 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)) 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): if EXIT: @@ -430,6 +432,8 @@ def ramp_to_default(client, args, state): position_protect_limit=None, ) 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: actual = motor_pos(state) 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"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: return state new_state = client.recv_latest() if new_state is not None: state = new_state 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.") return state @@ -853,7 +870,7 @@ def run_deploy(args): if r2_rose: print("\n[R2] IDLE -> 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() cmd_filter.reset() last_action[:] = 0.0 @@ -925,7 +942,7 @@ def run_deploy(args): cmd_filter.reset() last_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() rl_step = 0 continue @@ -965,7 +982,7 @@ def run_deploy(args): if r2_rose: print("\n[R2] FAULT -> 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() sm_state = 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("--rate-hz", type=float, default=50.0) - parser.add_argument("--ramp-time", type=float, default=2.5) - parser.add_argument("--ramp-hz", type=float, default=100.0) + parser.add_argument("--ramp-time", type=float, default=5.0) + parser.add_argument("--ramp-hz", type=float, default=50.0) parser.add_argument("--warmup-steps", type=int, default=50) parser.add_argument("--max-steps", type=int, default=0) parser.add_argument("--print-every", type=int, default=50)