Update sim/unitree_sdk2py_bridge.py

sim/unitree_sdk2py_bridge.py

@@ -16,21 +16,14 @@ from unitree_sdk2py.idl.default import (
     unitree_hg_msg_dds__HandState_ as HandState_default,
 )
 from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
-try:
-    from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_, OdoState_
-    HAS_ODOSTATE = True
-except ImportError:
-    from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_
-    OdoState_ = None
-    HAS_ODOSTATE = False
-    print("Warning: OdoState_ not available in unitree_sdk2py")
+
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_
 
 
 class UnitreeSdk2Bridge:
     """
-    This class is responsible for bridging the Unitree SDK2 with the Gr00t environment.
-    It is responsible for sending and receiving messages to and from the Unitree SDK2.
-    Both the body and hand are supported.
+    Bridge between Unitree SDK2 and the MuJoCo simulation.
+    Handles sending and receiving DDS messages for body and hand control.
     """
 
     def __init__(self, config):
@@ -44,10 +37,6 @@ class UnitreeSdk2Bridge:
                 unitree_hg_msg_dds__LowCmd_,
                 unitree_hg_msg_dds__LowState_ as LowState_default,
             )
-            try:
-                from unitree_sdk2py.idl.default import unitree_hg_msg_dds__OdoState_ as OdoState_default
-            except ImportError:
-                OdoState_default = None
             from unitree_sdk2py.idl.unitree_hg.msg.dds_ import IMUState_, LowCmd_, LowState_
 
             self.low_cmd = unitree_hg_msg_dds__LowCmd_()
@@ -85,14 +74,6 @@ class UnitreeSdk2Bridge:
         self.low_state_puber = ChannelPublisher("rt/lowstate", LowState_)
         self.low_state_puber.Init()
 
-        # Only create odo_state for supported robot types (if available)
-        if ("g1" in robot_type or "h1-2" in robot_type) and HAS_ODOSTATE and OdoState_default:
-            self.odo_state = OdoState_default()
-            self.odo_state_puber = ChannelPublisher("rt/odostate", OdoState_)
-            self.odo_state_puber.Init()
-        else:
-            self.odo_state = None
-            self.odo_state_puber = None
         self.torso_imu_state = IMUState_default()
         self.torso_imu_puber = ChannelPublisher("rt/secondary_imu", IMUState_)
         self.torso_imu_puber.Init()
@@ -224,12 +205,6 @@ class UnitreeSdk2Bridge:
             raise NotImplementedError("Frame sensor data is not implemented yet.")
         else:
             # Get data from ground truth
-            # Publish odo state only if available
-            if self.odo_state is not None:
-                self.odo_state.position[:] = obs["floating_base_pose"][:3]
-                self.odo_state.linear_velocity[:] = obs["floating_base_vel"][:3]
-                self.odo_state.orientation[:] = obs["floating_base_pose"][3:7]
-                self.odo_state.angular_velocity[:] = obs["floating_base_vel"][3:6]
             # quaternion: w, x, y, z
             self.low_state.imu_state.quaternion[:] = obs["floating_base_pose"][3:7]
             # angular velocity
@@ -246,11 +221,6 @@ class UnitreeSdk2Bridge:
         self.low_state.tick = int(obs["time"] * 1e3)
         self.low_state_puber.Write(self.low_state)
 
-        # Publish odo state only if available
-        if self.odo_state is not None and self.odo_state_puber is not None:
-            self.odo_state.tick = int(obs["time"] * 1e3)
-            self.odo_state_puber.Write(self.odo_state)
-
         self.torso_imu_puber.Write(self.torso_imu_state)
 
         # publish hand state