From 3f4ab07e5c703fba43e662dceb5ef540a06e987f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 19:49:25 -0400 Subject: [PATCH 1/2] UPGRADE: Adding return types to the base class for consistency and check for existance of 'time' key for sensors --- robohive/robot/hardware_base.py | 25 ++++++++++++++++--------- robohive/robot/hardware_dynamixel.py | 2 +- robohive/robot/hardware_franka.py | 6 +++--- robohive/robot/hardware_optitrack.py | 2 +- robohive/robot/hardware_realsense.py | 2 +- robohive/robot/hardware_robotiq.py | 6 +++--- 6 files changed, 25 insertions(+), 18 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index ef6944a0..19a0b565 100644 --- a/robohive/robot/hardware_base.py +++ b/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() \ No newline at end of file diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 6c769f18..16c99c55 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/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): diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index bb2b89d9..7a2b8f07 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -185,7 +185,7 @@ 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() @@ -193,8 +193,8 @@ def get_sensors(self): 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): diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 9a96e3b5..5aab551b 100644 --- a/robohive/robot/hardware_optitrack.py +++ b/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 diff --git a/robohive/robot/hardware_realsense.py b/robohive/robot/hardware_realsense.py index 0da3ec03..faa68e5f 100644 --- a/robohive/robot/hardware_realsense.py +++ b/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) diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py index 27b4e2f0..76182c8d 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/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) From e09299ac35527c086b5088f363f1faa0a8c5d999 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 20:13:30 -0400 Subject: [PATCH 2/2] FEATURE: Adding support for multiple kinds of sensors that can now carry complex data from the hardware into sensordata field of the model. Reply on sensordata field to create obs and obs_dicts --- robohive/robot/robot.py | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index f95f48e5..34279954 100644 --- a/robohive/robot/robot.py +++ b/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):