Extend supported sensors

robohive/robot/hardware_base.py

@@ -9,32 +9,39 @@
 import abc
 
 class hardwareBase(abc.ABC):
-    def __init__(self, name, *args, **kwargs):
+    def __init__(self, name, *args, **kwargs) -> None:
         self.name = name
 
     @abc.abstractmethod
-    def connect(self):
+    def connect(self) -> bool:
         """Establish hardware connection"""
 
     @abc.abstractmethod
-    def okay(self):
+    def okay(self) -> bool:
         """Return hardware health"""
 
     @abc.abstractmethod
-    def close(self):
+    def close(self) -> bool:
         """Close hardware connection"""
 
     @abc.abstractmethod
-    def reset(self):
+    def reset(self) -> None:
         """Reset hardware"""
 
     @abc.abstractmethod
-    def get_sensors(self):
-        """Get hardware sensors"""
+    def _get_sensors(self) -> dict:
+        """Get hardware sensors — returned dict must include a 'time' key"""
+
+    def get_sensors(self) -> dict:
+        """Get hardware sensors, enforcing 'time' key contract"""
+        data = self._get_sensors()
+        assert isinstance(data, dict) and 'time' in data, \
+            f"{self.name}: get_sensors() must return a dict containing a 'time' key, got {type(data)}"
+        return data
 
     @abc.abstractmethod
-    def apply_commands(self):
+    def apply_commands(self) -> None:
         """Apply hardware commands"""
 
-    def __del__(self):
+    def __del__(self) -> None:
         self.close()

robohive/robot/hardware_dynamixel.py

@@ -38,7 +38,7 @@ def close(self):
     def reset(self):
         """Reset hardware"""
 
-    def get_sensors(self):
+    def _get_sensors(self):
         """Get hardware sensors"""
 
     def apply_commands(self):

robohive/robot/hardware_franka.py

@@ -185,16 +185,16 @@ def reset(self, reset_pos=None, time_to_go=5):
             self.reset(reset_pos, time_to_go)
 
 
-    def get_sensors(self):
+    def _get_sensors(self) -> dict:
         """Get hardware sensors"""
         try:
             joint_pos = self.robot.get_joint_positions()
             joint_vel = self.robot.get_joint_velocities()
         except:
             print("Failed to get current sensors: ", end="")
             self.reconnect()
-            return self.get_sensors()
-        return {'joint_pos': joint_pos, 'joint_vel':joint_vel}
+            return self._get_sensors()
+        return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel}
 
 
     def apply_commands(self, q_desired=None, kp=None, kd=None):

robohive/robot/hardware_optitrack.py

@@ -134,7 +134,7 @@ def __repr__(self):
                     self.data_float[2], self.data_float[3], self.data_float[4])
 
     # get latest sensor value (helpful when there is a single sensors)
-    def get_sensors(self):
+    def _get_sensors(self) -> dict:
         # sensor_data isn't updated in place ==> it can be easily passed around and cached
         # repeated calls will return the same data_frame ==> no overhead for multiple queries to the same sensor reading
         return self.sensor_data

robohive/robot/hardware_realsense.py

@@ -58,7 +58,7 @@ def callback(self, pkt):
         timestamp_str_wo_nano = timestamp_str[:23] + timestamp_str[29:]
         self.most_recent_pkt_ts = datetime.datetime.fromisoformat(timestamp_str_wo_nano)
 
-    def get_sensors(self):
+    def _get_sensors(self) -> dict:
         # get all data from all topics
         last_img = copy.deepcopy(self.last_image_pkt)
         last_depth = copy.deepcopy(self.last_depth_pkt)

robohive/robot/hardware_robotiq.py

@@ -88,15 +88,15 @@ def reset(self, width=None, **kwargs):
         self.apply_commands(width=width, **kwargs)
 
 
-    def get_sensors(self):
+    def _get_sensors(self) -> dict:
         """Get hardware sensors"""
         try:
             curr_state = self.robot.get_state()
         except:
             print("RBQ:> Failed to get current sensors: ", end="")
             self.reconnect()
-            return self.get_sensors()
-        return np.array([curr_state.width])
+            return self._get_sensors()
+        return {'time': time.time(), 'width': np.array([curr_state.width])}
 
     def apply_commands(self, width:float, speed:float=0.1, force:float=0.1):
         assert width>=0.0 and width<=self.max_width, "Gripper desired width ({}) is out of bound (0,{})".format(width, self.max_width)

robohive/robot/robot.py

@@ -336,17 +336,21 @@ def configure_robot(self, sim, config_path):
                 device['sensor_ids'].append(sensor['adr']) # list of all ids
                 sensor_type = sim.model.sensor_type[sensor['sim_id']]
                 sensor_objid = sim.model.sensor_objid[sensor['sim_id']]
-                if sensor_type == mujoco.mjtSensor.mjSENS_JOINTPOS:  # mjSENS_JOINTPOS,// scalar joint position (hinge and slide only)
+                # sensordata_id: address in sim.data.sensordata for this sensor.
+                # Stored for all sensor types so Pass 2 of sensor2sim can write
+                # hardware readings into sensordata as the single authoritative source.
+                sensor['sensordata_id'] = int(sim.model.sensor_adr[sensor['sim_id']])
+                if sensor_type == mujoco.mjtSensor.mjSENS_JOINTPOS:  # scalar joint position (hinge and slide only)
                     sensor['data_type'] = 'qpos'
                     sensor['data_id'] = sim.model.jnt_qposadr[sensor_objid]
-                elif sensor_type == mujoco.mjtSensor.mjSENS_JOINTVEL:  # mjSENS_JOINTVEL,// scalar joint position (hinge and slide only)
+                elif sensor_type == mujoco.mjtSensor.mjSENS_JOINTVEL:  # scalar joint velocity (hinge and slide only)
                     sensor['data_type'] = 'qvel'
                     sensor['data_id'] = sim.model.jnt_dofadr[sensor_objid]
-                elif sensor_type == mujoco.mjtSensor.mjSENS_TENDON:  # mjSENS_TENDON // tendon force
-                    sensor['data_type'] = 'ten_length'
-                    sensor['data_id'] = sensor_objid
                 else:
-                    quit("ERROR: Sensor {} has unsupported sensor_type: {}".format(sensor['name'],sensor_type))
+                    # Non-invertible sensor: sim.forward() recomputes this from state.
+                    # Covers tendon position, joint actuator force, actuator force, etc.
+                    sensor['data_type'] = 'sensordata'
+                    sensor['data_id'] = sensor['sensordata_id']
 
             # configure device actuators
             device['actuator_ids'] = []
@@ -418,6 +422,8 @@ def get_sensors(self, noise_scale=None, random_generator=None):
                         # add noise
                         if noise_scale!=0:
                             s += noise_scale*sensor['noise']*self.np_random.uniform(low=-1.0, high=1.0)
+                        # ensure range
+                        s = np.clip(s, sensor['range'][0], sensor['range'][1])
                         sen.append(s)
                     current_sen[name] = np.array(sen)
 
@@ -489,19 +495,31 @@ def sensor2sim(self, sensor, sim):
             print("WARNING: Propagating noisy sensors back to sim can destablize simulation")
 
         sim.data.time = sensor['time']
+
+        # Pass 1: write invertible state — qpos and qvel survive sim.forward()
         for name, device in self.robot_config.items():
             if name == "default_robot":
                 sim.data.qpos[:] = device['sensor_data']['qpos']
                 sim.data.qvel[:] = device['sensor_data']['qvel']
-                if self.sim.model.na >0:
+                if self.sim.model.na > 0:
                     sim.data.act[:] = device['sensor_data']['act']
             else:
                 for s_id, s_val in enumerate(device['sensor']):
-                    # prompt(getattr(sim.data, s_val["data_type"])[s_val["data_id"]], sensor[name][s_id])
-                    data = getattr(sim.data, s_val["data_type"])
-                    data[s_val["data_id"]] = sensor[name][s_id]
+                    if s_val['data_type'] in ('qpos', 'qvel'):
+                        getattr(sim.data, s_val['data_type'])[s_val['data_id']] = sensor[name][s_id]
+
+        # Propagate qpos/qvel through kinematics; also recomputes sensordata from sim physics
         sim.forward()
 
+        # Pass 2: write all hardware readings into sensordata, making it the single
+        # authoritative source regardless of sensor type.  For qpos/qvel sensors this
+        # is redundant (forward() already propagated them) but keeps sensordata complete.
+        for name, device in self.robot_config.items():
+            if name == "default_robot":
+                continue
+            for s_id, s_val in enumerate(device['sensor']):
+                sim.data.sensordata[s_val['sensordata_id']] = sensor[name][s_id]
+
 
     # synchronize states between two sims
     def sync_sims(self, source_sim, destination_sim, model=True, data=True):