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) cmd_magnitude = np.linalg.norm(self.locomotion_cmd)
if cmd_magnitude < 0.05: 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
# balance/standing policy for small commands
selected_policy = self.policy_balance
else:
# walking policy for movement commands
selected_policy = self.policy_walk
# run policy inference # run policy inference
ort_inputs = {selected_policy.get_inputs()[0].name: obs_tensor.cpu().numpy()} ort_inputs = {selected_policy.get_inputs()[0].name: obs_tensor.cpu().numpy()}
@@ -1,4 +1,5 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import contextlib
import pickle import pickle
import threading import threading
import time import time
@@ -31,11 +32,9 @@ def state_forward_loop(
# optional downsampling (if robot dds rate > state_period) # optional downsampling (if robot dds rate > state_period)
if now - last_state_time >= state_period: if now - last_state_time >= state_period:
payload = pickle.dumps((kTopicLowState, msg), protocol=pickle.HIGHEST_PROTOCOL) payload = pickle.dumps((kTopicLowState, msg), protocol=pickle.HIGHEST_PROTOCOL)
try: # if no subscribers / tx buffer full, just drop
with contextlib.suppress(zmq.Again):
lowstate_sock.send(payload, zmq.NOBLOCK) lowstate_sock.send(payload, zmq.NOBLOCK)
except zmq.Again:
# if no subscribers / tx buffer full, just drop
pass
last_state_time = now last_state_time = now
+6 -8
View File
@@ -219,9 +219,7 @@ class UnitreeG1(Robot):
@property @property
def is_connected(self) -> bool: def is_connected(self) -> bool:
if self.lowstate_buffer.GetData() is None: return self.lowstate_buffer.GetData() is None
return False
return True
@property @property
def _motors_ft(self) -> dict[str, type]: def _motors_ft(self) -> dict[str, type]:
@@ -258,8 +256,8 @@ class UnitreeG1(Robot):
self, waist_yaw, waist_yaw_omega, imu_quat, imu_omega 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
"""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() rot_zWaist = 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() rot_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) rot_pelvis = np.dot(rot_torso, rot_zWaist.T)
w = np.dot(RzWaist, imu_omega[0]) - np.array([0, 0, waist_yaw_omega]) w = np.dot(rot_zWaist, imu_omega[0]) - np.array([0, 0, waist_yaw_omega])
return R.from_matrix(R_pelvis).as_quat()[[3, 0, 1, 2]], w return R.from_matrix(rot_pelvis).as_quat()[[3, 0, 1, 2]], w