diff --git a/source/isaaclab/isaaclab/actuators/actuator_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_cfg.py index e5351e4fa63..b6859dded9c 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_cfg.py +++ b/source/isaaclab/isaaclab/actuators/actuator_cfg.py @@ -191,6 +191,12 @@ class IdealPDActuatorCfg(ActuatorBaseCfg): class_type: type = actuator_pd.IdealPDActuator + min_delay: int = 0 + """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + + max_delay: int = 0 + """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + @configclass class DCMotorCfg(IdealPDActuatorCfg): @@ -258,15 +264,16 @@ class DelayedPDActuatorCfg(IdealPDActuatorCfg): class_type: type = actuator_pd.DelayedPDActuator - min_delay: int = 0 - """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" - - max_delay: int = 0 - """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + def __post_init__(self): + print( + DeprecationWarning( + "DelayPDActuatorCfg has been deprecated. All functionality has been added to IdealPDActuatorCfg" + ) + ) @configclass -class RemotizedPDActuatorCfg(DelayedPDActuatorCfg): +class RemotizedPDActuatorCfg(IdealPDActuatorCfg): """Configuration for a remotized PD actuator. Note: diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 11c39f20177..ced31b96139 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -169,6 +169,15 @@ class IdealPDActuator(ActuatorBase): The parameters :math:`\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends, and :math:`\tau_{motor, max}` is the maximum motor effort possible. These parameters are read from the configuration instance passed to the class. + + An optional functionality of delay is implemented, using a circular buffer that stores the actuator commands for + a certain number of physics steps. The most recent actuation value is pushed to the buffer at every physics step, + but the final actuation value applied to the simulation is delayed by a certain number of physics steps. + + The amount of time delay is configurable and can be set to a random value between the minimum and maximum time + delay bounds at every reset. The minimum and maximum time delay values are set in the configuration instance passed + to the class. + """ cfg: IdealPDActuatorCfg @@ -178,12 +187,51 @@ class IdealPDActuator(ActuatorBase): Operations. """ + def __init__(self, cfg: IdealPDActuatorCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + if cfg.max_delay < cfg.min_delay: + raise ValueError("IdealPDActuatorCfg: max_delay must be greater than or equal to min_delay") + # instantiate the delay buffers + if cfg.max_delay > 0: + self.positions_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + self.velocities_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + self.efforts_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + # all of the envs + self._ALL_INDICES = torch.arange(self._num_envs, dtype=torch.long, device=self._device) + def reset(self, env_ids: Sequence[int]): - pass + # number of environments (since env_ids can be a slice) + if env_ids is None or env_ids == slice(None): + num_envs = self._num_envs + else: + num_envs = len(env_ids) + # set a new random delay for environments in env_ids + if self.cfg.max_delay > 0: + time_lags = torch.randint( + low=self.cfg.min_delay, + high=self.cfg.max_delay + 1, + size=(num_envs,), + dtype=torch.int, + device=self._device, + ) + # set delays + self.positions_delay_buffer.set_time_lag(time_lags, env_ids) + self.velocities_delay_buffer.set_time_lag(time_lags, env_ids) + self.efforts_delay_buffer.set_time_lag(time_lags, env_ids) + # reset buffers + self.positions_delay_buffer.reset(env_ids) + self.velocities_delay_buffer.reset(env_ids) + self.efforts_delay_buffer.reset(env_ids) def compute( self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor ) -> ArticulationActions: + # apply delay based on the delay the model for all the set points + if self.cfg.max_delay > 0: + control_action.joint_positions = self.positions_delay_buffer.compute(control_action.joint_positions) + control_action.joint_velocities = self.velocities_delay_buffer.compute(control_action.joint_velocities) + control_action.joint_efforts = self.efforts_delay_buffer.compute(control_action.joint_efforts) + # compute errors error_pos = control_action.joint_positions - joint_pos error_vel = control_action.joint_velocities - joint_vel @@ -307,14 +355,8 @@ def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: class DelayedPDActuator(IdealPDActuator): """Ideal PD actuator with delayed command application. - This class extends the :class:`IdealPDActuator` class by adding a delay to the actuator commands. The delay - is implemented using a circular buffer that stores the actuator commands for a certain number of physics steps. - The most recent actuation value is pushed to the buffer at every physics step, but the final actuation value - applied to the simulation is lagged by a certain number of physics steps. - - The amount of time lag is configurable and can be set to a random value between the minimum and maximum time - lag bounds at every reset. The minimum and maximum time lag values are set in the configuration instance passed - to the class. + .. deprecated:: 2.2.0 + Please use :class:`IdealPDActuator` instead. """ cfg: DelayedPDActuatorCfg @@ -322,52 +364,17 @@ class DelayedPDActuator(IdealPDActuator): def __init__(self, cfg: DelayedPDActuatorCfg, *args, **kwargs): super().__init__(cfg, *args, **kwargs) - # instantiate the delay buffers - self.positions_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) - self.velocities_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) - self.efforts_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) - # all of the envs - self._ALL_INDICES = torch.arange(self._num_envs, dtype=torch.long, device=self._device) - - def reset(self, env_ids: Sequence[int]): - super().reset(env_ids) - # number of environments (since env_ids can be a slice) - if env_ids is None or env_ids == slice(None): - num_envs = self._num_envs - else: - num_envs = len(env_ids) - # set a new random delay for environments in env_ids - time_lags = torch.randint( - low=self.cfg.min_delay, - high=self.cfg.max_delay + 1, - size=(num_envs,), - dtype=torch.int, - device=self._device, + print( + DeprecationWarning( + "DelayPDActuator has been deprecated. All functionality has been added to IdealPDActuator" + ) ) - # set delays - self.positions_delay_buffer.set_time_lag(time_lags, env_ids) - self.velocities_delay_buffer.set_time_lag(time_lags, env_ids) - self.efforts_delay_buffer.set_time_lag(time_lags, env_ids) - # reset buffers - self.positions_delay_buffer.reset(env_ids) - self.velocities_delay_buffer.reset(env_ids) - self.efforts_delay_buffer.reset(env_ids) - - def compute( - self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor - ) -> ArticulationActions: - # apply delay based on the delay the model for all the setpoints - control_action.joint_positions = self.positions_delay_buffer.compute(control_action.joint_positions) - control_action.joint_velocities = self.velocities_delay_buffer.compute(control_action.joint_velocities) - control_action.joint_efforts = self.efforts_delay_buffer.compute(control_action.joint_efforts) - # compte actuator model - return super().compute(control_action, joint_pos, joint_vel) -class RemotizedPDActuator(DelayedPDActuator): +class RemotizedPDActuator(IdealPDActuator): """Ideal PD actuator with angle-dependent torque limits. - This class extends the :class:`DelayedPDActuator` class by adding angle-dependent torque limits to the actuator. + This class extends the :class:`IdealPDActuator` class by adding angle-dependent torque limits to the actuator. The torque limits are applied by querying a lookup table describing the relationship between the joint angle and the maximum output torque. The lookup table is provided in the configuration instance passed to the class. diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py index b1ca5a3f693..c4177d3a1ba 100644 --- a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -16,7 +16,8 @@ import pytest -from isaaclab.actuators import IdealPDActuatorCfg +from isaaclab.actuators import IdealPDActuator, IdealPDActuatorCfg +from isaaclab.utils.buffers import DelayBuffer from isaaclab.utils.types import ArticulationActions @@ -270,5 +271,160 @@ def test_ideal_pd_compute(num_envs, num_joints, device, effort_lim): ) +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_ideal_pd_actuator_init_delay(num_envs, num_joints, device): + """Test initialization of ideal pd actuator with delay.""" + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 200 + damping = 10 + effort_limit = 60.0 + velocity_limit = 50 + delay = 5 + + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_limit, + velocity_limit=velocity_limit, + min_delay=delay, + max_delay=delay, + ) + + # assume Articulation class: + # - finds joints (names and ids) associate with the provided joint_names_expr + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + ) + assert isinstance(actuator, IdealPDActuator) + # check device and shape + torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close( + actuator.effort_limit, + effort_limit * torch.ones(num_envs, num_joints, device=device), + ) + torch.testing.assert_close( + actuator.velocity_limit, velocity_limit * torch.ones(num_envs, num_joints, device=device) + ) + + # check delay buffers + assert isinstance(actuator.positions_delay_buffer, DelayBuffer) + assert isinstance(actuator.velocities_delay_buffer, DelayBuffer) + assert isinstance(actuator.efforts_delay_buffer, DelayBuffer) + + assert actuator.positions_delay_buffer.history_length == delay + assert actuator.velocities_delay_buffer.history_length == delay + assert actuator.efforts_delay_buffer.history_length == delay + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("effort_lim", [None, 80]) +def test_delay_pd_actuator_compute(num_envs, num_joints, device, effort_lim): + """Test the computation of the delay pd actuator.""" + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 20 + damping = 1 + effort_limit = effort_lim + velocity_limit = 50 + delay = 3 + # configure actuator + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_limit, + velocity_limit=velocity_limit, + min_delay=delay, + max_delay=delay, + ) + # instantiate actuator + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + stiffness=actuator_cfg.stiffness, + damping=actuator_cfg.damping, + ) + # requires a restart to create the delay + actuator.reset(range(num_envs)) + + desired_pos = [2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0] + desired_vel = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0] + feedforward_effort = [0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0] + + measured_joint_pos = 1.0 + measured_joint_vel = 0.1 + + # check delay buffer filling properly + for i in range(delay + 4): + + desired_control_action = ArticulationActions() + desired_control_action.joint_positions = desired_pos[i] * torch.ones(num_envs, num_joints, device=device) + desired_control_action.joint_velocities = desired_vel[i] * torch.ones(num_envs, num_joints, device=device) + desired_control_action.joint_efforts = feedforward_effort[i] * torch.ones(num_envs, num_joints, device=device) + + computed_control_action = actuator.compute( + desired_control_action, + measured_joint_pos * torch.ones(num_envs, num_joints, device=device), + measured_joint_vel * torch.ones(num_envs, num_joints, device=device), + ) + + if i <= delay: + expect = ( + stiffness * (desired_pos[0] - measured_joint_pos) + + damping * (desired_vel[0] - measured_joint_vel) + + feedforward_effort[0] + ) + else: + expect = ( + stiffness * (desired_pos[i - delay] - measured_joint_pos) + + damping * (desired_vel[i - delay] - measured_joint_vel) + + feedforward_effort[i - delay] + ) + + if effort_lim is not None: + expect_apply = min(expect, effort_lim) + else: + expect_apply = expect + + torch.testing.assert_close( + expect * torch.ones(num_envs, num_joints, device=device), + actuator.computed_effort, + ) + torch.testing.assert_close( + expect_apply * torch.ones(num_envs, num_joints, device=device), + actuator.applied_effort, + ) + torch.testing.assert_close( + actuator.applied_effort, + computed_control_action.joint_efforts, + ) + # check reset single env + actuator.reset([0]) + assert actuator.positions_delay_buffer._circular_buffer._num_pushes[0] == 0 + assert actuator.velocities_delay_buffer._circular_buffer._num_pushes[0] == 0 + assert actuator.efforts_delay_buffer._circular_buffer._num_pushes[0] == 0 + if num_envs > 1: + assert actuator.positions_delay_buffer._circular_buffer._num_pushes[1] == i + 1 + assert actuator.velocities_delay_buffer._circular_buffer._num_pushes[1] == i + 1 + assert actuator.efforts_delay_buffer._circular_buffer._num_pushes[1] == i + 1 + # check actuator reset all + actuator.reset(range(num_envs)) + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/spot.py b/source/isaaclab_assets/isaaclab_assets/robots/spot.py index 6513484965a..a04d13a00da 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/spot.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/spot.py @@ -12,7 +12,7 @@ """ import isaaclab.sim as sim_utils -from isaaclab.actuators import DelayedPDActuatorCfg, RemotizedPDActuatorCfg +from isaaclab.actuators import IdealPDActuatorCfg, RemotizedPDActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -160,7 +160,7 @@ joint_vel={".*": 0.0}, ), actuators={ - "spot_hip": DelayedPDActuatorCfg( + "spot_hip": IdealPDActuatorCfg( joint_names_expr=[".*_h[xy]"], effort_limit=45.0, stiffness=60.0,