diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 89668241..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,6 +130,9 @@ def __init__(self, # refresh the sensor cache self._sensor_cache_refresh() + # sim_id -> last control sent, the reference for vel_anchor=="control" + self._last_feasible_ctrl = {} + # Check if all hardware components are okay def hardware_okay(self, robot_config): @@ -216,6 +222,8 @@ 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(), invalidating its reference + self._last_feasible_ctrl = {} for name, device in self.robot_config.items(): if name == 'default_robot': continue @@ -579,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(): @@ -602,15 +616,21 @@ def process_actuator(self, controls, step_duration, out_id = actuator['sim_id'] if out_space == 'sim' else actuator['hdr_id'] control = controls[in_id] + sim_obs = getattr(self.sim.data, actuator["data_type"])[actuator["data_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": # 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 +640,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 +648,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