From b6f7e9ad0e37d24b2a92335e9c5060fe3c424b24 Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Fri, 28 Aug 2026 13:22:50 -0400 Subject: [PATCH 1/3] Update robot.py --- robohive/robot/robot.py | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 89668241..6e877d12 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -127,6 +127,13 @@ def __init__(self, # refresh the sensor cache self._sensor_cache_refresh() + # Hardware mode's last-feasible-control cache -- see process_actuator()'s use of + # it below. sim_id -> last feasible (post-clip) control value. Empty until the + # first process_actuator() call for a given actuator; cleared by hardware_reset() + # since a physical reset bypasses process_actuator() entirely and would otherwise + # leave this stale relative to the arm's new pose. + self._last_feasible_ctrl = {} + # Check if all hardware components are okay def hardware_okay(self, robot_config): @@ -216,6 +223,10 @@ def hardware_reset(self, reset_pos, space='sim'): 'hdr' (per-actuator vector ordered by hdr_id, as in hardware_apply_controls) """ assert space in ['sim', 'hdr'], "space must be 'sim' or 'hdr'" + # A physical reset bypasses process_actuator() entirely -- clear its + # last-feasible-control cache so the next call falls back to a fresh sensor + # read (the arm's real new pose) instead of anchoring against a pre-reset value. + self._last_feasible_ctrl = {} for name, device in self.robot_config.items(): if name == 'default_robot': continue @@ -602,15 +613,23 @@ def process_actuator(self, controls, step_duration, out_id = actuator['sim_id'] if out_space == 'sim' else actuator['hdr_id'] control = controls[in_id] + # Anchor the velocity-limit clip on our OWN last feasible (already- + # clipped) control instead of a hardware sensor read, in hardware mode + # only -- see process_actuator()'s use below and _last_feasible_ctrl's + # comment in __init__. In sim mode, sim.data is exactly what physics + # last did (perfectly synchronized), so the original sensor-based + # anchor is unchanged/still correct there. + if self.is_hardware and actuator['sim_id'] in self._last_feasible_ctrl: + last_obs = self._last_feasible_ctrl[actuator['sim_id']] + else: + last_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] if self._act_mode == "pos": # remap to the limits if normalized if normalized: control = (actuator['pos_range'][1]+actuator['pos_range'][0])/2.0 + \ control*(actuator['pos_range'][1]-actuator['pos_range'][0])/2.0 # enforce velocity limits - # ALERT: This depends on previous sensor. This is not ideal as it breaks MDP addumptions. Be careful if velocity_limits: - last_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] ctrl_desired_vel = (control - last_obs)/step_duration ctrl_feasible_vel = np.clip(ctrl_desired_vel, actuator['vel_range'][0], actuator['vel_range'][1]) control = last_obs + ctrl_feasible_vel*step_duration @@ -620,8 +639,6 @@ def process_actuator(self, controls, step_duration, control = (actuator['vel_range'][1]+actuator['vel_range'][0])/2.0 + \ control*(actuator['vel_range'][1]-actuator['vel_range'][0])/2.0 # enforce velocity limits - # ALERT: This depends on previous sensor. This is not ideal as it breaks MDP addumptions. Be careful - last_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] control = last_obs + control*step_duration else: raise TypeError("Unknown act mode: {}".format(self._act_mode)) @@ -630,6 +647,9 @@ def process_actuator(self, controls, step_duration, if position_limits: control = np.clip(control, actuator['pos_range'][0], actuator['pos_range'][1]) + if self.is_hardware: + self._last_feasible_ctrl[actuator['sim_id']] = control + # remap to desired space processed_controls[out_id] = control From 91a696dbd2022fab817a322179643ff6cdb5fc9d Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Wed, 2 Sep 2026 12:33:29 -0400 Subject: [PATCH 2/3] better handling of the velocity limit --- robohive/robot/robot.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 6e877d12..cb175087 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -613,16 +613,13 @@ def process_actuator(self, controls, step_duration, out_id = actuator['sim_id'] if out_space == 'sim' else actuator['hdr_id'] control = controls[in_id] - # Anchor the velocity-limit clip on our OWN last feasible (already- - # clipped) control instead of a hardware sensor read, in hardware mode - # only -- see process_actuator()'s use below and _last_feasible_ctrl's - # comment in __init__. In sim mode, sim.data is exactly what physics - # last did (perfectly synchronized), so the original sensor-based - # anchor is unchanged/still correct there. + sim_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] + # Default anchor for act_mode=="vel" (no saturation-aware choice there -- + # see below) and as the pre-first-tick fallback for act_mode=="pos". if self.is_hardware and actuator['sim_id'] in self._last_feasible_ctrl: last_obs = self._last_feasible_ctrl[actuator['sim_id']] else: - last_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] + last_obs = sim_obs if self._act_mode == "pos": # remap to the limits if normalized if normalized: @@ -630,6 +627,15 @@ def process_actuator(self, controls, step_duration, control*(actuator['pos_range'][1]-actuator['pos_range'][0])/2.0 # enforce velocity limits if velocity_limits: + if self.is_hardware: + # Anchor on the real position unless doing so would saturate + # this clip -- only then fall back to our own last feasible + # control (avoids fighting the hardware's own tracking loop). + sim_desired_vel = (control - sim_obs) / step_duration + if actuator['vel_range'][0] <= sim_desired_vel <= actuator['vel_range'][1]: + last_obs = sim_obs + else: + last_obs = sim_obs ctrl_desired_vel = (control - last_obs)/step_duration ctrl_feasible_vel = np.clip(ctrl_desired_vel, actuator['vel_range'][0], actuator['vel_range'][1]) control = last_obs + ctrl_feasible_vel*step_duration From d27af052c5b46f37f0479be358a40652bc89f8f7 Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 22 Sep 2026 10:01:32 -0400 Subject: [PATCH 3/3] anchor to control as an option --- robohive/robot/robot.py | 39 +++++++++++++++++---------------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index cb175087..58b58c23 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -59,6 +59,7 @@ def __init__(self, sensor_cache_maxsize = 5, # cache size for sensors noise_scale = 0, # scale for sensor noise random_generator = None, # random number generator + vel_anchor: str = "sensor", # sensor / control: velocity-limit reference (hardware) **kwargs, ): @@ -67,6 +68,8 @@ def __init__(self, self.name = robot_name+'(sim)' if is_hardware is None else robot_name+'(hdr)' self._act_mode = act_mode self.is_hardware = bool(is_hardware) + assert vel_anchor in ('sensor', 'control'), "unknown vel_anchor: {}".format(vel_anchor) + self._vel_anchor = vel_anchor self._sensor_cache_maxsize = sensor_cache_maxsize self._noise_scale = noise_scale if random_generator == None: @@ -127,11 +130,7 @@ def __init__(self, # refresh the sensor cache self._sensor_cache_refresh() - # Hardware mode's last-feasible-control cache -- see process_actuator()'s use of - # it below. sim_id -> last feasible (post-clip) control value. Empty until the - # first process_actuator() call for a given actuator; cleared by hardware_reset() - # since a physical reset bypasses process_actuator() entirely and would otherwise - # leave this stale relative to the arm's new pose. + # sim_id -> last control sent, the reference for vel_anchor=="control" self._last_feasible_ctrl = {} @@ -223,9 +222,7 @@ def hardware_reset(self, reset_pos, space='sim'): 'hdr' (per-actuator vector ordered by hdr_id, as in hardware_apply_controls) """ assert space in ['sim', 'hdr'], "space must be 'sim' or 'hdr'" - # A physical reset bypasses process_actuator() entirely -- clear its - # last-feasible-control cache so the next call falls back to a fresh sensor - # read (the arm's real new pose) instead of anchoring against a pre-reset value. + # a physical reset bypasses process_actuator(), invalidating its reference self._last_feasible_ctrl = {} for name, device in self.robot_config.items(): if name == 'default_robot': @@ -590,12 +587,18 @@ def process_actuator(self, controls, step_duration, normalized=True, position_limits=True, velocity_limits=True, - out_space='sim'): + out_space='sim', + vel_anchor=None): """ Process the actuation demands to (1) Remap provided controls to actuation space, (2) Enforces hardware position and velocity limits on the controls + INPUTS: + vel_anchor: 'sensor'/'control' reference for the velocity limit (hardware only, + sim always uses the sensor). None uses the robot's configured default. """ + vel_anchor = self._vel_anchor if vel_anchor is None else vel_anchor + assert vel_anchor in ('sensor', 'control'), "unknown vel_anchor: {}".format(vel_anchor) # last_obs = self.get_sensor_from_cache(-1) processed_controls = controls.copy() for name, device in self.robot_config.items(): @@ -614,10 +617,11 @@ def process_actuator(self, controls, step_duration, control = controls[in_id] sim_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_id"]] - # Default anchor for act_mode=="vel" (no saturation-aware choice there -- - # see below) and as the pre-first-tick fallback for act_mode=="pos". - if self.is_hardware and actuator['sim_id'] in self._last_feasible_ctrl: - last_obs = self._last_feasible_ctrl[actuator['sim_id']] + # Hardware's sensor read lags the control still being tracked, so it + # understates the room left in vel_range. 'control' measures against + # the last command instead, falling back to the sensor on the 1st tick. + if self.is_hardware and vel_anchor == "control": + last_obs = self._last_feasible_ctrl.get(actuator['sim_id'], sim_obs) else: last_obs = sim_obs if self._act_mode == "pos": @@ -627,15 +631,6 @@ def process_actuator(self, controls, step_duration, control*(actuator['pos_range'][1]-actuator['pos_range'][0])/2.0 # enforce velocity limits if velocity_limits: - if self.is_hardware: - # Anchor on the real position unless doing so would saturate - # this clip -- only then fall back to our own last feasible - # control (avoids fighting the hardware's own tracking loop). - sim_desired_vel = (control - sim_obs) / step_duration - if actuator['vel_range'][0] <= sim_desired_vel <= actuator['vel_range'][1]: - last_obs = sim_obs - else: - last_obs = sim_obs ctrl_desired_vel = (control - last_obs)/step_duration ctrl_feasible_vel = np.clip(ctrl_desired_vel, actuator['vel_range'][0], actuator['vel_range'][1]) control = last_obs + ctrl_feasible_vel*step_duration