From 9f61b5d89344958ce9960b17b0f058db17313930 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Thu, 12 Jun 2025 17:23:27 +0200 Subject: [PATCH 1/6] Running step. Need to add some new ones to see if the cliping works... --- .../isaaclab/envs/mdp/actions/actions_cfg.py | 111 +++++++++ .../envs/mdp/actions/binary_joint_actions.py | 15 -- .../envs/mdp/actions/non_holonomic_actions.py | 8 +- .../envs/mdp/actions/task_space_actions.py | 226 +++++++++++++++--- .../isaaclab/managers/manager_term_cfg.py | 3 - 5 files changed, 317 insertions(+), 46 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py index 9c932d227b0..9426ae7fd47 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py @@ -25,13 +25,19 @@ class JointActionCfg(ActionTermCfg): joint_names: list[str] = MISSING """List of joint names or regex expressions that the action will be mapped to.""" + scale: float | dict[str, float] = 1.0 """Scale factor for the action (float or dict of regex expressions). Defaults to 1.0.""" + offset: float | dict[str, float] = 0.0 """Offset factor for the action (float or dict of regex expressions). Defaults to 0.0.""" + preserve_order: bool = False """Whether to preserve the order of the joint names in the action output. Defaults to False.""" + clip: dict[str, tuple] | None = None + """Clip range for the action (dict of regex expressions). Defaults to None.""" + @configclass class JointPositionActionCfg(JointActionCfg): @@ -124,6 +130,9 @@ class JointPositionToLimitsActionCfg(ActionTermCfg): This operation is performed after applying the scale factor. """ + clip: dict[str, tuple] | None = None + """Clip range for the action (dict of regex expressions). Defaults to None.""" + @configclass class EMAJointPositionToLimitsActionCfg(JointPositionToLimitsActionCfg): @@ -155,8 +164,10 @@ class BinaryJointActionCfg(ActionTermCfg): joint_names: list[str] = MISSING """List of joint names or regex expressions that the action will be mapped to.""" + open_command_expr: dict[str, float] = MISSING """The joint command to move to *open* configuration.""" + close_command_expr: dict[str, float] = MISSING """The joint command to move to *close* configuration.""" @@ -197,17 +208,28 @@ class NonHolonomicActionCfg(ActionTermCfg): body_name: str = MISSING """Name of the body which has the dummy mechanism connected to.""" + x_joint_name: str = MISSING """The dummy joint name in the x direction.""" + y_joint_name: str = MISSING """The dummy joint name in the y direction.""" + yaw_joint_name: str = MISSING """The dummy joint name in the yaw direction.""" + scale: tuple[float, float] = (1.0, 1.0) """Scale factor for the action. Defaults to (1.0, 1.0).""" + offset: tuple[float, float] = (0.0, 0.0) """Offset factor for the action. Defaults to (0.0, 0.0).""" + clip: dict[str, tuple] | None = None + """Clip range for the action (dict of regex expressions). + + The expected keys are "v", and "yaw". Defaults to None for no clipping. + """ + ## # Task-space Actions. @@ -240,15 +262,49 @@ class OffsetCfg: joint_names: list[str] = MISSING """List of joint names or regex expressions that the action will be mapped to.""" + body_name: str = MISSING """Name of the body or frame for which IK is performed.""" + body_offset: OffsetCfg | None = None """Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied.""" + scale: float | tuple[float, ...] = 1.0 """Scale factor for the action. Defaults to 1.0.""" + controller: DifferentialIKControllerCfg = MISSING """The configuration for the differential IK controller.""" + # TODO: Should this be simplified to a list of tuples? More compact, less readable? + # TODO: Do we want to have an homogeneous behavior for the clip range? I think we do + # or we'd need unique clip names so that it's not confusing to the user. + + clip: dict[str, tuple] | None = None + """Clip range of the controller's command in the world frame (dict of regex expressions). + + The expected keys are "position", "orientation", and "wrench". Defaults to None for no clipping. + For "position" we expect a tuple of (min, max) for each dimension. (x, y, z) in this order. + For "orientation" we expect a tuple of (min, max) for each dimension. (roll, pitch, yaw) in this order. + + Example: + ..code-block:: python + { + "position": ((-1.0, 1.0), (-1.0, 1.0), (-1.0, 1.0)), # (x, y, z) + "orientation": ((-1.0, 1.0), (-1.0, 1.0), (-1.0, 1.0)), # (roll, pitch, yaw) + } + + ..note:: + This means that regardless of the :attr:`controller.use_relative_mode` setting, the clip range is always + applied in the world frame. This is done so that the meaning of the clip range is consistent across + different modes. + + ..note:: + If the :attr:`controller.command_type` is set to "pose", then both the position and orientation clip ranges + must be provided. To clip either one or the other, one can set large values to the clip range. + + + """ + @configclass class OperationalSpaceControllerActionCfg(ActionTermCfg): @@ -310,3 +366,58 @@ class OffsetCfg: Note: Functional only when ``nullspace_control`` is set to ``"position"`` within the ``OperationalSpaceControllerCfg``. """ + + # TODO: Here the clip effects are not homogeneous, but they have unique names that relate + # to specific control modes so it's fine to me. + + clip_pose_abs: list[tuple[float, float]] | None = None + """Clip range for the absolute pose targets. Defaults to None for no clipping. + + The expected format is a list of tuples, each containing two values. This effectively bounds + the reachable range of the end-effector in the world frame. + + Example: + ..code-block:: python + clip_pose_abs = [ + (min_x, max_x), + (min_y, max_y), + (min_z, max_z), + (min_roll, max_roll), + (min_pitch, max_pitch), + (min_yaw, max_yaw), + ] + """ + clip_pose_rel: list[tuple[float, float]] | None = None + """Clip range for the relative pose targets. Defaults to None for no clipping. + + The expected format is a list of tuples, each containing two values. This effectively limits + the end-effector's velocity in the task frame. + + Example: + ..code-block:: python + clip_pose_rel = [ + (min_x, max_x), + (min_y, max_y), + (min_z, max_z), + (min_roll, max_roll), + (min_pitch, max_pitch), + (min_yaw, max_yaw), + ] + """ + clip_wrench_abs: list[tuple[float, float]] | None = None + """Clip range for the absolute wrench targets. Defaults to None for no clipping. + + The expected format is a list of tuples, each containing two values. This effectively limits + the maximum force and torque that can be commanded in the task frame. + + Example: + ..code-block:: python + clip_wrench_abs = [ + (min_fx, max_fx), + (min_fy, max_fy), + (min_fz, max_fz), + (min_tx, max_tx), + (min_ty, max_ty), + (min_tz, max_tz), + ] + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py index f3a4fa37af1..dc75c90505d 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/binary_joint_actions.py @@ -84,17 +84,6 @@ def __init__(self, cfg: actions_cfg.BinaryJointActionCfg, env: ManagerBasedEnv) ) self._close_command[index_list] = torch.tensor(value_list, device=self.device) - # parse clip - if self.cfg.clip is not None: - if isinstance(cfg.clip, dict): - self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( - self.num_envs, self.action_dim, 1 - ) - index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) - self._clip[:, index_list] = torch.tensor(value_list, device=self.device) - else: - raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") - """ Properties. """ @@ -127,10 +116,6 @@ def process_actions(self, actions: torch.Tensor): binary_mask = actions < 0 # compute the command self._processed_actions = torch.where(binary_mask, self._close_command, self._open_command) - if self.cfg.clip is not None: - self._processed_actions = torch.clamp( - self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] - ) def reset(self, env_ids: Sequence[int] | None = None) -> None: self._raw_actions[env_ids] = 0.0 diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py index a3e5cebdbf5..2b0e61fa83e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/non_holonomic_actions.py @@ -109,11 +109,17 @@ def __init__(self, cfg: actions_cfg.NonHolonomicActionCfg, env: ManagerBasedEnv) self._offset = torch.tensor(self.cfg.offset, device=self.device).unsqueeze(0) # parse clip if self.cfg.clip is not None: + allowed_clipping_keys = ["v", "yaw"] if isinstance(cfg.clip, dict): + for key in self.cfg.clip.keys(): + if key not in allowed_clipping_keys: + raise ValueError(f"Unsupported clip key: {key}. Supported keys are {allowed_clipping_keys}.") self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( self.num_envs, self.action_dim, 1 ) - index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + index_list, _, value_list = string_utils.resolve_matching_names_values( + self.cfg.clip, allowed_clipping_keys + ) self._clip[:, index_list] = torch.tensor(value_list, device=self.device) else: raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index 89f51817179..84b34cdf323 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -13,7 +13,6 @@ from pxr import UsdPhysics import isaaclab.utils.math as math_utils -import isaaclab.utils.string as string_utils from isaaclab.assets.articulation import Articulation from isaaclab.controllers.differential_ik import DifferentialIKController from isaaclab.controllers.operational_space import OperationalSpaceController @@ -109,13 +108,78 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: self._offset_pos, self._offset_rot = None, None # parse clip - if self.cfg.clip is not None: - if isinstance(cfg.clip, dict): - self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( - self.num_envs, self.action_dim, 1 - ) - index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) - self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + if cfg.clip is not None: + if isinstance(self.cfg.clip, dict): + if self.cfg.controller.command_type == "position": + # Assign independent clip for each dimension of the position command + allowed_clipping_keys = ["position"] + # Check if the clip keys are supported + for key in self.cfg.clip.keys(): + if key not in allowed_clipping_keys: + raise ValueError( + f"Unsupported clip key: {key}. Supported keys are {allowed_clipping_keys}." + ) + # Check if the position key is present + if "position" not in self.cfg.clip.keys(): + raise ValueError(f"Unsupported clip key: {key}. Supported keys are {allowed_clipping_keys}.") + # Check if the position key has 3 dimensions + if len(self.cfg.clip["position"]) != 3: + raise ValueError( + f"Expected 3 dimensions for position command. Found {len(self.cfg.clip['position'])}." + ) + # Check that each tuple in the position key has 2 values + for tuple in self.cfg.clip["position"]: + if len(tuple) != 2: + raise ValueError(f"Expected tuple of (min, max) for position command. Found {tuple}.") + # Create the clip tensor + self._clip = torch.tensor(self.num_envs, 3, 2, device=self.device) + self._clip[:, 0] = torch.tensor(self.cfg.clip["position"][0], device=self.device) + self._clip[:, 1] = torch.tensor(self.cfg.clip["position"][1], device=self.device) + self._clip[:, 2] = torch.tensor(self.cfg.clip["position"][2], device=self.device) + elif self.cfg.controller.command_type == "pose": + allowed_clipping_keys = ["position", "orientation"] + # Check if the clip keys are supported + for key in self.cfg.clip.keys(): + if key not in allowed_clipping_keys: + raise ValueError( + f"Unsupported clip key: {key}. Supported keys are {allowed_clipping_keys}." + ) + # Check if the position key is present + if "position" not in self.cfg.clip.keys(): + raise ValueError("Missing required `position` key in clip dictionary.") + if "orientation" not in self.cfg.clip.keys(): + raise ValueError("Missing required `orientation` key in clip dictionary.") + # Check if the position key has 3 dimensions + if len(self.cfg.clip["position"]) != 3: + raise ValueError( + f"Expected 3 dimensions for position command. Found {len(self.cfg.clip['position'])}." + ) + # Check if the orientation key has 3 dimensions + if len(self.cfg.clip["orientation"]) != 3: + raise ValueError( + f"Expected 3 dimensions for orientation command. Found {len(self.cfg.clip['orientation'])}." + ) + # Check that each tuple in the position key has 2 values + for tuple in self.cfg.clip["position"]: + if len(tuple) != 2: + raise ValueError(f"Expected tuple of (min, max) for position command. Found {tuple}.") + # Check that each tuple in the orientation key has 2 values + for tuple in self.cfg.clip["orientation"]: + if len(tuple) != 2: + raise ValueError(f"Expected tuple of (min, max) for orientation command. Found {tuple}.") + # Create the clip tensor + self._clip = torch.tensor(self.num_envs, 7, 2, device=self.device) + self._clip[:, 0] = torch.tensor(self.cfg.clip["position"][0], device=self.device) + self._clip[:, 1] = torch.tensor(self.cfg.clip["position"][1], device=self.device) + self._clip[:, 2] = torch.tensor(self.cfg.clip["position"][2], device=self.device) + self._clip[:, 3] = torch.tensor(self.cfg.clip["orientation"][0], device=self.device) + self._clip[:, 4] = torch.tensor(self.cfg.clip["orientation"][1], device=self.device) + self._clip[:, 5] = torch.tensor(self.cfg.clip["orientation"][2], device=self.device) + else: + raise ValueError( + f"Unsupported command type: {self.cfg.controller.command_type}. Supported types are `position`" + " and `pose`." + ) else: raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") @@ -156,12 +220,66 @@ def process_actions(self, actions: torch.Tensor): # store the raw actions self._raw_actions[:] = actions self._processed_actions[:] = self.raw_actions * self._scale - if self.cfg.clip is not None: - self._processed_actions = torch.clamp( - self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] - ) # obtain quantities from simulation ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + # clip the actions if needed + if self.cfg.clip is not None: + if self.cfg.controller.use_relative_mode: + if self.cfg.controller.command_type == "position": + # Add the current position to the target position to get the target position in the world frame + target_position_w = self._processed_actions[:, :3] + ee_pos_curr + # Clip the target position in the world frame + clamped_target_position_w = torch.clamp( + target_position_w, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + # Subtract the current position to get the target position in the body frame + self._processed_actions[:, :3] = clamped_target_position_w - ee_pos_curr + elif self.cfg.controller.command_type == "pose": + # Apply the delta pose to the current pose to get the target pose in the world frame + target_position_w, target_quat_w = math_utils.apply_delta_pose( + ee_pos_curr, ee_quat_curr, self._processed_actions + ) + # Cast the target_quat_w to euler angles + target_euler_angles_w = math_utils.euler_xyz_from_quat(target_quat_w) + # Clip the pose + clamped_target_position_w = torch.clamp( + target_position_w, min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] + ) + clamped_target_euler_angles_w = torch.clamp( + target_euler_angles_w, min=self._clip[:, 3:, 0], max=self._clip[:, 3:, 1] + ) + # Subtract the current orientation to get the target orientation in the world frame and apply module [-pi, pi] + clamped_target_euler_angles_rel = math_utils.wrap_to_pi( + clamped_target_euler_angles_w - self._processed_actions[:, 3:6] + ) + # Apply the clamped target pose to the current pose to get the target pose in the body frame + self._processed_actions[:, :3] = target_position_w - ee_pos_curr + self._processed_actions[:, 3:] = clamped_target_euler_angles_rel + else: + if self.cfg.controller.command_type == "position": + # Clip the target position in the world frame + self._processed_actions[:, :3] = torch.clamp( + self._processed_actions[:, :3], min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] + ) + elif self.cfg.controller.command_type == "pose": + # Clip the target position in the world frame + self._processed_actions[:, :3] = torch.clamp( + self._processed_actions[:, :3], min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] + ) + # Cast the target quaternion to euler angles + target_euler_angles_w = math_utils.euler_xyz_from_quat(self._processed_actions[:, 3:7]) + # Clip the euler angles + clamped_target_euler_angles_w = torch.clamp( + target_euler_angles_w, min=self._clip[:, 3:, 0], max=self._clip[:, 3:, 1] + ) + # Cast the clamped euler angles back to quaternion + clamped_target_quat_w = math_utils.quat_from_euler_xyz( + clamped_target_euler_angles_w[:, 0], + clamped_target_euler_angles_w[:, 1], + clamped_target_euler_angles_w[:, 2], + ) + # Apply the clamped desired pose to the current pose to get the desired pose in the body frame + self._processed_actions[:, 3:] = clamped_target_quat_w # set command into controller self._ik_controller.set_command(self._processed_actions, ee_pos_curr, ee_quat_curr) @@ -377,6 +495,32 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma self._nullspace_joint_pos_target = None self._resolve_nullspace_joint_pos_targets() + # parse clip + if cfg.clip_pose_abs is not None: + if len(self.cfg.clip_pose_abs) != 6: + raise ValueError("clip_pose_abs must be a list of 6 tuples.") + for t in self.cfg.clip_pose_abs: + if len(t) != 2: + raise ValueError("Each tuple in clip_pose_abs must contain 2 values.") + self._clip_pose_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_pose_abs[:] = torch.tensor(self.cfg.clip_pose_abs, device=self.device) + if cfg.clip_pose_rel is not None: + if len(self.cfg.clip_pose_rel) != 6: + raise ValueError("clip_pose_rel must be a list of 6 tuples.") + for t in self.cfg.clip_pose_rel: + if len(t) != 2: + raise ValueError("Each tuple in clip_pose_rel must contain 2 values.") + self._clip_pose_rel = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_pose_rel[:] = torch.tensor(self.cfg.clip_pose_rel, device=self.device) + if cfg.clip_wrench_abs is not None: + if len(self.cfg.clip_wrench_abs) != 6: + raise ValueError("clip_wrench_abs must be a list of 6 tuples.") + for t in self.cfg.clip_wrench_abs: + if len(t) != 2: + raise ValueError("Each tuple in clip_wrench_abs must contain 2 values.") + self._clip_wrench_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_wrench_abs[:] = torch.tensor(self.cfg.clip_wrench_abs, device=self.device) + """ Properties. """ @@ -417,7 +561,7 @@ def process_actions(self, actions: torch.Tensor): """Pre-processes the raw actions and sets them as commands for for operational space control. Args: - actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + actions: The raw actions for operational space control. It is a tensor of shape (``num_envs``, ``action_dim``). """ @@ -465,7 +609,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: """Resets the raw actions and the sensors if available. Args: - env_ids (Sequence[int] | None): The environment indices to reset. If ``None``, all environments are reset. + env_ids: The environment indices to reset. If ``None``, all environments are reset. """ self._raw_actions[env_ids] = 0.0 if self._contact_sensor is not None: @@ -665,7 +809,7 @@ def _preprocess_actions(self, actions: torch.Tensor): """Pre-processes the raw actions for operational space control. Args: - actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + actions: The raw actions for operational space control. It is a tensor of shape (``num_envs``, ``action_dim``). """ # Store the raw actions. Please note that the actions contain task space targets @@ -675,20 +819,48 @@ def _preprocess_actions(self, actions: torch.Tensor): self._processed_actions[:] = self._raw_actions # Go through the command types one by one, and apply the pre-processing if needed. if self._pose_abs_idx is not None: - self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale - self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale + if self.cfg.clip_pose_abs is not None: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] = torch.clamp( + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] * self._position_scale, + min=self._clip_pose_abs[:, :3, 0], + max=self._clip_pose_abs[:, :3, 1], + ) + rpy = math_utils.euler_xyz_from_quat( + self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale + ) + rpy_clamped = torch.clamp(rpy, min=self._clip_pose_abs[:, 3:6, 0], max=self._clip_pose_abs[:, 3:6, 1]) + self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] = ( + math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) + ) + else: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale if self._pose_rel_idx is not None: - self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale - self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale + if self.cfg.clip_pose_rel is not None: + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] = torch.clamp( + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] * self._position_scale, + min=self._clip_pose_rel[:, :3, 0], + max=self._clip_pose_rel[:, :3, 1], + ) + rpy = math_utils.euler_xyz_from_quat( + self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] * self._orientation_scale + ) + rpy_clamped = torch.clamp(rpy, min=self._clip_pose_rel[:, 3:6, 0], max=self._clip_pose_rel[:, 3:6, 1]) + self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] = ( + math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) + ) + else: + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale if self._wrench_abs_idx is not None: - self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale - if self._stiffness_idx is not None: - self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] *= self._stiffness_scale - self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] = torch.clamp( - self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6], - min=self.cfg.controller_cfg.motion_stiffness_limits_task[0], - max=self.cfg.controller_cfg.motion_stiffness_limits_task[1], - ) + if self.cfg.clip_wrench_abs is not None: + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] = torch.clamp( + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] * self._wrench_scale, + min=self._clip_wrench_abs[:, :6, 0], + max=self._clip_wrench_abs[:, :6, 1], + ) + else: + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale if self._damping_ratio_idx is not None: self._processed_actions[ :, self._damping_ratio_idx : self._damping_ratio_idx + 6 diff --git a/source/isaaclab/isaaclab/managers/manager_term_cfg.py b/source/isaaclab/isaaclab/managers/manager_term_cfg.py index 9927f91ce1a..2f4c1de6504 100644 --- a/source/isaaclab/isaaclab/managers/manager_term_cfg.py +++ b/source/isaaclab/isaaclab/managers/manager_term_cfg.py @@ -93,9 +93,6 @@ class for more details. debug_vis: bool = False """Whether to visualize debug information. Defaults to False.""" - clip: dict[str, tuple] | None = None - """Clip range for the action (dict of regex expressions). Defaults to None.""" - ## # Command manager. From 6bf91b6cc591ea5b2782cdf49aa6d7407fd9ebfa Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 13 Jun 2025 09:53:27 +0200 Subject: [PATCH 2/6] manually tested the IK wrapper. It seems to be working well. --- .../isaaclab/envs/mdp/actions/task_space_actions.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index 84b34cdf323..f9b741b73b4 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -168,7 +168,11 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: if len(tuple) != 2: raise ValueError(f"Expected tuple of (min, max) for orientation command. Found {tuple}.") # Create the clip tensor - self._clip = torch.tensor(self.num_envs, 7, 2, device=self.device) + self._clip = torch.zeros((self.num_envs, 6, 2), device=self.device, dtype=torch.float32) + out = torch.tensor(self.cfg.clip["position"][0], device=self.device) + print(self._clip.shape, out.shape) + print(out) + self._clip[:, 0] = torch.tensor(self.cfg.clip["position"][0], device=self.device) self._clip[:, 1] = torch.tensor(self.cfg.clip["position"][1], device=self.device) self._clip[:, 2] = torch.tensor(self.cfg.clip["position"][2], device=self.device) @@ -182,6 +186,7 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: ) else: raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + print(self._clip) """ Properties. @@ -240,7 +245,7 @@ def process_actions(self, actions: torch.Tensor): ee_pos_curr, ee_quat_curr, self._processed_actions ) # Cast the target_quat_w to euler angles - target_euler_angles_w = math_utils.euler_xyz_from_quat(target_quat_w) + target_euler_angles_w = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(target_quat_w)), 0, 1) # Clip the pose clamped_target_position_w = torch.clamp( target_position_w, min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] @@ -253,7 +258,7 @@ def process_actions(self, actions: torch.Tensor): clamped_target_euler_angles_w - self._processed_actions[:, 3:6] ) # Apply the clamped target pose to the current pose to get the target pose in the body frame - self._processed_actions[:, :3] = target_position_w - ee_pos_curr + self._processed_actions[:, :3] = clamped_target_position_w - ee_pos_curr self._processed_actions[:, 3:] = clamped_target_euler_angles_rel else: if self.cfg.controller.command_type == "position": @@ -267,7 +272,7 @@ def process_actions(self, actions: torch.Tensor): self._processed_actions[:, :3], min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] ) # Cast the target quaternion to euler angles - target_euler_angles_w = math_utils.euler_xyz_from_quat(self._processed_actions[:, 3:7]) + target_euler_angles_w = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(self._processed_actions[:, 3:7])), 0, 1) # Clip the euler angles clamped_target_euler_angles_w = torch.clamp( target_euler_angles_w, min=self._clip[:, 3:, 0], max=self._clip[:, 3:, 1] From b42bbdcf93bfb54329e351173c82f166a426f74d Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 13 Jun 2025 09:59:27 +0200 Subject: [PATCH 3/6] manually validated the behavior of the OSC controller --- .../isaaclab/envs/mdp/actions/task_space_actions.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index f9b741b73b4..a7a64404ee9 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -830,9 +830,9 @@ def _preprocess_actions(self, actions: torch.Tensor): min=self._clip_pose_abs[:, :3, 0], max=self._clip_pose_abs[:, :3, 1], ) - rpy = math_utils.euler_xyz_from_quat( + rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat( self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale - ) + )), 0, 1) rpy_clamped = torch.clamp(rpy, min=self._clip_pose_abs[:, 3:6, 0], max=self._clip_pose_abs[:, 3:6, 1]) self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] = ( math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) @@ -847,9 +847,9 @@ def _preprocess_actions(self, actions: torch.Tensor): min=self._clip_pose_rel[:, :3, 0], max=self._clip_pose_rel[:, :3, 1], ) - rpy = math_utils.euler_xyz_from_quat( + rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat( self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] * self._orientation_scale - ) + )), 0, 1) rpy_clamped = torch.clamp(rpy, min=self._clip_pose_rel[:, 3:6, 0], max=self._clip_pose_rel[:, 3:6, 1]) self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] = ( math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) From b53fc218afa7e645ee4ab7fecc2f00848649cc69 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 18 Jun 2025 10:36:39 +0200 Subject: [PATCH 4/6] reworked the code to be more flexible. --- .../envs/mdp/actions/task_space_actions.py | 205 +++++++++++++++--- 1 file changed, 176 insertions(+), 29 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index a7a64404ee9..3a217160c6f 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -500,31 +500,8 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma self._nullspace_joint_pos_target = None self._resolve_nullspace_joint_pos_targets() - # parse clip - if cfg.clip_pose_abs is not None: - if len(self.cfg.clip_pose_abs) != 6: - raise ValueError("clip_pose_abs must be a list of 6 tuples.") - for t in self.cfg.clip_pose_abs: - if len(t) != 2: - raise ValueError("Each tuple in clip_pose_abs must contain 2 values.") - self._clip_pose_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_pose_abs[:] = torch.tensor(self.cfg.clip_pose_abs, device=self.device) - if cfg.clip_pose_rel is not None: - if len(self.cfg.clip_pose_rel) != 6: - raise ValueError("clip_pose_rel must be a list of 6 tuples.") - for t in self.cfg.clip_pose_rel: - if len(t) != 2: - raise ValueError("Each tuple in clip_pose_rel must contain 2 values.") - self._clip_pose_rel = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_pose_rel[:] = torch.tensor(self.cfg.clip_pose_rel, device=self.device) - if cfg.clip_wrench_abs is not None: - if len(self.cfg.clip_wrench_abs) != 6: - raise ValueError("clip_wrench_abs must be a list of 6 tuples.") - for t in self.cfg.clip_wrench_abs: - if len(t) != 2: - raise ValueError("Each tuple in clip_wrench_abs must contain 2 values.") - self._clip_wrench_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_wrench_abs[:] = torch.tensor(self.cfg.clip_wrench_abs, device=self.device) + # parse clipping cfg + self._parse_clipping_cfg(self.cfg) """ Properties. @@ -621,6 +598,118 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: self._contact_sensor.reset(env_ids) if self._task_frame_transformer is not None: self._task_frame_transformer.reset(env_ids) + + """ + Parameter modification functions. + """ + + def _validate_clipping_values(self, value: list[tuple[float, float]] | torch.Tensor, name: str) -> torch.Tensor: + if isinstance(value, torch.Tensor): + assert value.shape == (self.num_envs, 6, 2), f"Expected {name} to be a tensor of shape ({self.num_envs}, 6, 2) but got {value.shape}" + elif isinstance(value, list): + if len(value) != 6: + value = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, value, name) + for i in range(6): + self._clip_pose_abs[:, i, 0] = -value[i][0] + self._clip_pose_abs[:, i, 1] = value[i][1] + else: + raise ValueError(f"Expected {name} to be a tensor or a list but got {type(value)}") + return value + + def _validate_scale_values(self, target, value, name): + if isinstance(value, torch.Tensor): + assert value.shape == (self.num_envs,), f"Expected {name} to be a tensor of shape ({self.num_envs},) but got {value.shape}" + elif isinstance(value, float): + value = torch.full((self.num_envs,), value, device=self.device) + return value + + def set_clipping_values( + self, + pose_abs_clip: list[tuple[float, float]] | torch.Tensor | None = None, + pose_rel_clip: list[tuple[float, float]] | torch.Tensor | None = None, + wrench_clip: list[tuple[float, float]] | torch.Tensor | None = None, + ): + """Sets the clipping values for the pose and wrench commands. + + If a tensor is provided, it must be of shape (num_envs, 6, 2). The setter performs a direct assignment so no + copy is made. If a list is provided, it must be a list of tuples, each containing two values. The setter will + convert the list to a tensor and assign it to the clipping values. + + Args: + pose_abs_clip: The clipping values for the pose absolute command. + pose_rel_clip: The clipping values for the pose relative command. + wrench_clip: The clipping values for the wrench command. + """ + + if pose_abs_clip is not None: + if isinstance(pose_abs_clip, torch.Tensor): + assert pose_abs_clip.shape == (self.num_envs, 6, 2), f"Expected pose_abs_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {pose_abs_clip.shape}" + self._clip_pose_abs = pose_abs_clip + elif isinstance(pose_abs_clip, list): + if len(pose_abs_clip) != 6: + raise ValueError("pose_abs_clip must be a list of 6 tuples") + for i in range(6): + self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] + self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] + else: + raise ValueError(f"Expected pose_abs_clip to be a tensor or a list but got {type(pose_abs_clip)}") + + if pose_rel_clip is not None: + if isinstance(pose_rel_clip, torch.Tensor): + assert pose_rel_clip.shape == (self.num_envs, 6, 2), f"Expected pose_rel_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {pose_rel_clip.shape}" + self._clip_pose_rel = pose_rel_clip + elif isinstance(pose_abs_clip, list): + if len(pose_abs_clip) != 6: + raise ValueError("pose_abs_clip must be a list of 6 tuples") + for i in range(6): + self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] + self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] + else: + raise ValueError(f"Expected pose_rel_clip to be a tensor or a list but got {type(pose_rel_clip)}") + + if wrench_clip is not None: + if isinstance(wrench_clip, torch.Tensor): + assert wrench_clip.shape == (self.num_envs, 6, 2), f"Expected wrench_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {wrench_clip.shape}" + self._clip_wrench_abs = wrench_clip + elif isinstance(pose_abs_clip, list): + if len(pose_abs_clip) != 6: + raise ValueError("pose_abs_clip must be a list of 6 tuples") + for i in range(6): + self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] + self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] + else: + raise ValueError(f"Expected wrench_clip to be a tensor or a list but got {type(wrench_clip)}") + + def set_scale_values( + self, + pos_scale: float | torch.Tensor | None = None, + ori_scale: float | torch.Tensor | None = None, + wrench_scale: float | torch.Tensor | None = None, + stiffness_scale: float | torch.Tensor | None = None, + damping_ratio_scale: float | torch.Tensor | None = None, + ): + + if pos_scale is not None: + if isinstance(pos_scale, torch.Tensor): + assert pos_scale.shape == (self.num_envs,), f"Expected pos_scale to be a tensor of shape ({self.num_envs},) but got {pos_scale.shape}" + self._position_scale = pos_scale + elif isinstance(pos_scale, float): + self._position_scale = torch.full((self.num_envs,), pos_scale, device=self.device) + else: + raise ValueError(f"Expected pos_scale to be a tensor or a float but got {type(pos_scale)}") + if ori_scale is not None: + ori_scale = self._validate_modified_param(ori_scale, "ori_scale") + self._orientation_scale.copy_(ori_scale) + if wrench_scale is not None: + wrench_scale = self._validate_modified_param(wrench_scale, "wrench_scale") + self._wrench_scale.copy_(wrench_scale) + if stiffness_scale is not None: + stiffness_scale = self._validate_modified_param(stiffness_scale, "stiffness_scale") + self._stiffness_scale.copy_(stiffness_scale) + if damping_ratio_scale is not None: + damping_ratio_scale = self._validate_modified_param(damping_ratio_scale, "damping_ratio_scale") + self._damping_ratio_scale.copy_(damping_ratio_scale) + """ Helper functions. @@ -830,9 +919,8 @@ def _preprocess_actions(self, actions: torch.Tensor): min=self._clip_pose_abs[:, :3, 0], max=self._clip_pose_abs[:, :3, 1], ) - rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat( - self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale - )), 0, 1) + normed_quat = math_utils.normalize(self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale) + rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(normed_quat)), 0, 1) rpy_clamped = torch.clamp(rpy, min=self._clip_pose_abs[:, 3:6, 0], max=self._clip_pose_abs[:, 3:6, 1]) self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] = ( math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) @@ -851,7 +939,7 @@ def _preprocess_actions(self, actions: torch.Tensor): self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] * self._orientation_scale )), 0, 1) rpy_clamped = torch.clamp(rpy, min=self._clip_pose_rel[:, 3:6, 0], max=self._clip_pose_rel[:, 3:6, 1]) - self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] = ( + self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] = ( math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) ) else: @@ -875,3 +963,62 @@ def _preprocess_actions(self, actions: torch.Tensor): min=self.cfg.controller_cfg.motion_damping_ratio_limits_task[0], max=self.cfg.controller_cfg.motion_damping_ratio_limits_task[1], ) + + @staticmethod + def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> list[tuple[float, float]]: + """Generates the clipping configuration for the operational space controller. + + The expected format is a list of tuples, each containing two values. Note that the order in which the tuples are provided + must match the order of the active axes in motion_control_axes_task. + + Args: + control_flags: The control flags for the operational space controller. + clip_cfg: The clipping configuration for the operational space controller. + name: The name of the clipping configuration. + """ + # Ensure the length of the clip_cfg is the same as the number of active axes + if len(clip_cfg) != sum(control_flags): + raise ValueError(f"{name} must be a list of tuples of the same length as there are active axes in motion_control_axes_task. There are {sum(control_flags)} active axes and {len(clip_cfg)} tuples in {name}.") + clip_pose_abs_new = [] + # Iterate over the control flags and add the corresponding clip to the list + for i, flag in enumerate(control_flags): + # If the axis is active, add the corresponding clip + if flag: + # Get the corresponding clip + clip = clip_cfg[sum(control_flags[:i])] + # Ensure the clip is a tuple of 2 values + if len(clip) != 2: + raise ValueError(f"Each tuple in {name} must contain 2 values.") + clip_pose_abs_new.append(clip) + else: + # If the axis is not active, add a clip of (-inf, inf). (Don't clip) + clip_pose_abs_new.append((-float('inf'), float('inf'))) + return clip_pose_abs_new + + def _parse_clipping_cfg(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg) -> None: + """Parses the clipping configuration for the operational space controller. + + Args: + cfg: The configuration of the action term. + """ + + # Parse clip based on the controller cfg + + + # Parse clip_pose_abs + if cfg.clip_pose_abs is not None: + clip_pose_abs = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_pose_abs, "clip_pose_abs") + self._clip_pose_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_pose_abs[:] = torch.tensor(clip_pose_abs, device=self.device) + + # Parse clip_pose_rel + if cfg.clip_pose_rel is not None: + clip_pose_rel = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_pose_rel, "clip_pose_rel") + self._clip_pose_rel = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_pose_rel[:] = torch.tensor(clip_pose_rel, device=self.device) + + # Parse clip_wrench_abs + if cfg.clip_wrench_abs is not None: + clip_wrench_abs = self._gen_clip(self.cfg.controller_cfg.contact_wrench_control_axes_task, cfg.clip_wrench_abs, "clip_wrench_abs") + self._clip_wrench_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_wrench_abs[:] = torch.tensor(clip_wrench_abs, device=self.device) \ No newline at end of file From 82df3440ccb0c87eb244041acdf80d32d45dd372 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 18 Jun 2025 11:54:57 +0200 Subject: [PATCH 5/6] Reworked the code to split between position and orientation rather than pose_abs and pos_rel. --- .../isaaclab/envs/mdp/actions/actions_cfg.py | 40 ++- .../envs/mdp/actions/task_space_actions.py | 268 ++++++++++-------- 2 files changed, 170 insertions(+), 138 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py index 9426ae7fd47..7fed2945ffb 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py @@ -367,44 +367,42 @@ class OffsetCfg: ``OperationalSpaceControllerCfg``. """ - # TODO: Here the clip effects are not homogeneous, but they have unique names that relate - # to specific control modes so it's fine to me. + # TODO: Here the clip effects are not homogeneous, I don't like that depending on the controller mode the behavior + # is different. It makes it hard to understand what does what. Should we have an explicit velocity clip? - clip_pose_abs: list[tuple[float, float]] | None = None - """Clip range for the absolute pose targets. Defaults to None for no clipping. - - The expected format is a list of tuples, each containing two values. This effectively bounds - the reachable range of the end-effector in the world frame. + clip_position: list[tuple[float, float]] | None = None + """Clip range for the position targets. Defaults to None for no clipping. + + The expected format is a list of tuples, each containing two values. When using the controller in ``"abs"`` mode + this limits the reachable range of the end-effector in the world frame. When using the controller in ``"rel"`` mode + this limits the maximum velocity of the end-effector in the task frame. This must match the number of active axes + in :attr:`controller_cfg.motion_control_axes_task`. Example: ..code-block:: python - clip_pose_abs = [ + clip_position = [ (min_x, max_x), (min_y, max_y), (min_z, max_z), - (min_roll, max_roll), - (min_pitch, max_pitch), - (min_yaw, max_yaw), ] """ - clip_pose_rel: list[tuple[float, float]] | None = None - """Clip range for the relative pose targets. Defaults to None for no clipping. + clip_orientation: list[tuple[float, float]] | None = None + """Clip range for the orientation targets. Defaults to None for no clipping. - The expected format is a list of tuples, each containing two values. This effectively limits - the end-effector's velocity in the task frame. + The expected format is a list of tuples, each containing two values. When using the controller in ``"abs"`` mode + this limits the reachable range of the end-effector in the world frame. When using the controller in ``"rel"`` mode + this limits the maximum velocity of the end-effector in the task frame. This must match the number of active axes + in :attr:`controller_cfg.motion_control_axes_task`. Example: ..code-block:: python - clip_pose_rel = [ - (min_x, max_x), - (min_y, max_y), - (min_z, max_z), + clip_orientation = [ (min_roll, max_roll), (min_pitch, max_pitch), (min_yaw, max_yaw), ] """ - clip_wrench_abs: list[tuple[float, float]] | None = None + clip_wrench: list[tuple[float, float]] | None = None """Clip range for the absolute wrench targets. Defaults to None for no clipping. The expected format is a list of tuples, each containing two values. This effectively limits @@ -412,7 +410,7 @@ class OffsetCfg: Example: ..code-block:: python - clip_wrench_abs = [ + clip_wrench = [ (min_fx, max_fx), (min_fy, max_fy), (min_fz, max_fz), diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index 3a217160c6f..3f11bcc4d6a 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -603,82 +603,34 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: Parameter modification functions. """ - def _validate_clipping_values(self, value: list[tuple[float, float]] | torch.Tensor, name: str) -> torch.Tensor: - if isinstance(value, torch.Tensor): - assert value.shape == (self.num_envs, 6, 2), f"Expected {name} to be a tensor of shape ({self.num_envs}, 6, 2) but got {value.shape}" - elif isinstance(value, list): - if len(value) != 6: - value = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, value, name) - for i in range(6): - self._clip_pose_abs[:, i, 0] = -value[i][0] - self._clip_pose_abs[:, i, 1] = value[i][1] - else: - raise ValueError(f"Expected {name} to be a tensor or a list but got {type(value)}") - return value - - def _validate_scale_values(self, target, value, name): - if isinstance(value, torch.Tensor): - assert value.shape == (self.num_envs,), f"Expected {name} to be a tensor of shape ({self.num_envs},) but got {value.shape}" - elif isinstance(value, float): - value = torch.full((self.num_envs,), value, device=self.device) - return value - def set_clipping_values( self, - pose_abs_clip: list[tuple[float, float]] | torch.Tensor | None = None, - pose_rel_clip: list[tuple[float, float]] | torch.Tensor | None = None, + position_clip: list[tuple[float, float]] | torch.Tensor | None = None, + orientation_clip: list[tuple[float, float]] | torch.Tensor | None = None, wrench_clip: list[tuple[float, float]] | torch.Tensor | None = None, - ): + ) -> None: """Sets the clipping values for the pose and wrench commands. - If a tensor is provided, it must be of shape (num_envs, 6, 2). The setter performs a direct assignment so no - copy is made. If a list is provided, it must be a list of tuples, each containing two values. The setter will - convert the list to a tensor and assign it to the clipping values. + If a tensor is provided, it must be of shape (num_envs, 3, 2) for position and orientation, and (num_envs, 6, 2) + for wrench. The setter performs a direct assignment so no copy is made. If a list is provided, it must be a list + of tuples, each containing two values. The setter will convert the list to a tensor and assign it to the clipping + values. Args: - pose_abs_clip: The clipping values for the pose absolute command. - pose_rel_clip: The clipping values for the pose relative command. + position_clip: The clipping values for the position command. + orientation_clip: The clipping values for the orientation command. wrench_clip: The clipping values for the wrench command. """ - if pose_abs_clip is not None: - if isinstance(pose_abs_clip, torch.Tensor): - assert pose_abs_clip.shape == (self.num_envs, 6, 2), f"Expected pose_abs_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {pose_abs_clip.shape}" - self._clip_pose_abs = pose_abs_clip - elif isinstance(pose_abs_clip, list): - if len(pose_abs_clip) != 6: - raise ValueError("pose_abs_clip must be a list of 6 tuples") - for i in range(6): - self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] - self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] - else: - raise ValueError(f"Expected pose_abs_clip to be a tensor or a list but got {type(pose_abs_clip)}") - - if pose_rel_clip is not None: - if isinstance(pose_rel_clip, torch.Tensor): - assert pose_rel_clip.shape == (self.num_envs, 6, 2), f"Expected pose_rel_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {pose_rel_clip.shape}" - self._clip_pose_rel = pose_rel_clip - elif isinstance(pose_abs_clip, list): - if len(pose_abs_clip) != 6: - raise ValueError("pose_abs_clip must be a list of 6 tuples") - for i in range(6): - self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] - self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] - else: - raise ValueError(f"Expected pose_rel_clip to be a tensor or a list but got {type(pose_rel_clip)}") - + if position_clip is not None: + position_clip = self._validate_clipping_values(self._clip_position.shape, position_clip, "position_clip") + self._clip_position = position_clip + if orientation_clip is not None: + orientation_clip = self._validate_clipping_values(self._clip_orientation.shape, orientation_clip, "orientation_clip") + self._clip_orientation = orientation_clip if wrench_clip is not None: - if isinstance(wrench_clip, torch.Tensor): - assert wrench_clip.shape == (self.num_envs, 6, 2), f"Expected wrench_clip to be a tensor of shape ({self.num_envs}, 6, 2) but got {wrench_clip.shape}" - self._clip_wrench_abs = wrench_clip - elif isinstance(pose_abs_clip, list): - if len(pose_abs_clip) != 6: - raise ValueError("pose_abs_clip must be a list of 6 tuples") - for i in range(6): - self._clip_pose_abs[:, i, 0] = -pose_abs_clip[i][0] - self._clip_pose_abs[:, i, 1] = pose_abs_clip[i][1] - else: - raise ValueError(f"Expected wrench_clip to be a tensor or a list but got {type(wrench_clip)}") + wrench_clip = self._validate_clipping_values(self._clip_wrench.shape, wrench_clip, "wrench_clip") + self._clip_wrench = wrench_clip def set_scale_values( self, @@ -687,29 +639,36 @@ def set_scale_values( wrench_scale: float | torch.Tensor | None = None, stiffness_scale: float | torch.Tensor | None = None, damping_ratio_scale: float | torch.Tensor | None = None, - ): + ) -> None: + """Sets the scale values for the operational space controller. + + If a tensor is provided, it must be of shape (num_envs,). The setter performs a direct assignment so no + copy is made. If a float is provided, it will be converted to a tensor of shape (num_envs,) and assigned + to the scale values. + + Args: + pos_scale: The scale values for the position targets. + ori_scale: The scale values for the orientation targets. + wrench_scale: The scale values for the wrench targets. + stiffness_scale: The scale values for the stiffness targets. + damping_ratio_scale: The scale values for the damping ratio targets. + """ if pos_scale is not None: - if isinstance(pos_scale, torch.Tensor): - assert pos_scale.shape == (self.num_envs,), f"Expected pos_scale to be a tensor of shape ({self.num_envs},) but got {pos_scale.shape}" - self._position_scale = pos_scale - elif isinstance(pos_scale, float): - self._position_scale = torch.full((self.num_envs,), pos_scale, device=self.device) - else: - raise ValueError(f"Expected pos_scale to be a tensor or a float but got {type(pos_scale)}") + pos_scale = self._validate_scale_values(pos_scale, "pos_scale") + self._position_scale = pos_scale if ori_scale is not None: - ori_scale = self._validate_modified_param(ori_scale, "ori_scale") - self._orientation_scale.copy_(ori_scale) + ori_scale = self._validate_scale_values(ori_scale, "ori_scale") + self._orientation_scale = ori_scale if wrench_scale is not None: - wrench_scale = self._validate_modified_param(wrench_scale, "wrench_scale") - self._wrench_scale.copy_(wrench_scale) + wrench_scale = self._validate_scale_values(wrench_scale, "wrench_scale") + self._wrench_scale = wrench_scale if stiffness_scale is not None: - stiffness_scale = self._validate_modified_param(stiffness_scale, "stiffness_scale") - self._stiffness_scale.copy_(stiffness_scale) + stiffness_scale = self._validate_scale_values(stiffness_scale, "stiffness_scale") + self._stiffness_scale = stiffness_scale if damping_ratio_scale is not None: - damping_ratio_scale = self._validate_modified_param(damping_ratio_scale, "damping_ratio_scale") - self._damping_ratio_scale.copy_(damping_ratio_scale) - + damping_ratio_scale = self._validate_scale_values(damping_ratio_scale, "damping_ratio_scale") + self._damping_ratio_scale = damping_ratio_scale """ Helper functions. @@ -913,44 +872,44 @@ def _preprocess_actions(self, actions: torch.Tensor): self._processed_actions[:] = self._raw_actions # Go through the command types one by one, and apply the pre-processing if needed. if self._pose_abs_idx is not None: - if self.cfg.clip_pose_abs is not None: + if self._clip_position is not None: self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] = torch.clamp( self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] * self._position_scale, - min=self._clip_pose_abs[:, :3, 0], - max=self._clip_pose_abs[:, :3, 1], + min=self._clip_position[:, :, 0], + max=self._clip_position[:, :, 1], ) + else: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale + if self._clip_orientation is not None: normed_quat = math_utils.normalize(self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale) rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(normed_quat)), 0, 1) - rpy_clamped = torch.clamp(rpy, min=self._clip_pose_abs[:, 3:6, 0], max=self._clip_pose_abs[:, 3:6, 1]) + rpy_clamped = torch.clamp(rpy, min=self._clip_orientation[:, :, 0], max=self._clip_orientation[:, :, 1]) self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] = ( math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) ) else: - self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale if self._pose_rel_idx is not None: - if self.cfg.clip_pose_rel is not None: + if self._clip_position is not None: self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] = torch.clamp( self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] * self._position_scale, - min=self._clip_pose_rel[:, :3, 0], - max=self._clip_pose_rel[:, :3, 1], - ) - rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat( - self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 7] * self._orientation_scale - )), 0, 1) - rpy_clamped = torch.clamp(rpy, min=self._clip_pose_rel[:, 3:6, 0], max=self._clip_pose_rel[:, 3:6, 1]) - self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] = ( - math_utils.quat_from_euler_xyz(rpy_clamped[:, 0], rpy_clamped[:, 1], rpy_clamped[:, 2]) + min=self._clip_position[:, :, 0], + max=self._clip_position[:, :, 1], ) else: self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale + if self._clip_orientation is not None: + rpy = self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] * self._orientation_scale + rpy_clamped = torch.clamp(rpy, min=self._clip_orientation[:, :, 0], max=self._clip_orientation[:, :, 1]) + self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] = rpy_clamped + else: self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale if self._wrench_abs_idx is not None: - if self.cfg.clip_wrench_abs is not None: + if self.cfg.clip_wrench is not None: self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] = torch.clamp( self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] * self._wrench_scale, - min=self._clip_wrench_abs[:, :6, 0], - max=self._clip_wrench_abs[:, :6, 1], + min=self._clip_wrench[:, :, 0], + max=self._clip_wrench[:, :, 1], ) else: self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale @@ -976,11 +935,19 @@ def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> clip_cfg: The clipping configuration for the operational space controller. name: The name of the clipping configuration. """ + allowed_names = ["clip_position", "clip_orientation", "clip_wrench"] + if name not in allowed_names: + raise ValueError(f"Expected {name} to be one of {allowed_names} but got {name}") + + # Iterate over the control flags and add the corresponding clip to the list + if name == "clip_position": + control_flags = control_flags[:3] + elif name == "clip_orientation": + control_flags = control_flags[3:] # Ensure the length of the clip_cfg is the same as the number of active axes if len(clip_cfg) != sum(control_flags): raise ValueError(f"{name} must be a list of tuples of the same length as there are active axes in motion_control_axes_task. There are {sum(control_flags)} active axes and {len(clip_cfg)} tuples in {name}.") clip_pose_abs_new = [] - # Iterate over the control flags and add the corresponding clip to the list for i, flag in enumerate(control_flags): # If the axis is active, add the corresponding clip if flag: @@ -1002,23 +969,90 @@ def _parse_clipping_cfg(self, cfg: actions_cfg.OperationalSpaceControllerActionC cfg: The configuration of the action term. """ - # Parse clip based on the controller cfg + # Parse clip_position + if cfg.clip_position is not None: + clip_position = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_position, "clip_position") + self._clip_position = torch.zeros((self.num_envs, 3, 2), device=self.device) + self._clip_position[:] = torch.tensor(clip_position, device=self.device) + else: + self._clip_position = None + + # Parse clip_orientation + if cfg.clip_orientation is not None: + clip_orientation = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_orientation, "clip_orientation") + self._clip_orientation = torch.zeros((self.num_envs, 3, 2), device=self.device) + self._clip_orientation[:] = torch.tensor(clip_orientation, device=self.device) + else: + self._clip_orientation = None + # Parse clip_wrench + if cfg.clip_wrench is not None: + clip_wrench = self._gen_clip(self.cfg.controller_cfg.contact_wrench_control_axes_task, cfg.clip_wrench, "clip_wrench") + self._clip_wrench = torch.zeros((self.num_envs, 6, 2), device=self.device) + self._clip_wrench[:] = torch.tensor(clip_wrench, device=self.device) + else: + self._clip_wrench = None - # Parse clip_pose_abs - if cfg.clip_pose_abs is not None: - clip_pose_abs = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_pose_abs, "clip_pose_abs") - self._clip_pose_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_pose_abs[:] = torch.tensor(clip_pose_abs, device=self.device) + def _validate_clipping_values(self, target_shape: torch.Tensor, value: list[tuple[float, float]] | torch.Tensor, name: str) -> torch.Tensor: + """Validates the clipping values for the operational space controller. - # Parse clip_pose_rel - if cfg.clip_pose_rel is not None: - clip_pose_rel = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_pose_rel, "clip_pose_rel") - self._clip_pose_rel = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_pose_rel[:] = torch.tensor(clip_pose_rel, device=self.device) + Args: + target_shape: The shape of the target tensor. + value: The clipping values to validate. + name: The name of the clipping configuration. + + Returns: + The validated clipping values. - # Parse clip_wrench_abs - if cfg.clip_wrench_abs is not None: - clip_wrench_abs = self._gen_clip(self.cfg.controller_cfg.contact_wrench_control_axes_task, cfg.clip_wrench_abs, "clip_wrench_abs") - self._clip_wrench_abs = torch.zeros((self.num_envs, 6, 2), device=self.device) - self._clip_wrench_abs[:] = torch.tensor(clip_wrench_abs, device=self.device) \ No newline at end of file + Raises: + ValueError: If the clipping values are not a tensor or a list. + ValueError: If the clipping values are not of the correct shape. + ValueError: If the clipping values are not a list of tuples of 2 values. + """ + allowed_names = ["position_clip", "orientation_clip", "wrench_clip"] + if name not in allowed_names: + raise ValueError(f"Expected {name} to be one of {allowed_names} but got {name}") + + if isinstance(value, torch.Tensor): + if value.shape != target_shape: + raise ValueError(f"Expected {name} to be a tensor of shape {target_shape} but got {value.shape}") + tensor_clip = value + elif isinstance(value, list): + tensor_clip = torch.zeros(target_shape, device=self.device) + if len(value) != target_shape[1]: + if name in ["position_clip"]: + value = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task[:3], value, name) + elif name in ["orientation_clip"]: + value = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task[3:], value, name) + else: + value = self._gen_clip(self.cfg.controller_cfg.contact_wrench_control_axes_task, value, name) + tensor_clip[:] = torch.tensor(value, device=self.device) + else: + raise ValueError(f"Expected {name} to be a tensor or a list but got {type(value)}") + return tensor_clip + + def _validate_scale_values(self, value: float | torch.Tensor, name: str) -> torch.Tensor: + """Validates the scale values for the operational space controller. + + Args: + value: The scale values to validate. + name: The name of the scale configuration. + + Returns: + The validated scale values. + + Raises: + ValueError: If the scale values are not a tensor or a float. + ValueError: If the scale values are not of the correct shape. + """ + if isinstance(value, torch.Tensor): + if len(value.shape) == 1: + value = value.unsqueeze(-1) + if value.shape != (self.num_envs, 1): + raise ValueError(f"Expected {name} to be a tensor of shape ({self.num_envs}, 1) but got {value.shape}") + elif isinstance(value, float): + value = torch.full((self.num_envs, 1), value, device=self.device) + else: + raise ValueError(f"Expected {name} to be a tensor or a float but got {type(value)}") + return value + From da139b4ce2a342bf50c1cfd697353e0c49c5e2a4 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Wed, 18 Jun 2025 11:56:27 +0200 Subject: [PATCH 6/6] pre-commits --- .../isaaclab/envs/mdp/actions/actions_cfg.py | 4 +- .../envs/mdp/actions/task_space_actions.py | 69 ++++++++++++------- 2 files changed, 47 insertions(+), 26 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py index 7fed2945ffb..3b4b1d549fe 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py @@ -368,11 +368,11 @@ class OffsetCfg: """ # TODO: Here the clip effects are not homogeneous, I don't like that depending on the controller mode the behavior - # is different. It makes it hard to understand what does what. Should we have an explicit velocity clip? + # is different. It makes it hard to understand what does what. Should we have an explicit velocity clip? clip_position: list[tuple[float, float]] | None = None """Clip range for the position targets. Defaults to None for no clipping. - + The expected format is a list of tuples, each containing two values. When using the controller in ``"abs"`` mode this limits the reachable range of the end-effector in the world frame. When using the controller in ``"rel"`` mode this limits the maximum velocity of the end-effector in the task frame. This must match the number of active axes diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index 3f11bcc4d6a..705969b3172 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -172,7 +172,7 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: out = torch.tensor(self.cfg.clip["position"][0], device=self.device) print(self._clip.shape, out.shape) print(out) - + self._clip[:, 0] = torch.tensor(self.cfg.clip["position"][0], device=self.device) self._clip[:, 1] = torch.tensor(self.cfg.clip["position"][1], device=self.device) self._clip[:, 2] = torch.tensor(self.cfg.clip["position"][2], device=self.device) @@ -245,7 +245,9 @@ def process_actions(self, actions: torch.Tensor): ee_pos_curr, ee_quat_curr, self._processed_actions ) # Cast the target_quat_w to euler angles - target_euler_angles_w = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(target_quat_w)), 0, 1) + target_euler_angles_w = torch.transpose( + torch.stack(math_utils.euler_xyz_from_quat(target_quat_w)), 0, 1 + ) # Clip the pose clamped_target_position_w = torch.clamp( target_position_w, min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] @@ -272,7 +274,9 @@ def process_actions(self, actions: torch.Tensor): self._processed_actions[:, :3], min=self._clip[:, :3, 0], max=self._clip[:, :3, 1] ) # Cast the target quaternion to euler angles - target_euler_angles_w = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(self._processed_actions[:, 3:7])), 0, 1) + target_euler_angles_w = torch.transpose( + torch.stack(math_utils.euler_xyz_from_quat(self._processed_actions[:, 3:7])), 0, 1 + ) # Clip the euler angles clamped_target_euler_angles_w = torch.clamp( target_euler_angles_w, min=self._clip[:, 3:, 0], max=self._clip[:, 3:, 1] @@ -598,7 +602,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: self._contact_sensor.reset(env_ids) if self._task_frame_transformer is not None: self._task_frame_transformer.reset(env_ids) - + """ Parameter modification functions. """ @@ -615,18 +619,20 @@ def set_clipping_values( for wrench. The setter performs a direct assignment so no copy is made. If a list is provided, it must be a list of tuples, each containing two values. The setter will convert the list to a tensor and assign it to the clipping values. - + Args: position_clip: The clipping values for the position command. orientation_clip: The clipping values for the orientation command. wrench_clip: The clipping values for the wrench command. """ - + if position_clip is not None: position_clip = self._validate_clipping_values(self._clip_position.shape, position_clip, "position_clip") self._clip_position = position_clip if orientation_clip is not None: - orientation_clip = self._validate_clipping_values(self._clip_orientation.shape, orientation_clip, "orientation_clip") + orientation_clip = self._validate_clipping_values( + self._clip_orientation.shape, orientation_clip, "orientation_clip" + ) self._clip_orientation = orientation_clip if wrench_clip is not None: wrench_clip = self._validate_clipping_values(self._clip_wrench.shape, wrench_clip, "wrench_clip") @@ -881,7 +887,9 @@ def _preprocess_actions(self, actions: torch.Tensor): else: self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale if self._clip_orientation is not None: - normed_quat = math_utils.normalize(self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale) + normed_quat = math_utils.normalize( + self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] * self._orientation_scale + ) rpy = torch.transpose(torch.stack(math_utils.euler_xyz_from_quat(normed_quat)), 0, 1) rpy_clamped = torch.clamp(rpy, min=self._clip_orientation[:, :, 0], max=self._clip_orientation[:, :, 1]) self.processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] = ( @@ -899,7 +907,9 @@ def _preprocess_actions(self, actions: torch.Tensor): else: self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale if self._clip_orientation is not None: - rpy = self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] * self._orientation_scale + rpy = ( + self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] * self._orientation_scale + ) rpy_clamped = torch.clamp(rpy, min=self._clip_orientation[:, :, 0], max=self._clip_orientation[:, :, 1]) self.processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] = rpy_clamped else: @@ -926,7 +936,7 @@ def _preprocess_actions(self, actions: torch.Tensor): @staticmethod def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> list[tuple[float, float]]: """Generates the clipping configuration for the operational space controller. - + The expected format is a list of tuples, each containing two values. Note that the order in which the tuples are provided must match the order of the active axes in motion_control_axes_task. @@ -938,7 +948,7 @@ def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> allowed_names = ["clip_position", "clip_orientation", "clip_wrench"] if name not in allowed_names: raise ValueError(f"Expected {name} to be one of {allowed_names} but got {name}") - + # Iterate over the control flags and add the corresponding clip to the list if name == "clip_position": control_flags = control_flags[:3] @@ -946,7 +956,11 @@ def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> control_flags = control_flags[3:] # Ensure the length of the clip_cfg is the same as the number of active axes if len(clip_cfg) != sum(control_flags): - raise ValueError(f"{name} must be a list of tuples of the same length as there are active axes in motion_control_axes_task. There are {sum(control_flags)} active axes and {len(clip_cfg)} tuples in {name}.") + raise ValueError( + f"{name} must be a list of tuples of the same length as there are active axes in" + f" motion_control_axes_task. There are {sum(control_flags)} active axes and {len(clip_cfg)} tuples in" + f" {name}." + ) clip_pose_abs_new = [] for i, flag in enumerate(control_flags): # If the axis is active, add the corresponding clip @@ -959,19 +973,21 @@ def _gen_clip(control_flags, clip_cfg: list[tuple[float, float]], name: str) -> clip_pose_abs_new.append(clip) else: # If the axis is not active, add a clip of (-inf, inf). (Don't clip) - clip_pose_abs_new.append((-float('inf'), float('inf'))) - return clip_pose_abs_new + clip_pose_abs_new.append((-float("inf"), float("inf"))) + return clip_pose_abs_new def _parse_clipping_cfg(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg) -> None: """Parses the clipping configuration for the operational space controller. - + Args: cfg: The configuration of the action term. """ # Parse clip_position if cfg.clip_position is not None: - clip_position = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_position, "clip_position") + clip_position = self._gen_clip( + self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_position, "clip_position" + ) self._clip_position = torch.zeros((self.num_envs, 3, 2), device=self.device) self._clip_position[:] = torch.tensor(clip_position, device=self.device) else: @@ -979,28 +995,34 @@ def _parse_clipping_cfg(self, cfg: actions_cfg.OperationalSpaceControllerActionC # Parse clip_orientation if cfg.clip_orientation is not None: - clip_orientation = self._gen_clip(self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_orientation, "clip_orientation") + clip_orientation = self._gen_clip( + self.cfg.controller_cfg.motion_control_axes_task, cfg.clip_orientation, "clip_orientation" + ) self._clip_orientation = torch.zeros((self.num_envs, 3, 2), device=self.device) self._clip_orientation[:] = torch.tensor(clip_orientation, device=self.device) else: self._clip_orientation = None - # Parse clip_wrench + # Parse clip_wrench if cfg.clip_wrench is not None: - clip_wrench = self._gen_clip(self.cfg.controller_cfg.contact_wrench_control_axes_task, cfg.clip_wrench, "clip_wrench") + clip_wrench = self._gen_clip( + self.cfg.controller_cfg.contact_wrench_control_axes_task, cfg.clip_wrench, "clip_wrench" + ) self._clip_wrench = torch.zeros((self.num_envs, 6, 2), device=self.device) self._clip_wrench[:] = torch.tensor(clip_wrench, device=self.device) else: self._clip_wrench = None - def _validate_clipping_values(self, target_shape: torch.Tensor, value: list[tuple[float, float]] | torch.Tensor, name: str) -> torch.Tensor: + def _validate_clipping_values( + self, target_shape: torch.Tensor, value: list[tuple[float, float]] | torch.Tensor, name: str + ) -> torch.Tensor: """Validates the clipping values for the operational space controller. Args: target_shape: The shape of the target tensor. value: The clipping values to validate. name: The name of the clipping configuration. - + Returns: The validated clipping values. @@ -1012,7 +1034,7 @@ def _validate_clipping_values(self, target_shape: torch.Tensor, value: list[tupl allowed_names = ["position_clip", "orientation_clip", "wrench_clip"] if name not in allowed_names: raise ValueError(f"Expected {name} to be one of {allowed_names} but got {name}") - + if isinstance(value, torch.Tensor): if value.shape != target_shape: raise ValueError(f"Expected {name} to be a tensor of shape {target_shape} but got {value.shape}") @@ -1030,7 +1052,7 @@ def _validate_clipping_values(self, target_shape: torch.Tensor, value: list[tupl else: raise ValueError(f"Expected {name} to be a tensor or a list but got {type(value)}") return tensor_clip - + def _validate_scale_values(self, value: float | torch.Tensor, name: str) -> torch.Tensor: """Validates the scale values for the operational space controller. @@ -1055,4 +1077,3 @@ def _validate_scale_values(self, value: float | torch.Tensor, name: str) -> torc else: raise ValueError(f"Expected {name} to be a tensor or a float but got {type(value)}") return value -