From 3f4ab07e5c703fba43e662dceb5ef540a06e987f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 19:49:25 -0400 Subject: [PATCH 1/4] 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/4] 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): From 70f43e77e6d9ed9c2c29ba0963559c9b6344a9c6 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 7 Jul 2026 16:53:10 -0400 Subject: [PATCH 3/4] BUGFIX: squeeze_dims collapsing single-value obs into unconcatenatable 0-d scalars squeeze_dims() previously did a blanket np.squeeze(), which removed every singleton dimension instead of just the (num_traj=1, horizon=1) wrapper added by expand_dims(). Single-value observations (e.g. shape (1,)) were squeezed all the way down to 0-d scalars, which then failed to concatenate with other proprio/obs vectors in get_proprioception() and obsdict2obsvec(). Now only squeeze the leading axes that expand_dims actually added, leaving real data dimensions (and pre-existing scalar rwd_dict entries) untouched. --- robohive/envs/obs_vec_dict.py | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/robohive/envs/obs_vec_dict.py b/robohive/envs/obs_vec_dict.py index 4dece6bb..36a771b0 100644 --- a/robohive/envs/obs_vec_dict.py +++ b/robohive/envs/obs_vec_dict.py @@ -63,10 +63,29 @@ def initialize(self, obs_dict, ordered_obs_keys): t, obsvec = self.obsdict2obsvec(obs_dict, ordered_obs_keys) self.obsvec_cache_flush(t, obsvec) # populate the cache with initial obsvec values - # Squeeze out singleton dimensions + # Squeeze out the (num_traj=1, horizon=1) dims added by expand_dims. + # Squeezing all singleton dims (np.squeeze(arr)) would also collapse + # single-value obs (e.g. shape (1,)) down to 0-d scalars, which then + # fail to concatenate with other obs vectors downstream. def squeeze_dims(self, obs_dict): for key in obs_dict.keys(): - obs_dict[key] = np.squeeze(obs_dict[key]) + val = np.asarray(obs_dict[key]) + + # expand_dims() only ever adds two leading dims: (num_traj=1, horizon=1, ...). + # Some dict values (e.g. scalar entries in rwd_dict like 'done', 'solved') + # never went through expand_dims, so they may have 0 or 1 dims. Only squeeze + # axis 0 and/or axis 1 if they actually exist AND are singleton -- this way we + # never touch a real data dimension (e.g. a single-value obs of shape (1,)). + axes_to_squeeze = [] + if val.ndim > 0 and val.shape[0] == 1: + axes_to_squeeze.append(0) + if val.ndim > 1 and val.shape[1] == 1: + axes_to_squeeze.append(1) + + if axes_to_squeeze: + val = np.squeeze(val, axis=tuple(axes_to_squeeze)) + + obs_dict[key] = val return obs_dict # Exapand observation dimensions to (num_traj=1, horizon=1, obs_dim) From 157aa4c24f46c04e1159cb9baedd2903c2cdb18f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Thu, 9 Jul 2026 15:10:13 -0400 Subject: [PATCH 4/4] FEATURE: Adding a warning if time key is not found --- robohive/robot/hardware_base.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 19a0b565..6b003d69 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -7,6 +7,8 @@ # Base robot class for other hardware devices to inheret from import abc +import warnings + class hardwareBase(abc.ABC): def __init__(self, name, *args, **kwargs) -> None: @@ -35,8 +37,10 @@ def _get_sensors(self) -> dict: 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)}" + if not (isinstance(data, dict) and 'time' in data): + warnings.warn( + f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}. " + "Please add 'time' details to your sensor data to suppress this warning.") return data @abc.abstractmethod