fix linter

This commit is contained in:
Martino Russi
2025-11-27 10:43:15 +01:00
parent 36ed02adfa
commit 9a90b7dcb2
3 changed files with 10 additions and 18 deletions
+1 -6
View File
@@ -218,12 +218,7 @@ class GrootLocomotionController:
cmd_magnitude = np.linalg.norm(self.locomotion_cmd)
if cmd_magnitude < 0.05:
# balance/standing policy for small commands
selected_policy = self.policy_balance
else:
# walking policy for movement commands
selected_policy = self.policy_walk
selected_policy = self.policy_balance if cmd_magnitude < 0.05 else self.policy_walk #balance/standing policy for small commands, walking policy for movement commands
# run policy inference
ort_inputs = {selected_policy.get_inputs()[0].name: obs_tensor.cpu().numpy()}
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
import contextlib
import pickle
import threading
import time
@@ -31,11 +32,9 @@ def state_forward_loop(
# optional downsampling (if robot dds rate > state_period)
if now - last_state_time >= state_period:
payload = pickle.dumps((kTopicLowState, msg), protocol=pickle.HIGHEST_PROTOCOL)
try:
lowstate_sock.send(payload, zmq.NOBLOCK)
except zmq.Again:
# if no subscribers / tx buffer full, just drop
pass
with contextlib.suppress(zmq.Again):
lowstate_sock.send(payload, zmq.NOBLOCK)
last_state_time = now
+6 -8
View File
@@ -219,9 +219,7 @@ class UnitreeG1(Robot):
@property
def is_connected(self) -> bool:
if self.lowstate_buffer.GetData() is None:
return False
return True
return self.lowstate_buffer.GetData() is None
@property
def _motors_ft(self) -> dict[str, type]:
@@ -258,8 +256,8 @@ class UnitreeG1(Robot):
self, waist_yaw, waist_yaw_omega, imu_quat, imu_omega
): # transform imu data from torso to pelvis frame
"""Transform IMU data from torso to pelvis frame."""
RzWaist = R.from_euler("z", waist_yaw).as_matrix()
R_torso = R.from_quat([imu_quat[1], imu_quat[2], imu_quat[3], imu_quat[0]]).as_matrix()
R_pelvis = np.dot(R_torso, RzWaist.T)
w = np.dot(RzWaist, imu_omega[0]) - np.array([0, 0, waist_yaw_omega])
return R.from_matrix(R_pelvis).as_quat()[[3, 0, 1, 2]], w
rot_zWaist = R.from_euler("z", waist_yaw).as_matrix()
rot_torso = R.from_quat([imu_quat[1], imu_quat[2], imu_quat[3], imu_quat[0]]).as_matrix()
rot_pelvis = np.dot(rot_torso, rot_zWaist.T)
w = np.dot(rot_zWaist, imu_omega[0]) - np.array([0, 0, waist_yaw_omega])
return R.from_matrix(rot_pelvis).as_quat()[[3, 0, 1, 2]], w