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) 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):