From 60404fbc9807faa4f298e057c74329e49bf2bee5 Mon Sep 17 00:00:00 2001 From: Shafeef Omar Date: Fri, 29 May 2026 19:42:21 +0200 Subject: [PATCH] Fix stand controllers tracking wrong pose due to float32 time quantization The Go2/G1 stand controllers derived their interpolation phase from a "time" observation that stored wall-clock time.time() in a float32 tensor. At the current epoch (~1.78e9) float32 resolves only ~128 s steps, so the elapsed time (obs["time"] - start_time) collapsed to 0.0 for up to ~128 s. This pinned phase = tanh(0) = 0, making STAND_UP hold the stand-down pose (and STAND_DOWN hold the stand-up pose) instead of ramping between them. Compute elapsed time directly from time.time() (float64) in Go2StandUpController, Go2StandDownController and G1LowLevelController, matching the approach already used by the RL and gain-tuning controllers. Co-Authored-By: Claude Opus 4.8 (1M context) --- src/controllers/stand_controller.py | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/controllers/stand_controller.py b/src/controllers/stand_controller.py index 3b02616..a55e581 100644 --- a/src/controllers/stand_controller.py +++ b/src/controllers/stand_controller.py @@ -127,10 +127,9 @@ def register_observations(self): def compute_lowlevelcmd(self, state): super().compute_lowlevelcmd(state) - obs = self.obs_manager.compute(state) - time = obs["time"] - self.start_time - phase = torch.tanh(torch.tensor(time / 1.2, device=self.device)) + elapsed = time.time() - self.start_time + phase = torch.tanh(torch.tensor(elapsed / 1.2, device=self.device)) cmd = {} for i in range(self.robot.num_joints): @@ -178,10 +177,8 @@ def register_observations(self): def compute_lowlevelcmd(self, state): super().compute_lowlevelcmd(state) - obs = self.obs_manager.compute(state) - - time = obs["time"] - self.start_time - phase = torch.tanh(torch.tensor(time / 1.2, device=self.device)) + elapsed = time.time() - self.start_time + phase = torch.tanh(torch.tensor(elapsed / 1.2, device=self.device)) cmd = {} for i in range(self.robot.num_joints): cmd[f"motor_{i}"] = { @@ -916,8 +913,7 @@ def register_observations(self): def compute_lowlevelcmd(self, state): super().compute_lowlevelcmd(state) - obs = self.obs_manager.compute(state) - time = obs["time"] - self.start_time + elapsed = time.time() - self.start_time Kp = [ 60, @@ -1028,10 +1024,10 @@ class Mode: cmd = {f"motor_{i}": {"q": 0.0, "kp": 0.0, "dq": 0.0, "kd": 0.0, "tau": 0.0} for i in range(G1_NUM_MOTOR)} cmd["mode_pr"] = Mode.PR - if time < self.total_time: + if elapsed < self.total_time: # [Stage 1]: set robot to zero posture for i in range(G1_NUM_MOTOR): - ratio = torch.clamp(torch.tensor(time / self.total_time, device=self.device), 0.0, 1.0) + ratio = torch.clamp(torch.tensor(elapsed / self.total_time, device=self.device), 0.0, 1.0) cmd["mode_pr"] = Mode.PR cmd[f"motor_{i}"] = { "q": (1.0 - ratio) * state["robot/joint_pos"][self.dof_idx[i]], @@ -1042,11 +1038,11 @@ class Mode: "mode": 1, } - elif time < self.total_time * 2: + elif elapsed < self.total_time * 2: # [Stage 2]: swing ankle using PR mode max_P = torch.pi * 30.0 / 180.0 max_R = torch.pi * 10.0 / 180.0 - t = time - self.total_time + t = elapsed - self.total_time L_P_des = max_P * torch.sin(2.0 * torch.pi * t) L_R_des = max_R * torch.sin(2.0 * torch.pi * t) R_P_des = max_P * torch.sin(2.0 * torch.pi * t) @@ -1086,7 +1082,7 @@ class Mode: # [Stage 3]: swing ankle using AB mode max_A = torch.pi * 30.0 / 180.0 max_B = torch.pi * 10.0 / 180.0 - t = time - self.total_time * 2 + t = elapsed - self.total_time * 2 L_A_des = max_A * torch.sin(2.0 * torch.pi * t) L_B_des = max_B * torch.sin(2.0 * torch.pi * t + torch.pi) R_A_des = -max_A * torch.sin(2.0 * torch.pi * t)