From f7f496ea0d90cd95209d740e7a2ef47a601280ee Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Mon, 18 Aug 2025 23:27:48 -0700 Subject: [PATCH 01/12] Enhance Pink IK controller with null-space posture control and improved testing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add NullSpacePostureTask for natural shoulder/waist joint postures during manipulation - Refactor IK with damping, null-space control, and joint optimization - Improve testing framework with JSON-based configurations and stricter tolerances (1mm/1° vs 30mm/10°) - Apply IK improvements across all environments (PickPlace, NutPour, ExhaustPipe) - Add waist-enabled manipulation environments and target_eef_link_names mapping - Disable IK joint limits safety checks so that it always produces a solution --- CONTRIBUTORS.md | 3 + docs/index.rst | 2 +- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 22 + .../controllers/null_space_posture_task.py | 217 +++++++++ .../isaaclab/isaaclab/controllers/pink_ik.py | 69 ++- .../isaaclab/controllers/pink_ik_cfg.py | 3 + .../envs/mdp/actions/pink_actions_cfg.py | 7 + .../mdp/actions/pink_task_space_actions.py | 27 +- source/isaaclab/isaaclab/utils/string.py | 10 +- .../pink_ik_gr1_test_configs.json | 87 ++++ .../isaaclab/test/controllers/test_pink_ik.py | 451 ++++++++++++------ source/isaaclab/test/utils/test_string.py | 8 + source/isaaclab_tasks/config/extension.toml | 2 +- source/isaaclab_tasks/docs/CHANGELOG.rst | 19 + .../manipulation/pick_place/__init__.py | 17 +- .../exhaustpipe_gr1t2_base_env_cfg.py | 47 ++ .../exhaustpipe_gr1t2_pink_ik_env_cfg.py | 47 +- .../pick_place/nutpour_gr1t2_base_env_cfg.py | 47 ++ .../nutpour_gr1t2_pink_ik_env_cfg.py | 47 +- .../pick_place/pickplace_gr1t2_env_cfg.py | 94 +++- .../pickplace_gr1t2_waist_enabled_env_cfg.py | 133 ++++++ 22 files changed, 1169 insertions(+), 192 deletions(-) create mode 100644 source/isaaclab/isaaclab/controllers/null_space_posture_task.py create mode 100644 source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 17cd64b4db39..3816e0f20d14 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -70,6 +70,7 @@ Guidelines for modifications: * HoJin Jeon * Hongwei Xiong * Hongyu Li +* Huihua Zhao * Iretiayo Akinola * Jack Zeng * Jan Kerner @@ -94,6 +95,7 @@ Guidelines for modifications: * Maurice Rahme * Michael Gussert * Michael Noseworthy +* Michael Lin * Miguel Alonso Jr * Mingyu Lee * Muhong Guo @@ -129,6 +131,7 @@ Guidelines for modifications: * Wei Yang * Xavier Nal * Xinpeng Liu +* Yan Chang * Yang Jin * Yanzi Zhu * Yijie Guo diff --git a/docs/index.rst b/docs/index.rst index ffd94c35857e..d5e85bc8cab5 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -46,7 +46,7 @@ For more information about the framework, please refer to the `paper None: + r"""Create task. + + Args: + cost: value used to cast joint angle differences to a homogeneous + cost, in :math:`[\mathrm{cost}] / [\mathrm{rad}]`. + lm_damping: Unitless scale of the Levenberg-Marquardt (only when + the error is large) regularization term, which helps when + targets are infeasible. Increase this value if the task is too + jerky under infeasible targets, but beware that too large a + damping can slow down the task. + gain: Task gain :math:`\alpha \in [0, 1]` for additional low-pass + filtering. Defaults to 1.0 (no filtering). + frame_task_controlled_joints: Dictionary of frame link names to controlled joint names. + """ + super().__init__(cost=cost, gain=gain, lm_damping=lm_damping) + self.target_q: np.ndarray | None = None + self.frame_task_controlled_joints: dict[str, list[str]] = frame_task_controlled_joints or {} + self._joint_name_to_index: dict[str, int] | None = None + self._selected_joint_indices: list[np.ndarray] | None = None + self._jacobian_dim: int = 0 + + def __repr__(self) -> str: + """Human-readable representation of the task.""" + return ( + f"NullSpacePostureTask(cost={self.cost}, gain={self.gain}, lm_damping={self.lm_damping}," + f" frame_task_controlled_joints={self.frame_task_controlled_joints})" + ) + + def _build_joint_mapping(self, configuration: Configuration) -> None: + """Build efficient joint name to index mapping. + + Args: + configuration: Robot configuration. + """ + if self._joint_name_to_index is not None: + return + + # Create O(1) lookup dictionary for joint names + joint_names = configuration.model.names.tolist()[1:] # Skip root joint + self._joint_name_to_index = {name: idx for idx, name in enumerate(joint_names)} + + # Build selected joint indices efficiently + self._selected_joint_indices = [] + for controlled_joints in self.frame_task_controlled_joints.values(): + indices = [self._joint_name_to_index[joint] for joint in controlled_joints] + self._selected_joint_indices.append(np.array(sorted(indices), dtype=int)) + + def set_target(self, target_q: np.ndarray) -> None: + """Set target posture. + + Args: + target_q: Target vector in the configuration space. If the model + has a floating base, then this vector should include + floating-base coordinates (although they have no effect on the + posture task). + """ + self.target_q = target_q.copy() + + def set_target_from_configuration(self, configuration: Configuration) -> None: + """Set target posture from a robot configuration. + + Args: + configuration: Robot configuration. + """ + self.set_target(configuration.q) + + def compute_error(self, configuration: Configuration) -> np.ndarray: + r"""Compute posture task error. + + This method computes the posture error for the null space posture task. + The error is the difference between the target and current configuration, + excluding the floating base coordinates (if present). + + Note: + The actual null space projection is applied in the Jacobian (see `compute_jacobian`). + This function only returns the configuration difference in the actuated joint space. + + Args: + configuration: Robot configuration :math:`q`. + + Returns: + Posture task error :math:`e(q) = q^* - q`, where only actuated joints are considered. + + Raises: + ValueError: If no posture target has been set. + """ + if self.target_q is None: + raise ValueError("No posture target has been set. Call set_target() first.") + + _, root_nv = get_root_joint_dim(configuration.model) + + # Compute configuration difference + diff = pin.difference( + configuration.model, + self.target_q, + configuration.q, + )[root_nv:] + + return diff + + def compute_jacobian(self, configuration: Configuration) -> np.ndarray: + r"""Compute the posture task Jacobian (null space projector). + + This method computes the sum of the left and right null space projectors, + each defined as :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse + of the corresponding end-effector Jacobian. The null space projectors are + masked to only affect the selected controlled joints for each side. + + The final Jacobian returned is: + :math:`J(q) = N_{\text{left}}(q) + N_{\text{right}}(q)` + + Args: + configuration: Robot configuration :math:`q`. + + Returns: + Posture task Jacobian :math:`J(q)`, which is the sum of the left and right + null space projectors, each masked to their respective controlled joints. + """ + # Initialize joint mapping if needed + if self._joint_name_to_index is None: + self._build_joint_mapping(configuration) + + # Early return if no frame tasks defined + if not self.frame_task_controlled_joints: + print("No frame tasks defined") + # Return identity matrix if no frame tasks defined + n_joints = len(configuration.model.names) - 1 # Exclude root joint + return np.eye(n_joints) + + # Get frame names + frame_names = list(self.frame_task_controlled_joints.keys()) + + # Get Jacobians for all frame tasks + J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in frame_names] + + # Cache Jacobian dimension + if self._jacobian_dim == 0: + self._jacobian_dim = J_frame_tasks[0].shape[1] + + # Initialize null space task matrix + null_space_task = np.zeros((self._jacobian_dim, self._jacobian_dim)) + + # Compute null space projectors efficiently + for i, (J_frame_task, selected_indices) in enumerate(zip(J_frame_tasks, self._selected_joint_indices or [])): + # Create mask for selected joints + mask = np.zeros(J_frame_task.shape[1], dtype=bool) + mask[selected_indices] = True + + # Compute pseudoinverse and null space projector + J_pinv = np.linalg.pinv(J_frame_task) + null_space_full = np.eye(J_frame_task.shape[1]) - J_pinv @ J_frame_task + + # Only update rows corresponding to selected joints + null_space_task[mask, :] += null_space_full[mask, :] + + return null_space_task diff --git a/source/isaaclab/isaaclab/controllers/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik.py index 8fff42247223..7619a5ea55d6 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik.py @@ -14,8 +14,13 @@ from pink import solve_ik from pink.configuration import Configuration +from pink.tasks import FrameTask from pinocchio.robot_wrapper import RobotWrapper +from isaaclab.assets import ArticulationCfg +from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask +from isaaclab.utils.string import resolve_matching_names_values + from .pink_ik_cfg import PinkIKControllerCfg @@ -25,7 +30,7 @@ class PinkIKController: The Pink IK controller is available at: https://github.com/stephane-caron/pink """ - def __init__(self, cfg: PinkIKControllerCfg, device: str): + def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str): """Initialize the Pink IK Controller. Args: @@ -38,29 +43,54 @@ def __init__(self, cfg: PinkIKControllerCfg, device: str): self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0 ) + # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos, + # where the joint_pos keys may be regex patterns and the values are the initial positions. + # We want to assign to each Pink joint name the value from the first matching regex key in joint_pos. + pink_joint_names = self.pink_configuration.model.names.tolist()[1:] + joint_pos_dict = robot_cfg.init_state.joint_pos + + # Use resolve_matching_names_values to match Pink joint names to joint_pos values + indices, names, values = resolve_matching_names_values( + joint_pos_dict, pink_joint_names, preserve_order=False, require_all_keys_matched=False + ) + if len(indices) != len(pink_joint_names): + unmatched = [name for name in pink_joint_names if name not in names] + raise KeyError( + "Could not find a match for all Pink joint names in robot_cfg.init_state.joint_pos. " + f"Unmatched: {unmatched}, Expected: {pink_joint_names}" + ) + self.init_joint_positions = np.array(values) + # Set the default targets for each task from the configuration for task in cfg.variable_input_tasks: + if isinstance(task, NullSpacePostureTask): + task.set_target(self.init_joint_positions) + continue task.set_target_from_configuration(self.pink_configuration) for task in cfg.fixed_input_tasks: task.set_target_from_configuration(self.pink_configuration) # Map joint names from Isaac Lab to Pink's joint conventions - pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints - isaac_lab_joint_names = cfg.joint_names + self.pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints + self.isaac_lab_joint_names = cfg.joint_names + + # Frame task link names + self.frame_task_link_names = [] + for task in cfg.variable_input_tasks: + if isinstance(task, FrameTask): + self.frame_task_link_names.append(task.frame) # Create reordering arrays for joint indices - self.isaac_lab_to_pink_ordering = [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names] + self.isaac_lab_to_pink_ordering = [ + self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names + ] self.pink_to_isaac_lab_ordering = [ - pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names + self.pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in self.isaac_lab_joint_names ] self.cfg = cfg self.device = device - """ - Operations. - """ - def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: """Reorder the input array based on the provided ordering. @@ -80,6 +110,16 @@ def initialize(self): """ pass + def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): + """Update the null space joint targets. + + Args: + curr_joint_pos: The current joint positions. + """ + for task in self.cfg.variable_input_tasks: + if isinstance(task, NullSpacePostureTask): + task.set_target(curr_joint_pos) + def compute( self, curr_joint_pos: np.ndarray, @@ -103,15 +143,20 @@ def compute( # pink.solve_ik can raise an exception if the solver fails try: velocity = solve_ik( - self.pink_configuration, self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, dt, solver="osqp" + self.pink_configuration, + self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, + dt, + solver="osqp", + safety_break=self.cfg.fail_on_joint_limit_violation, ) Delta_q = velocity * dt - except (AssertionError, Exception): + except (AssertionError, Exception) as e: # Print warning and return the current joint positions as the target # Not using omni.log since its not available in CI during docs build if self.cfg.show_ik_warnings: print( - "Warning: IK quadratic solver could not find a solution! Did not update the target joint positions." + "Warning: IK quadratic solver could not find a solution! Did not update the target joint" + f" positions.\nError: {e}" ) return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) diff --git a/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py index 52bea14f6ccb..824db01a3cbf 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py @@ -57,3 +57,6 @@ class PinkIKControllerCfg: show_ik_warnings: bool = True """Show warning if IK solver fails to find a solution.""" + + fail_on_joint_limit_violation: bool = True + """Fail the IK solver if a joint limit is violated.""" diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py index 6b7c412de7dc..d8da1e421983 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -34,3 +34,10 @@ class PinkInverseKinematicsActionCfg(ActionTermCfg): controller: PinkIKControllerCfg = MISSING """Configuration for the Pink IK controller that will be used to solve the inverse kinematics.""" + + target_eef_link_names: dict[str, str] = MISSING + """Dictionary mapping task names to controlled link names for the Pink IK controller. + + This dictionary should map the task names (e.g., 'left_wrist', 'right_wrist') to the + corresponding link names in the URDF that will be controlled by the IK solver. + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index e0d008834a52..bbdfd68f1fb4 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -10,6 +10,8 @@ from collections.abc import Sequence from typing import TYPE_CHECKING +from pink.tasks import FrameTask + import isaaclab.utils.math as math_utils from isaaclab.assets.articulation import Articulation from isaaclab.controllers.pink_ik import PinkIKController @@ -57,7 +59,11 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma assert env.num_envs > 0, "Number of environments specified are less than 1." self._ik_controllers = [] for _ in range(env.num_envs): - self._ik_controllers.append(PinkIKController(cfg=copy.deepcopy(self.cfg.controller), device=self.device)) + self._ik_controllers.append( + PinkIKController( + cfg=copy.deepcopy(self.cfg.controller), robot_cfg=env.scene.cfg.robot, device=self.device + ) + ) # Create tensors to store raw and processed actions self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) @@ -113,8 +119,11 @@ def pose_dim(self) -> int: @property def action_dim(self) -> int: """Dimension of the action space (based on number of tasks and pose dimension).""" - # The tasks for all the controllers are the same, hence just using the first one to calculate the action_dim - return len(self._ik_controllers[0].cfg.variable_input_tasks) * self.pose_dim + self.hand_joint_dim + # Count only FrameTask instances in variable_input_tasks + frame_tasks_count = sum( + 1 for task in self._ik_controllers[0].cfg.variable_input_tasks if isinstance(task, FrameTask) + ) + return frame_tasks_count * self.pose_dim + self.hand_joint_dim @property def raw_actions(self) -> torch.Tensor: @@ -163,7 +172,6 @@ def process_actions(self, actions: torch.Tensor): """ # Store the raw actions self._raw_actions[:] = actions - self._processed_actions[:] = self.raw_actions # Make a copy of actions before modifying so that raw actions are not modified actions_clone = actions.clone() @@ -204,10 +212,11 @@ def process_actions(self, actions: torch.Tensor): # Loop through each task and set the target for env_index, ik_controller in enumerate(self._ik_controllers): for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): - target = task.transform_target_to_world - target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy() - target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy() - task.set_target(target) + if isinstance(task, FrameTask): + target = task.transform_target_to_world + target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy() + target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy() + task.set_target(target) def apply_actions(self): # start_time = time.time() # Capture the time before the step @@ -223,7 +232,7 @@ def apply_actions(self): all_envs_joint_pos_des = torch.cat((all_envs_joint_pos_des, self._target_hand_joint_positions), dim=1) self._processed_actions = all_envs_joint_pos_des - self._asset.set_joint_position_target(self._processed_actions, self._joint_ids) + self._asset.set_joint_position_target(all_envs_joint_pos_des, self._joint_ids) def reset(self, env_ids: Sequence[int] | None = None) -> None: """Reset the action term for specified environments. diff --git a/source/isaaclab/isaaclab/utils/string.py b/source/isaaclab/isaaclab/utils/string.py index e0bf5dc45780..ca6a5cca8886 100644 --- a/source/isaaclab/isaaclab/utils/string.py +++ b/source/isaaclab/isaaclab/utils/string.py @@ -272,7 +272,10 @@ def resolve_matching_names( def resolve_matching_names_values( - data: dict[str, Any], list_of_strings: Sequence[str], preserve_order: bool = False + data: dict[str, Any], + list_of_strings: Sequence[str], + preserve_order: bool = False, + require_all_keys_matched: bool = True, ) -> tuple[list[int], list[str], list[Any]]: """Match a list of regular expressions in a dictionary against a list of strings and return the matched indices, names, and values. @@ -293,6 +296,7 @@ def resolve_matching_names_values( data: A dictionary of regular expressions and values to match the strings in the list. list_of_strings: A list of strings to match. preserve_order: Whether to preserve the order of the query keys in the returned values. Defaults to False. + require_all_keys_matched: Whether to require that all keys in the dictionary are matched. Defaults to True. Returns: A tuple of lists containing the matched indices, names, and values. @@ -300,7 +304,7 @@ def resolve_matching_names_values( Raises: TypeError: When the input argument :attr:`data` is not a dictionary. ValueError: When multiple matches are found for a string in the dictionary. - ValueError: When not all regular expressions in the data keys are matched. + ValueError: When not all regular expressions in the data keys are matched (if require_all_keys_matched is True). """ # check valid input if not isinstance(data, dict): @@ -354,7 +358,7 @@ def resolve_matching_names_values( names_list = names_list_reorder values_list = values_list_reorder # check that all regular expressions are matched - if not all(keys_match_found): + if require_all_keys_matched and not all(keys_match_found): # make this print nicely aligned for debugging msg = "\n" for key, value in zip(data.keys(), keys_match_found): diff --git a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json new file mode 100644 index 000000000000..9d28a942316a --- /dev/null +++ b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json @@ -0,0 +1,87 @@ +{ + "tolerances": { + "position": 0.002, + "pd_position": 0.001, + "rotation": 0.02, + "check_errors": true + }, + "num_iterations": 12, + "tests": { + "stay_still": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "allowed_steps_per_motion": 10, + "repeat": 2 + }, + "vertical_movement": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] + ], + "allowed_steps_per_motion": 15, + "repeat": 2 + }, + "horizontal_movement": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.13, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.13, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "allowed_steps_per_motion": 15, + "repeat": 2 + }, + "horizontal_small_movement": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.22, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.22, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] + ], + "allowed_steps_per_motion": 15, + "repeat": 2 + }, + "forward_waist_bending_movement": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5] + ], + "allowed_steps_per_motion": 70, + "repeat": 3 + }, + "rotation_movements": { + "left_hand_pose": [ + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0], + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [-0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071] + ], + "right_hand_pose": [ + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071], + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], + [0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0] + ], + "allowed_steps_per_motion": 25, + "repeat": 2 + } + } +} diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index a1b7993b9bde..1a1c448885a7 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -4,6 +4,8 @@ # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" +import json +import os import sys # Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim @@ -20,9 +22,11 @@ import contextlib import gymnasium as gym +import numpy as np import torch import pytest +from pink.configuration import Configuration from isaaclab.utils.math import axis_angle_from_quat, matrix_from_quat, quat_from_matrix, quat_inv @@ -31,179 +35,328 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg +@pytest.fixture(scope="module") +def test_cfg(): + """Load test configuration.""" + config_path = os.path.join(os.path.dirname(__file__), "test_configs", "pink_ik_gr1_test_configs.json") + with open(config_path) as f: + return json.load(f) + + +@pytest.fixture(scope="module") +def test_params(test_cfg): + """Set up test parameters.""" + return { + "position_tolerance": test_cfg["tolerances"]["position"], + "rotation_tolerance": test_cfg["tolerances"]["rotation"], + "pd_position_tolerance": test_cfg["tolerances"]["pd_position"], + "check_errors": test_cfg["tolerances"]["check_errors"], + } + + +def create_test_env(num_envs): + """Create a test environment with the Pink IK controller.""" + env_name = "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0" + device = "cuda:0" + + try: + env_cfg = parse_env_cfg(env_name, device=device, num_envs=num_envs) + # Modify scene config to not spawn the packing table to avoid collision with the robot + del env_cfg.scene.packing_table + del env_cfg.terminations.object_dropping + del env_cfg.terminations.time_out + return gym.make(env_name, cfg=env_cfg).unwrapped, env_cfg + except Exception as e: + print(f"Failed to create environment: {str(e)}") + raise + + +@pytest.fixture(scope="module") +def env_and_cfg(): + """Create environment and configuration for tests.""" + env, env_cfg = create_test_env(num_envs=1) + + # Set up camera view + env.sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 1.0]) + + return env, env_cfg + + @pytest.fixture -def pink_ik_test_config(): - """Test configuration for Pink IK controller tests.""" - # End effector position mean square error tolerance in meters - pos_tolerance = 0.03 # 3 cm - # End effector orientation mean square error tolerance in radians - rot_tolerance = 0.17 # 10 degrees - - # Number of environments - num_envs = 1 - - # Number of joints in the 2 robot hands - num_joints_in_robot_hands = 22 - - # Number of steps to wait for controller convergence - num_steps_controller_convergence = 25 - - num_times_to_move_hands_up = 3 - num_times_to_move_hands_down = 3 - - # Create starting setpoints with respect to the env origin frame - # These are the setpoints for the forward kinematics result of the - # InitialStateCfg specified in `PickPlaceGR1T2EnvCfg` - y_axis_z_axis_90_rot_quaternion = [0.5, 0.5, -0.5, 0.5] - left_hand_roll_link_pos = [-0.23, 0.28, 1.1] - left_hand_roll_link_pose = left_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion - right_hand_roll_link_pos = [0.23, 0.28, 1.1] - right_hand_roll_link_pose = right_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion +def test_setup(env_and_cfg): + """Set up test case - runs before each test.""" + env, env_cfg = env_and_cfg + + num_joints_in_robot_hands = env_cfg.actions.pink_ik_cfg.controller.num_hand_joints + + # Get Action Term and IK controller + action_term = env.action_manager.get_term(name="pink_ik_cfg") + pink_controllers = action_term._ik_controllers + articulation = action_term._asset + + # Initialize Pink Configuration for forward kinematics + kinematics_model = Configuration( + pink_controllers[0].robot_wrapper.model, + pink_controllers[0].robot_wrapper.data, + pink_controllers[0].robot_wrapper.q0, + ) + left_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["left_wrist"] + right_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["right_wrist"] return { - "pos_tolerance": pos_tolerance, - "rot_tolerance": rot_tolerance, - "num_envs": num_envs, + "env": env, + "env_cfg": env_cfg, "num_joints_in_robot_hands": num_joints_in_robot_hands, - "num_steps_controller_convergence": num_steps_controller_convergence, - "num_times_to_move_hands_up": num_times_to_move_hands_up, - "num_times_to_move_hands_down": num_times_to_move_hands_down, - "left_hand_roll_link_pose": left_hand_roll_link_pose, - "right_hand_roll_link_pose": right_hand_roll_link_pose, + "action_term": action_term, + "pink_controllers": pink_controllers, + "articulation": articulation, + "kinematics_model": kinematics_model, + "left_target_link_name": left_target_link_name, + "right_target_link_name": right_target_link_name, } -def test_gr1t2_ik_pose_abs(pink_ik_test_config): - """Test IK controller for GR1T2 humanoid. +def test_stay_still(test_setup, test_cfg): + """Test staying still.""" + print("Running stay still test...") + run_movement_test(test_setup, test_cfg["tests"]["stay_still"], test_cfg) - This test validates that the Pink IK controller can accurately track commanded - end-effector poses for a humanoid robot. It specifically: - 1. Creates a GR1T2 humanoid robot with the Pink IK controller - 2. Sends target pose commands to the left and right hand roll links - 3. Checks that the observed poses of the links match the target poses within tolerance - 4. Tests adaptability by moving the hands up and down multiple times +def test_vertical_movement(test_setup, test_cfg): + """Test vertical movement of robot hands.""" + print("Running vertical movement test...") + run_movement_test(test_setup, test_cfg["tests"]["vertical_movement"], test_cfg) - The test succeeds when the controller can accurately converge to each new target - position, demonstrating both accuracy and adaptability to changing targets. - """ - env_name = "Isaac-PickPlace-GR1T2-Abs-v0" - device = "cuda:0" - env_cfg = parse_env_cfg(env_name, device=device, num_envs=pink_ik_test_config["num_envs"]) +def test_horizontal_movement(test_setup, test_cfg): + """Test horizontal movement of robot hands.""" + print("Running horizontal movement test...") + run_movement_test(test_setup, test_cfg["tests"]["horizontal_movement"], test_cfg) - # create environment from loaded config - env = gym.make(env_name, cfg=env_cfg).unwrapped - # reset before starting - obs, _ = env.reset() +def test_horizontal_small_movement(test_setup, test_cfg): + """Test small horizontal movement of robot hands.""" + print("Running horizontal small movement test...") + run_movement_test(test_setup, test_cfg["tests"]["horizontal_small_movement"], test_cfg) - num_runs = 0 # Counter for the number of runs - move_hands_up = True - test_counter = 0 +def test_forward_waist_bending_movement(test_setup, test_cfg): + """Test forward waist bending movement of robot hands.""" + print("Running forward waist bending movement test...") + run_movement_test(test_setup, test_cfg["tests"]["forward_waist_bending_movement"], test_cfg) + - # Get poses from config - left_hand_roll_link_pose = pink_ik_test_config["left_hand_roll_link_pose"].copy() - right_hand_roll_link_pose = pink_ik_test_config["right_hand_roll_link_pose"].copy() +def test_rotation_movements(test_setup, test_cfg): + """Test rotation movements of robot hands.""" + print("Running rotation movements test...") + run_movement_test(test_setup, test_cfg["tests"]["rotation_movements"], test_cfg) + + +def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): + """Run a movement test with the given configuration.""" + env = test_setup["env"] + num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"] + + left_hand_poses = np.array(test_config["left_hand_pose"], dtype=np.float32) + right_hand_poses = np.array(test_config["right_hand_pose"], dtype=np.float32) + + curr_pose_idx = 0 + test_counter = 0 + num_runs = 0 - # simulate environment -- run everything in inference mode with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): - while simulation_app.is_running() and not simulation_app.is_exiting(): + obs, _ = env.reset() + while simulation_app.is_running() and not simulation_app.is_exiting(): num_runs += 1 - setpoint_poses = left_hand_roll_link_pose + right_hand_roll_link_pose - actions = setpoint_poses + [0.0] * pink_ik_test_config["num_joints_in_robot_hands"] - actions = torch.tensor(actions, device=device) - actions = torch.stack([actions for _ in range(env.num_envs)]) - obs, _, _, _, _ = env.step(actions) + # Call auxiliary function if provided + if aux_function is not None: + aux_function(num_runs) - left_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][ - :, env.scene["robot"].data.body_names.index("left_hand_roll_link"), :7 - ] - right_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][ - :, env.scene["robot"].data.body_names.index("right_hand_roll_link"), :7 - ] - - # The setpoints are wrt the env origin frame - # The observations are also wrt the env origin frame - left_hand_roll_link_feedback = left_hand_roll_link_pose_obs - left_hand_roll_link_setpoint = ( - torch.tensor(left_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1) - ) - left_hand_roll_link_pos_error = left_hand_roll_link_setpoint[:, :3] - left_hand_roll_link_feedback[:, :3] - left_hand_roll_link_rot_error = axis_angle_from_quat( - quat_from_matrix( - matrix_from_quat(left_hand_roll_link_setpoint[:, 3:]) - * matrix_from_quat(quat_inv(left_hand_roll_link_feedback[:, 3:])) - ) - ) - - right_hand_roll_link_feedback = right_hand_roll_link_pose_obs - right_hand_roll_link_setpoint = ( - torch.tensor(right_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1) - ) - right_hand_roll_link_pos_error = right_hand_roll_link_setpoint[:, :3] - right_hand_roll_link_feedback[:, :3] - right_hand_roll_link_rot_error = axis_angle_from_quat( - quat_from_matrix( - matrix_from_quat(right_hand_roll_link_setpoint[:, 3:]) - * matrix_from_quat(quat_inv(right_hand_roll_link_feedback[:, 3:])) - ) - ) - - if num_runs % pink_ik_test_config["num_steps_controller_convergence"] == 0: - # Check if the left hand roll link is at the target position - torch.testing.assert_close( - torch.mean(torch.abs(left_hand_roll_link_pos_error), dim=1), - torch.zeros(env.num_envs, device="cuda:0"), - rtol=0.0, - atol=pink_ik_test_config["pos_tolerance"], - ) + # Create actions from hand poses and joint positions + setpoint_poses = np.concatenate([left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx]]) + actions = np.concatenate([setpoint_poses, np.zeros(num_joints_in_robot_hands)]) + actions = torch.tensor(actions, device=env.device, dtype=torch.float32) + actions = actions.repeat(env.num_envs, 1) - # Check if the right hand roll link is at the target position - torch.testing.assert_close( - torch.mean(torch.abs(right_hand_roll_link_pos_error), dim=1), - torch.zeros(env.num_envs, device="cuda:0"), - rtol=0.0, - atol=pink_ik_test_config["pos_tolerance"], - ) + # Step environment + obs, _, _, _, _ = env.step(actions) - # Check if the left hand roll link is at the target orientation - torch.testing.assert_close( - torch.mean(torch.abs(left_hand_roll_link_rot_error), dim=1), - torch.zeros(env.num_envs, device="cuda:0"), - rtol=0.0, - atol=pink_ik_test_config["rot_tolerance"], + # Check convergence and verify errors + if num_runs % test_config["allowed_steps_per_motion"] == 0: + print("Computing errors...") + errors = compute_errors( + test_setup, env, left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx] ) + print_debug_info(errors, test_counter) + if test_cfg["tolerances"]["check_errors"]: + verify_errors(errors, test_setup, test_cfg["tolerances"]) + + curr_pose_idx = (curr_pose_idx + 1) % len(left_hand_poses) + if curr_pose_idx == 0: + test_counter += 1 + if test_counter > test_config["repeat"]: + print("Test completed successfully") + break + + +def get_link_pose(env, link_name): + """Get the position and orientation of a link.""" + link_index = env.scene["robot"].data.body_names.index(link_name) + link_states = env.scene._articulations["robot"]._data.body_link_state_w + link_pose = link_states[:, link_index, :7] + return link_pose[:, :3], link_pose[:, 3:7] + + +def calculate_rotation_error(current_rot, target_rot): + """Calculate the rotation error between current and target orientations in axis-angle format.""" + if isinstance(target_rot, torch.Tensor): + target_rot_tensor = ( + target_rot.unsqueeze(0).expand(current_rot.shape[0], -1) if target_rot.dim() == 1 else target_rot + ) + else: + target_rot_tensor = torch.tensor(target_rot, device=current_rot.device) + if target_rot_tensor.dim() == 1: + target_rot_tensor = target_rot_tensor.unsqueeze(0).expand(current_rot.shape[0], -1) + + return axis_angle_from_quat( + quat_from_matrix(matrix_from_quat(target_rot_tensor) * matrix_from_quat(quat_inv(current_rot))) + ) + + +def compute_errors(test_setup, env, left_target_pose, right_target_pose): + """Compute all error metrics for the current state.""" + action_term = test_setup["action_term"] + pink_controllers = test_setup["pink_controllers"] + articulation = test_setup["articulation"] + kinematics_model = test_setup["kinematics_model"] + left_target_link_name = test_setup["left_target_link_name"] + right_target_link_name = test_setup["right_target_link_name"] + num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"] + + # Get current hand positions and orientations + left_hand_pos, left_hand_rot = get_link_pose(env, left_target_link_name) + right_hand_pos, right_hand_rot = get_link_pose(env, right_target_link_name) + + # Create setpoint tensors + device = env.device + num_envs = env.num_envs + left_hand_pose_setpoint = torch.tensor(left_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) + right_hand_pose_setpoint = torch.tensor(right_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) + # compensate for the hand rotational offset + # nominal_hand_pose_rotmat = matrix_from_quat(torch.tensor(env_cfg.actions.pink_ik_cfg.controller.hand_rotational_offset, device=env.device)) + left_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(left_hand_pose_setpoint[:, 3:7])) + right_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(right_hand_pose_setpoint[:, 3:7])) + + # Calculate position and rotation errors + left_pos_error = left_hand_pose_setpoint[:, :3] - left_hand_pos + right_pos_error = right_hand_pose_setpoint[:, :3] - right_hand_pos + left_rot_error = calculate_rotation_error(left_hand_rot, left_hand_pose_setpoint[:, 3:]) + right_rot_error = calculate_rotation_error(right_hand_rot, right_hand_pose_setpoint[:, 3:]) + + # Calculate PD controller errors + ik_controller = pink_controllers[0] + pink_controlled_joint_ids = action_term._pink_controlled_joint_ids + + # Get current and target positions + curr_joints = articulation.data.joint_pos[:, pink_controlled_joint_ids].cpu().numpy()[0] + target_joints = action_term.processed_actions[:, :num_joints_in_robot_hands].cpu().numpy()[0] + + # Reorder joints for Pink IK + curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_ordering] + target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_ordering] + + # Run forward kinematics + kinematics_model.update(curr_joints) + left_curr_pos = kinematics_model.get_transform_frame_to_world( + frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link" + ).translation + right_curr_pos = kinematics_model.get_transform_frame_to_world( + frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link" + ).translation + + kinematics_model.update(target_joints) + left_target_pos = kinematics_model.get_transform_frame_to_world( + frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link" + ).translation + right_target_pos = kinematics_model.get_transform_frame_to_world( + frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link" + ).translation + + # Calculate PD errors + left_pd_error = ( + torch.tensor(left_target_pos - left_curr_pos, device=device, dtype=torch.float32) + .unsqueeze(0) + .repeat(num_envs, 1) + ) + right_pd_error = ( + torch.tensor(right_target_pos - right_curr_pos, device=device, dtype=torch.float32) + .unsqueeze(0) + .repeat(num_envs, 1) + ) + + return { + "left_pos_error": left_pos_error, + "right_pos_error": right_pos_error, + "left_rot_error": left_rot_error, + "right_rot_error": right_rot_error, + "left_pd_error": left_pd_error, + "right_pd_error": right_pd_error, + } - # Check if the right hand roll link is at the target orientation - torch.testing.assert_close( - torch.mean(torch.abs(right_hand_roll_link_rot_error), dim=1), - torch.zeros(env.num_envs, device="cuda:0"), - rtol=0.0, - atol=pink_ik_test_config["rot_tolerance"], - ) - # Change the setpoints to move the hands up and down as per the counter - test_counter += 1 - if move_hands_up and test_counter > pink_ik_test_config["num_times_to_move_hands_up"]: - move_hands_up = False - elif not move_hands_up and test_counter > ( - pink_ik_test_config["num_times_to_move_hands_down"] - + pink_ik_test_config["num_times_to_move_hands_up"] - ): - # Test is done after moving the hands up and down - break - if move_hands_up: - left_hand_roll_link_pose[1] += 0.05 - left_hand_roll_link_pose[2] += 0.05 - right_hand_roll_link_pose[1] += 0.05 - right_hand_roll_link_pose[2] += 0.05 - else: - left_hand_roll_link_pose[1] -= 0.05 - left_hand_roll_link_pose[2] -= 0.05 - right_hand_roll_link_pose[1] -= 0.05 - right_hand_roll_link_pose[2] -= 0.05 - - env.close() +def verify_errors(errors, test_setup, tolerances): + """Verify that all error metrics are within tolerance.""" + env = test_setup["env"] + device = env.device + num_envs = env.num_envs + zero_tensor = torch.zeros(num_envs, device=device) + + for hand in ["left", "right"]: + # Check PD controller errors + pd_error_norm = torch.norm(errors[f"{hand}_pd_error"], dim=1) + torch.testing.assert_close( + pd_error_norm, + zero_tensor, + rtol=0.0, + atol=tolerances["pd_position"], + msg=( + f"{hand.capitalize()} hand PD controller error ({pd_error_norm.item():.6f}) exceeds tolerance" + f" ({tolerances['pd_position']:.6f})" + ), + ) + + # Check IK position errors + pos_error_norm = torch.norm(errors[f"{hand}_pos_error"], dim=1) + torch.testing.assert_close( + pos_error_norm, + zero_tensor, + rtol=0.0, + atol=tolerances["position"], + msg=( + f"{hand.capitalize()} hand IK position error ({pos_error_norm.item():.6f}) exceeds tolerance" + f" ({tolerances['position']:.6f})" + ), + ) + + # Check rotation errors + rot_error_max = torch.max(errors[f"{hand}_rot_error"]) + torch.testing.assert_close( + rot_error_max, + torch.zeros_like(rot_error_max), + rtol=0.0, + atol=tolerances["rotation"], + msg=( + f"{hand.capitalize()} hand IK rotation error ({rot_error_max.item():.6f}) exceeds tolerance" + f" ({tolerances['rotation']:.6f})" + ), + ) + + +def print_debug_info(errors, test_counter): + """Print debug information about the current state.""" + print(f"\nTest iteration {test_counter + 1}:") + for hand in ["left", "right"]: + print(f"Measured {hand} hand position error:", errors[f"{hand}_pos_error"]) + print(f"Measured {hand} hand rotation error:", errors[f"{hand}_rot_error"]) diff --git a/source/isaaclab/test/utils/test_string.py b/source/isaaclab/test/utils/test_string.py index 501f73805810..6b4a50bea457 100644 --- a/source/isaaclab/test/utils/test_string.py +++ b/source/isaaclab/test/utils/test_string.py @@ -162,6 +162,14 @@ def test_resolve_matching_names_values_with_basic_strings(): query_names = {"a|c": 1, "b": 0, "f": 2} with pytest.raises(ValueError): _ = string_utils.resolve_matching_names_values(query_names, target_names) + # test require_all_keys_matched=False + data = {"a|c": 1, "b": 2, "f": 3} + index_list, names_list, values_list = string_utils.resolve_matching_names_values( + data, target_names, require_all_keys_matched=False + ) + assert index_list == [0, 1, 2] + assert names_list == ["a", "b", "c"] + assert values_list == [1, 2, 1] def test_resolve_matching_names_values_with_basic_strings_and_preserved_order(): diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 289d07ed5b79..0299870aca2e 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.10.46" +version = "0.10.47" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 98e23db6c816..07728e84697b 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,25 @@ Changelog --------- +0.10.47 (2025-07-25) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* New ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` environment that enables the waist degrees-of-freedom for the GR1T2 robot. + + +Changed +^^^^^ + +* Updated pink inverse kinematics controller configuration for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``) + to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom without + the robot control drifting to a bending posture. +* Tuned the pink inverse kinematics controller and joint PD controllers for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``) + to improve the end-effector tracking accuracy and speed. Achieving position and orientation accuracy test within **(2 mm, 1 degree)**. + + 0.10.46 (2025-08-16) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py index db926c6a162c..e1f785cd78b6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py @@ -6,7 +6,13 @@ import gymnasium as gym import os -from . import agents, exhaustpipe_gr1t2_pink_ik_env_cfg, nutpour_gr1t2_pink_ik_env_cfg, pickplace_gr1t2_env_cfg +from . import ( + agents, + exhaustpipe_gr1t2_pink_ik_env_cfg, + nutpour_gr1t2_pink_ik_env_cfg, + pickplace_gr1t2_env_cfg, + pickplace_gr1t2_waist_enabled_env_cfg, +) gym.register( id="Isaac-PickPlace-GR1T2-Abs-v0", @@ -37,3 +43,12 @@ }, disable_env_checker=True, ) + +gym.register( + id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": pickplace_gr1t2_waist_enabled_env_cfg.PickPlaceGR1T2WaistEnabledEnvCfg, + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index 554203a8b7c2..9675c3030c30 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -9,6 +9,7 @@ import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.devices.openxr import XrCfg from isaaclab.envs import ManagerBasedRLEnvCfg @@ -134,6 +135,52 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=9000, + damping=160.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + stiffness=9000.0, + damping=160.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + stiffness=9000.0, + damping=160.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + stiffness=400, + damping=8, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + stiffness=400, + damping=8, + ), + }, ) # Set table view camera diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index c430a194483d..2424599cebb5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -3,9 +3,10 @@ # # SPDX-License-Identifier: BSD-3-Clause -from pink.tasks import FrameTask +from pink.tasks import DampingTask, FrameTask import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg from isaaclab.devices import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg @@ -108,6 +109,10 @@ def __post_init__(self): "L_thumb_distal_joint", "R_thumb_distal_joint", ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, # the robot in the sim scene we are controlling asset_name="robot", # Configuration for the IK controller @@ -118,20 +123,52 @@ def __post_init__(self): base_link_name="base_link", num_hand_joints=22, show_ik_warnings=False, + fail_on_joint_limit_violation=False, # Determines whether to pink solver will fail due to a joint limit violation variable_input_tasks=[ FrameTask( "GR1T2_fourier_hand_6dof_left_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, ), FrameTask( "GR1T2_fourier_hand_6dof_right_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + frame_task_controlled_joints={ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + }, ), ], fixed_input_tasks=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index a59bd6dfab3e..004fb501a800 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -9,6 +9,7 @@ import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.devices.openxr import XrCfg from isaaclab.envs import ManagerBasedRLEnvCfg @@ -155,6 +156,52 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=9000, + damping=160.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + stiffness=9000.0, + damping=160.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + stiffness=9000.0, + damping=160.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + stiffness=400, + damping=8, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + stiffness=400, + damping=8, + ), + }, ) # Set table view camera diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index fd39e47df7ae..ea074f230435 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -3,9 +3,10 @@ # # SPDX-License-Identifier: BSD-3-Clause -from pink.tasks import FrameTask +from pink.tasks import DampingTask, FrameTask import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg from isaaclab.devices import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg @@ -106,6 +107,10 @@ def __post_init__(self): "L_thumb_distal_joint", "R_thumb_distal_joint", ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, # the robot in the sim scene we are controlling asset_name="robot", # Configuration for the IK controller @@ -116,20 +121,52 @@ def __post_init__(self): base_link_name="base_link", num_hand_joints=22, show_ik_warnings=False, + fail_on_joint_limit_violation=False, # Determines whether to pink solver will fail due to a joint limit violation variable_input_tasks=[ FrameTask( "GR1T2_fourier_hand_6dof_left_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, ), FrameTask( "GR1T2_fourier_hand_6dof_right_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + frame_task_controlled_joints={ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + }, ), ], fixed_input_tasks=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index f19bc3629f60..e4d08c712180 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -6,12 +6,14 @@ import tempfile import torch -from pink.tasks import FrameTask +from pink.tasks import DampingTask, FrameTask import isaaclab.controllers.utils as ControllerUtils import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg @@ -93,6 +95,52 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=4400, + damping=40.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + stiffness=None, + damping=None, + ), + }, ) # Ground plane @@ -199,6 +247,10 @@ class ActionsCfg: "L_thumb_distal_joint", "R_thumb_distal_joint", ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, # the robot in the sim scene we are controlling asset_name="robot", # Configuration for the IK controller @@ -209,20 +261,52 @@ class ActionsCfg: base_link_name="base_link", num_hand_joints=22, show_ik_warnings=False, + fail_on_joint_limit_violation=False, # Determines whether to pink solver will fail due to a joint limit violation variable_input_tasks=[ FrameTask( "GR1T2_fourier_hand_6dof_left_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, ), FrameTask( "GR1T2_fourier_hand_6dof_right_hand_pitch_link", - position_cost=1.0, # [cost] / [m] + position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] lm_damping=10, # dampening for solver for step jumps - gain=0.1, + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.1, + lm_damping=1, + frame_task_controlled_joints={ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + }, ), ], fixed_input_tasks=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py new file mode 100644 index 000000000000..e19b339b8862 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -0,0 +1,133 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile +import torch + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.utils import configclass + +from .pickplace_gr1t2_env_cfg import ActionsCfg, EventCfg, ObjectTableSceneCfg, ObservationsCfg, TerminationsCfg + + +@configclass +class PickPlaceGR1T2WaistEnabledEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), + # left hand joint pos (11), right hand joint pos (11)] + idle_action = torch.tensor([ + -0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ]) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 6 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 120 # 120Hz + self.sim.render_interval = 2 + + # Add waist joint to pink_ik_cfg + self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_yaw_joint") + self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_pitch_joint") + self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_roll_joint") + self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_yaw_joint") + self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_pitch_joint") + self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_roll_joint") + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + ControllerUtils.change_revolute_to_fixed( + temp_urdf_output_path, self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path + self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) From bcb1ae4a0d83505fff67b98a24952c26fb8c3d5c Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Thu, 14 Aug 2025 14:14:02 -0700 Subject: [PATCH 02/12] Adding mimic envs for Waist Enabled Environment --- source/isaaclab_mimic/config/extension.toml | 2 +- source/isaaclab_mimic/docs/CHANGELOG.rst | 8 ++ .../envs/pinocchio_envs/__init__.py | 10 ++ ...pickplace_gr1t2_waist_enabled_mimic_env.py | 129 ++++++++++++++++++ ...place_gr1t2_waist_enabled_mimic_env_cfg.py | 111 +++++++++++++++ .../manipulation/pick_place/__init__.py | 1 + 6 files changed, 260 insertions(+), 1 deletion(-) create mode 100644 source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py create mode 100644 source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py diff --git a/source/isaaclab_mimic/config/extension.toml b/source/isaaclab_mimic/config/extension.toml index 88cffbe962d2..5fa8eb214513 100644 --- a/source/isaaclab_mimic/config/extension.toml +++ b/source/isaaclab_mimic/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "1.0.12" +version = "1.0.13" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index c4260d355d12..a234c5cd3ab8 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +1.0.13 (2025-08-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`PickPlaceGR1T2WaistEnabledEnvCfg` and :class:`PickPlaceGR1T2WaistEnabledMimicEnvCfg` for GR1T2 robot manipulation tasks with waist joint control enabled. + 1.0.12 (2025-07-31) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py index 89dc84590215..c782576c3630 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -11,6 +11,7 @@ from .nutpour_gr1t2_mimic_env_cfg import NutPourGR1T2MimicEnvCfg from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg +from .pickplace_gr1t2_waist_enabled_mimic_env_cfg import PickPlaceGR1T2WaistEnabledMimicEnvCfg gym.register( id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0", @@ -21,6 +22,15 @@ disable_env_checker=True, ) +gym.register( + id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs.pinocchio_envs:PickPlaceGR1T2MimicEnv", + kwargs={ + "env_cfg_entry_point": pickplace_gr1t2_waist_enabled_mimic_env_cfg.PickPlaceGR1T2WaistEnabledMimicEnvCfg, + }, + disable_env_checker=True, +) + gym.register( id="Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0", entry_point="isaaclab_mimic.envs.pinocchio_envs:PickPlaceGR1T2MimicEnv", diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py new file mode 100644 index 000000000000..8b12863de17d --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py @@ -0,0 +1,129 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class PickPlaceGR1T2WaistEnabledMimicEnv(ManagerBasedRLMimicEnv): + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + eef_pos_name = f"{eef_name}_eef_pos" + eef_quat_name = f"{eef_name}_eef_quat" + + target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_wrist_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + noise: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + + # target position and rotation + target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"]) + target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"]) + + target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) + target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) + + # gripper actions + left_gripper_action = gripper_action_dict["left"] + right_gripper_action = gripper_action_dict["right"] + + if action_noise_dict is not None: + pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) + pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) + quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) + quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) + + target_left_eef_pos += pos_noise_left + target_right_eef_pos += pos_noise_right + target_left_eef_rot_quat += quat_noise_left + target_right_eef_rot_quat += quat_noise_right + + return torch.cat( + ( + target_left_eef_pos, + target_left_eef_rot_quat, + target_right_eef_pos, + target_right_eef_rot_quat, + left_gripper_action, + right_gripper_action, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + target_poses = {} + + target_left_wrist_position = action[:, 0:3] + target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) + target_poses["left"] = target_pose_left + + target_right_wrist_position = action[:, 7:10] + target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) + target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) + target_poses["right"] = target_pose_right + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + return {"left": actions[:, 14:25], "right": actions[:, 25:]} diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py new file mode 100644 index 000000000000..5dd25d235384 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py @@ -0,0 +1,111 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_waist_enabled_env_cfg import ( + PickPlaceGR1T2WaistEnabledEnvCfg, +) + + +@configclass +class PickPlaceGR1T2WaistEnabledMimicEnvCfg(PickPlaceGR1T2WaistEnabledEnvCfg, MimicEnvCfg): + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "demo_src_gr1t2_demo_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = False + self.datagen_config.generation_num_trials = 1000 + self.datagen_config.generation_select_src_per_subtask = False + self.datagen_config.generation_select_src_per_arm = False + self.datagen_config.generation_relative = False + self.datagen_config.generation_joint_pos = False + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.num_demo_to_render = 10 + self.datagen_config.num_fail_demo_to_render = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="idle_right", + first_subtask_start_offset_range=(0, 0), + # Randomization range for starting index of the first subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for the source subtask segment during data generation + # selection_strategy="nearest_neighbor_object", + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=3, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["right"] = subtask_configs + + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["left"] = subtask_configs diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py index e1f785cd78b6..ff72798e0f4c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py @@ -49,6 +49,7 @@ entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ "env_cfg_entry_point": pickplace_gr1t2_waist_enabled_env_cfg.PickPlaceGR1T2WaistEnabledEnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), }, disable_env_checker=True, ) From e49e787d07a18a6c5ab36a573a354aeb89ef514f Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Fri, 15 Aug 2025 14:27:20 -0700 Subject: [PATCH 03/12] Updating documentation --- .../manipulation/gr-1_pick_place_waist.jpg | Bin 0 -> 61739 bytes docs/source/overview/environments.rst | 5 +++++ docs/source/overview/teleop_imitation.rst | 5 ++++- 3 files changed, 9 insertions(+), 1 deletion(-) create mode 100644 docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg diff --git a/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg b/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg new file mode 100644 index 0000000000000000000000000000000000000000..1f99cb72741922f75be1236462193841eb6b7f31 GIT binary patch literal 61739 zcmb4qby$;M{PtkL*k~9jog)>TNREw=hEalmq5?8dkbxpdmxR)#h-?zlAl;~R%1CMH z?rz_Ge{cN%o@rADIpLrB^V5$rlW>X(NclIGz>JfboBHL^bl%BCPsQD;&=N079siX zNgx>|@j-ejFctCF|IhL7CxC&H$Y1^`~Zufnu%?O5Zt|N4tz&F*lzb#Gwa|UaXC&Bq9x)Q|MztMGxvY25oZ+`NQl-* z$$%7O%eK?RHl1XZ8T!QJ+lGs^x=0H}e) zg&BYh02RR0+tuxuZ`zO3?JKb&Q*ZD(K`g+yZ+Cd#2%fj}a;=IT~P zZ0<6yU%Pl=%dkHdu5U!;@vcdLD-Ub!9o4E64$!XJo8y}=Ppp+xPVrGjqftE|?!6r- z^#-Ybvp(sDA$&K64;kF=z-Qe66FF;N4j{FwF*z}HQ*I9wo6iBAgy$k)LLzh3&4FJ# z#o45kX1sU5?zWN813!0s=IU=YZ!U2wMcs(ClaX0?4r~+QKBv^A+?2!n55)aN8t(v( zpqg>`I3{Rt`~+wJ43x6V*+D28cxv#MgOnv+_K^}4w}xzFUb*Yc_xNh)9DREro6ll> zirW!ROH)HJ8%df}6V=in@{^!4#B<>yq}KW@a4muDgHq=KY7R~nIc4CPC8C_3hv#ER zx)9~%URv~@4>-CR7&~!ag*51g7^P_r!%^I?Wf=D-slc*$-$cq@pG|GNqp#iK4pKKD zYLK`&%>s14Rm%!e-7c;*LZYDMJ!k>R%>EeraudqHO#jBXF(a_qI9}FhvOTbQwpb&J zM>-Ys_PN8x_cT974U)r;E9;>-6Eq}Zj{au*x95;UTuaw|j_zHahy`Sb=-5?o4(F9I z24dU&aziku--%S`dT?k9P8G#ffI1=YFRGQ*{CF40+`{=jXBr7C^o=!4&84%%l}#w0 z;g#PGbpgC#YejzDfG8Dei!bDzM!(P8_+3(cDo?P``vk!>9N zy=vMLDooM1LW=A^n=z;O2Ffctw`@ggtzC^4?_nIt6W!=BrdtsNf5x9lV>M!eC9RkjT$C z6o2u}o5#7Tm!`3XKfaJ`e6;ut9lsRe4|ji&VmQzQ-j?S%%}A?>AEcC!pxuXw=POQy zbDvY>%Yq8$O_nhn4>bAr4cmk6ae0WIi;BNI(T1`h+(3?$YFD33Y!Q3G$fKUpI@}pM zb;%ANA&a;__EUniWQ+T=MOh_IausewojXUh(z{>J=<@umeHyu-y7Tad~A!rb{ zsC9y-Epk*eIG_yC z6K=DeN|`L(D?W`RWF7PYgTE?f?VuFju+wa^V5d`oYa1e?_L-Leo>0=;Y&N5g7?qH% zJv`a>ANgxM4Me|WVpPL@H|E@h{5mC&|4bX}iJjKkk&HvJX69a{d1E-=O*=nO-Mchp zJSvVUp4Zxf(#Qz#6qg*M<+Q3CTzAR>QY34SO!3~`7!vGvM8u1N-p7|1_CHPXf^^eR zYSZfWsptAY_3G%QrA0ph6faJMQ_QywIF{{jfagb<%M$$G1$J49Z1Aad2PAfwi=6tu z4HOCyU+;cCnMywVcynHBR~`?UGeq2cO-jq_rj3{YuE{MXUX&xrDuZJgm14;OrX6&r z@44><$?VzOWpSUuu!7OQ4J!z@s*1H$I6xUnp)Sd3=JDG2IBDVF4L%HLC!JvrA6puj z)Me+-!@7jxW&@4J#yr)hpDpk_-Ig5f0t`EO?fOtivd_kaRnnUh z7a6rA;R#36X==$IhR`{ae{<;h5M5}naYKplwH=4We_?!4!m|@+eb1{{ey(FTEv7sC z9&_HvGwN%lechG<^K}H=lrck*o?M>6hbwX!=Q$CcZ=oI6G{h?zJHzqakV7kHhv{*K3y_B1Yd2TG^&ROw4gVL>mUW(*BE0Dy!F8)a~uDJPsJ4H3!LV3Y$Uywl^Z2R%|{+g@aHx3}fs z>Qe2A-QN%2FkE!N-sh>6rk%7>zAw>e4Su#bHae)lcmRHy6KFZHlq>j8GJ%< z^+uWL^AC-#6HTCpS=&?hOJ~yvaD%jjI%Aw5$vOItIXG+kJS-0HkkwF9I0vU$Fj~gY z^B&;&_`Io8L6nBM=MlnzYgJwCri{`vcohZm)qKrSEO#3aNopJ%?*|2yXiWy-WI$^u z-uPIRrAeZn41r!eYaGMhd**c~MgZm2cXN%?U_wOmV}J~O(i4J`QU7!*wd$Czj%3{q zB%v3Y7D@7pF3eHKXp4mHlmMu4WP`od?1fO|t=9enB->QJ0wDg5e@OF;$FOF@c+9Fg z$^y3@x-cYlaLxbS-a;vq*zYS#rwdGEy$vH4I{|qNSviqHEul6`Kh|+i7X6^ixQII+ zUiV4>JL5|C;q(;K2B(SydCB_TECM%q)-jzk3CxIHOzW5c55Vmy4>NYubcPhm4W^+k zMcDBbFXQa|`zI}5+brKb)Is7d5hTF#df#?HskvU>@~vQ!y*%6;Dz>d*LaA^L z)mQDVgllV`qLrlC{{adgekfj=L;Bqdod3Q4!$I~&?IPng)HjSGi1jJu`fO|^??1p0 zoB`UR-Q~69f3$Fbe`7_J0&MbvQX8i(r$2%N6z;A4Y$I>Kc@iGXuq-N2<9349+PYT% zaW*zEt>Oh+ANI{_12XVWt+jNH-^FObi}qn=@PjeV`9MI1n;VSN*n1U%Zu(=z{Vebj#kmAr1ZPnC$s!WM{{bjq ztXWnRs3*JLgWvtwGJ-xByY+Ml)V4x#?=*liD`T@Nb?!)=Dd6bjII^uU?spJl24Y~p z4?rual&D!1b#6KdOkygY8tA9!kMxbk6u#9Y+HrFzu2!o>8< zeDncgf+9Fwoxljbu;HNC%_VT!+D4K$XLd3n)m|QxM}pq2PP^dv^FcyJT|FfNWAXZA zX&$YDM?=V2@F8|sA7!NUhawM1NJ4_>0q0%DKOW@Hm zxW#5zEgZo9s3d=a(I;0p%CD~Ni2Y+l=JqV~B6^rfY=y&-JBh8TX{5~4ecssPFCa&(+e%l7%rno_bY^1*3Eqj)D24}y`RrYI$&a^Zf4R<5jOfADOc@oQ&w8o|=#Tzf8+NuDdu z3vXDe2H`#jGhU~}`eG6U5++zUwT?MC(en4d+00CAFoxYqto@(j65r8n{V`HPg@h*8 zcezuA1G4oycKpb^C4DgvnTC_LdaIj$P9bKOM9n&N?L@{Yem)G6Q{(_o*668vGVxg} zPj$2h+3P-8KDE=(Nje!=7yqxt;)k>PWp2wF+6D{Ez#^LL5-XPP-rrq0bP>{=PP>HV zyJ)@+g(?Mw(mqk6Am)aPmT6>!suuz9?QHiFL-&a3yB1!*KaR^Y!~&=yLKzeR7)LD-+85G8m2xu2(OgHw4AllxzB`*F-es-SmM=x5wYv7C4LUmQb$!8(?W6qdR(wEDB zs*Jo6F21TA;$yAof1Ek?mM8_41@93v1<$8q-t$7{n4kEPPJ+4H?WlBCx8SLwTnIqx zm+J)pujCxurE?%~js}XtxC=cFX59R((G1wcgmpa4W*Mp;EeNx>X!x1?syG7l**&fFy*Ts6+YpS&c!}1DmN_?7t z{`8*~Zd6s>kKLK>)c&CMg~=U!k&52Ktwl-f{cpoEDQLOaue0}tp>Z$M40`2PKA@E> zmy+zYkX-9n(JKt+V|!QYzH2T)=Zof@3Zde>up2z~tysD!Cb0e}XBsn~5vOvI zpeXl3&B=iF=r--!-D2)|#m|qJFB?`rJkQ=OOX)73U``t_ic42V-(};><-kt=@FgWX zA?SrD{m2jU2_?&my5+)--V=bb8etn`#vI)g+B;5PK5;zZ0FxC6hPeKYSl#B?ah%^5izF#B8C^N#dys} zuK?m7p1W$uEuw2ZH~eWwkVkKIvOOHg@Fz7b84N-ckJ4REc3+JKT#g_Qe>}%5YWU8} zV-*+?z@rR_91|tN>@zV^FV~t%1?6r*A6SlvQJ5k1XFX=Y1K7&~^gi#xEC&3UVZ;~O zC0*Y8Sb`4o>lCfNDS{Ii*e=u-*-Y^?+rAGIuXjo7e%mpxka#tp5aUa3obV--^lt?R zDJTjcs+nzbE-+QomW>~{@FtocEj770ETv|W?@xlo-%s)l88ox>(9v#98Rn_Q~n z5+9ZP*)@toKbBBnOExlP@&`NeP->ZRfnmEGht~^DgXR|Tm zR$m4B;xn)3F?9X$dm%7cl_4$7MOZj@@?mi6WtjOzk8t2ekX@k09le|K>-}|s*$uvS zFmD)(>OFU4-Q1WK9(5Fx-{6g*Or7^o(ws!`>f>E%-OOi`11|eWHYC=_zQIFz+E*`w z{4FVa$MWP|refx?I#(Fh-?W1EH3L8Q?)fVb_}pR<>)ti=phcLJ+DJ<0joa8)_ycFn z2vDDh6IIaM5#juav6C!)O+sf~P3a1ri6dxtg=>LxPZ;dWi@`|>dfJxJm zXPvijU7*RfR+j!Nz?Y%o8F)s};umK(Da`EqiDA7z z#v9=rFK;Wt1EJEc2jD*k=v56>7spu8&8(nq=MN4hT#f04uWh=9R{7%XymNWq&KywE z-KIwT`RnZCc)XBO75**H&Q4fS`{16xf@p@-StpEJKg%zX_OMoGtZC&3lxAaQ`H|~c ze$X9BO}ES){1Y3Nj<*z~nj?Y^H3X|rw;HQqGe+AYEHzBX3g#JokJwE*E0pXU=k^Ep z;$Q0`28apxI^tRC*Ht&0$ZX#T*t~(iuSzbI&Rffg;yNx@%(qO2rHhK~7;P@A8%goV zQPO`HD~g!Muq_IfN7q@~oyoTm3rr#s6ojbw2Ux=3`k^2eb-IV+(dn~^)wq{MNM+fn zAZ6}hy7&E}nzQ7<&@RzwdVV4@HY9Q&9E9>oiyF6b<~f|uFim%EH(gJ~uRihwdFh6?WH339Ik41EDp zo8+XR_P{l@=ogbtM!W2I&-b@+nKj@YHtqwIl7kcjYwK|TJ>|Jw6#?~!clnE!IzxF+ zOOtpofJ;nXs<(bqI=hj5Rr&EkA@mvWLRu{Z!D3FfUK1ncFi-uEx;)i_HHz+9H99#ytXkU%7?&0tYWqk z+qNKDOR79Jy|B}G=*0Rc;Ftp-#i_aWw8LT-rG3f)!ph9`U|?M1eI%6qLJ;tPOZx6P ziq^63YYh=QydB=ap&lcyjywuj_w*Wfg4{U!J zQ5y0q`8@1-44KFjg&&;eY`TJO`?xdwKquKxIB;uS{IkmbAesNUmX1Fk@=wW$Bk-d2 zynt^gEB5oova1sI5Cr-gZ;Y43K8|DoIC0H?al7rK5Op>58JN5IvK z;im>X)}~&0@+T0O>@ZpGTlMdtm-mB8`k^d|$Df_KGZUv3v1{}6)G&yTpmzmzIoNWWLqrmArp zT93K6%T=12HGaieh;v9{yk``Og&)K4mL zjB)F@P@whqa!E0NgM6jqpZ)hujrTPp=~SL!ZWZe!ap^dsKRRm5@#!yKVtCD(A9-N4 z`VsWRLWAc~C1UNqK% zv&MN_f1nDHwaaz0p(7>2T<*uqL4&-%YHSI~2U57*X_UZN)-@h2@@jf7*7UKQ$;rF< zpt2%2ll0otKAsLf;4CeMeN|f3FL~=mE3c%@KY%rwbR+_gm9fpKr4|CtnEHzW>@4YT zO^H&eM9r!Yd+5ICgmuetV0pxI^K}^aO}abvx8?=@0ltd;1JuK59xl)BHfKo+)Us)v zJ`ED={}>8lkX+1lL%Xj0O{ETc%{dOEvHtwj$>jUZOmS{qHuLttv!TI`;t2GYO;Ntl zs}3?B44Z^uXIHh>vuB8&=goT;k-tuWC{;aehGeVQ<_YvNOFUhBP|Xd3N;rg+QjtIp zUfRWbM)CFMwnXl0eevj zmrLw$M#Y>8Z*c3hU0PIAL{yV-FvoUEX_w6yN#Paeut@fn(KBB)`s z`T9FCZps+=5xeqlIog~aOLwG|_Ph6eJ3wniu}&8X9ROo79x=ma=!u6 zg;)$mnsEP&eLGoS%)M-j>r+RQD?i2fQft2yZ*<89&e8zMn`LONs&1)x{r9>h1dOJv z49Sc*?mn~p5Kyw*?gwX{sh!GlD8}*iUY(fwTTx4q^Zu73gogukj<(s#kTG<; zXL9C@>_2roXcvV0+$udc0uI>LR(N3^Y@=Z+TupG}TS8e_(NNn)#h6eA#+q19Q#pO8 zYH1&0ia;*G`4}yY{L+qr$kj)Yzq8eIy`nO}(Nmrh9@@!RU(t%7DpfR z55VOJIh`|wX1CXhh$T-Y%r^<_!Of(Y(rCNMJn$q|`}jBXn|X_8VU4>J>r-KzGy^(>Vl6z1VUkfS$n z#jrm3AV`p_{ZfvzCvnodv4ne#~D3BS;sU7vx*ql@XJ}mIV6XH9MG9K$!dJmeep;NJNP8e(HAv`DFcjv z9)P*|@KitW`m6aBo}idpb@##w-#=@wdC&%Uec#m0&FV*UqTjc`pogF2M5`6bPZv3T z0QfI=zoF#>9?X^*IyxKhoaDxYlG(`rHj?uX{#8YYxj^Eh-iWr?>&N1~r{(YxQ!6r_ zNqCfcbICq=T*yl-!?v-#i^J~#6Lj69zIXzkL=X< zsoeD!Uw#n{{UkD2jGFeqGA{o9@qc^Kb%nrfhF&8qy?E1n)WJ0_*;7J<>FSyeh{=tD}Zf12|qKFM5JTH5BXB(0{= z^5UJ{m_;LjDGjohmPPSeXW!O)LiSy>*Ugp^)-N%U2Yl08a&(i7-G(Ame2>pyi#$&< z@@rjdisWRV(qjQkXAsD;dXVoMYlM?JovUnQA0Vtvx znlP({ujw09ZHhXqI`>XPiu~@37tB6-=m7>PU+FSGZ(JUwSlKA`Q5ePU#weQD?vXrE1EPndX zUx#Tn*J}IYXOh2W?T@Bx8{Y6RpqjcvA3M@K{N$!PdWKByriGYI1Yvv8rVpFDCp0r3 z7*}4cOdDSV0+9p0v1vyV8js(|Hfye-9`-aGiN7DT*1ia{u9d zMdNtsEqvL>Kf+pcza7~wq?D3;?&3XJ1`B&X#&CZvPuTg1|MYy&n$6ieAcn4KunZ5ZK3mf8RRn_b1QCmd!V@Wd5EX96Xr_@_+8&3~CHB z#Rb)blDwGU=6Pfy(k5zg%5a8ef{0bStnl!BkQCX^kZ2!9EjKbAcVw4$iHpz*blHqX z%Dg(sfK(n-I( zz&n{~ikjaEdR4C)&oqp_-7pQM`RthlHw>zc zai)-6__o7R)zW|Kes@zr-}x$f7d;Wo7K=B~Y!2jfNNElf)#3^Y!0wDA)jr!o0SVKo zBuNWTA_0sGVWFa8A}?rGh0n^CV$SXw(>WzHBjBuG6+J&t@vAcadG1md%6kFMkRfwC zV`F8th+O#_)>?{#eraqEjJDWv$&zD1OR9OtQ1OTZ(3Pqd^6r zVhPEw&jwOkE%17V7G9d6c>3cbn$<8@qd)Otb=EAm>}yCiK~M&45@+tt42I>N7ePrP zdIH$0h+%l-twC10!%B%HHt|;4%dsV<7xL(b8rPg5uixmZ-DJ8JS5aSS89@vnm*>{b zIxZUfIwO@5B-y#GF;_@Do#W?NZ4Yr(6UV>N3cq9X)$uJ|$)c7s zl0AHRtpCD<(CA{{FJ_5%%r{pZ%jL7-C@Pg~q+F&-OJLnXx{1E{M zt2GDK;#~#m0ye#2bP$76c02^&hS%##V*d>*sT-^(XkF&YZ0QUeWXbc3#s^PaKF0fI zIV_+Y-u)E<4mnG-Db;oE*Z5CllnjQy?Bnp&92s`ByZU1xG9Rx2Jsi)ZClfYzHTO1V zh*{#UWKgy3M;bX!-jKwioMzD_82|9AFdI=+dy+Dt{&?7}3GrvO2gv zG0?WV=4DY{fKm;x-PdKZaP=$*xy0}sylRfG(6k(sPkh@lhcZ$Fk-5k(2lBBiq94UP zt?xDD^nMYViKBn9d+~dij>Grwwe>1Pz{;qWHezv+z@z&8-c|PWOPD*uLcH<@e|W6H zkUlMRVyST^M=)E3Ea&KJUH-FiD@0L|i)X!nN-WeUDvw%}>vbIzvvGqQkj)c;@9h|2 zC|+CNrmkevmOHF^aL@hpor)NL}Y8 zK;K+i^sN9E;=+mc}&A6sczbH)u*W zsx%pymlI0yzrWspK2P%5$#0T(>yIYNMExX@`%yQ4MI+eLz)n@7_XEY-QEL#em8+vF zvbp}HbGB<|dW#NMScK**4Vk^{^0DRQL9=(AJEgdX&$n<6(#SRo z!JFbCRrZ-5O~ks6b3UG+jbEp*n^P6#33@OD!EUsY{Do3|cthZpS_g?lXoATS#wak7 zcb?ZaAtK2_X?F+O$TAJBHMfRavDf9RhAl*CSNTuOGmUGJ?TVxtKLA}6sqR~^gpKa> zYs7-~3m(V+g8FT*3KaGxvZ8?tb!6>kMCK2JvYMAsO(7Jmif30dQZKnWGy@mhQ*wq}?9^TRSz4!CgI@d_S*JX%}nERR8h7atP+gx9GF}u+h$A>*A zeP$|T(vt_kFJzWOXsU1Z$V zy)X$EJxr%@z3>74Uk^Co3`6YR*89es?t7*wV~^NdIgDl|5+95qIk9jMFdfDzM>lGq zwT17z>tCn&JeTPawq$=s`KfPrA3%Sbzv`1i?$`r0scL1VCvV%zJd2qB$`wO#<4E zYLh`2{x5D%w}`oSN=kftBWX0tN0XSdSla(I$nT9RQs_EN;bEj(cj2@G3?m|qqVwE) zD4POf4`VIm4kyTxlF|ttqvLtTNubwZE^VVQ9`gJ0e183JkJGq*!Kh9c7Rs(V9c;8P z@K7C`<=5A8_#6Q$?+&IzjL1&SJ+p#nbJdJR?_xwVIRg+AB*UFp3&$|li(HX?e8ouq zd@!%{Z`gGO)8ux-R!IcCyb$>0V`oX{gyOxa9u@oio{zMvcWx3>=&aHdx;wU-(4Me~oCt5f zQH^%(%b-&CJ&x?Z=;DEhc9{KTOfQPai%wtWP*c0YqtS2Y6m%l9dc6ky&bxmoP}W2! zrh1TeaznBv(Sjt4nj zs`x%p`9c=fIp<`65f0B=gk&M#=b%4*&5S8%NbaYcVONb;5)(< zt@sAN@y#8}W#T;fG&Ea7uq47eh7_x=b%$#WEc(pkMewzWsDW~GWY%F<8T9^T$NkKH zF?=%f;1b!qQU_x~pO}(1qNS4IiMc7FShQ*^dgYVq+w$w_Ca5Vbl|YB2(xu6XwIPS^ zf`2H7L}HsD-$ngU5Rok;~#DZ~5iNh$m7 zNoEC-$m`#h(8#yByfrqvj!aY$qY3?XM{k(Uu;SipKp)DNw%O*a}w z`5XTMW}EDMZfRcRQa^vGxyVhmnKGpmd*G|8eYMG;!A>7y)bVnN!3Me5RME||C3lK` zy14#OjYp!447xw}RKfD{9p=_f{})jow`_&==Xj<_Kk4lKBO*XLA+Za(%J+-ULCx4Pk@kw^sZ~*z*bIanPUfFZrRR7naNor(luA8&p|&NuDfM*jXDQ;F0Keb^kB z?Kuet>&k87CAt#WpLA}ELJWn3)EQ8v&7=JdV-dNJE8U}I=iMEQm00ec5FXzDJap2l zn6s^slhiGd!B}`AM1(HFTY3i-qM4S$UhJQSNx%(!N;QDlH*+-~2=ioQUFAvfm0X15 z-!I>bE!1lGc~5><&iWsqf?+R1h`!^qI;f2L)*rsyLeisg64#u-Qyq9S2y{7L?YqPsXA-iRmB>WO2nYQ zC4Vipjpu>-*??AhFM;HD5avLz&E^67$qn>HWE=TGE_DXe>*{it+k)2m)JarKh0!hl zzGL~x<~lLOuuD;`pV!#xA5b&xQLp29Wxh}{@mDo>Rl|bbwZAisXai8;`MkF9Zy4~z zSO$Dgq!P$Af$k{#0=qS!MC)s$yAo0CO$*-dNRb`Fzi|G(pK4q}(u{rsQF9WNKfo_4 zm@hG|H}h|3MY;wst;Mykv}s*wfDEdhe-ZI~zZQFkOz3WmRTLpqq*ok^3jOh_;M-RQ zmojVinyJ6aR+43&u~2W*uT>xg8txsQ!71xc&#fbb%oBSq`sZZUojuk(#t$X14U>y^2DU`|Dp_!h|Ho7pXf^FT^fN?!>A@ zh#cZy_8A-K3eiO$kKn=-D9W8`n^w%Y|MIn64OeO7J4^1(Od#FfS_B0DILRT5UmLsc z;=>;l_Hy2dFlPfTe4x{SsGcF60nPEAUx`Nb0{k5aWvG&4Dv(w*lY&?gq9? zHv$&6;Z(fG1ZnqYO3H82%u^%qPoZldIF%UJA!Y8tUpQFvMJmOfH;i+*I-@S-6IR=p z76l?hJB*LqD(+&qLv30+_DPaj4yV#+#xMmhUwzzPCHE-~ap5y`(eg3(mJZ~_ZmgRk z>|6FLTAf%r2?FWYv=D-n0FDUpr7PF@w7IrMZgg*}LSmYOPL2)Hf>@Q8*pQP3HQTFJ z^c`7Jx|8=swOJw-=oKKRWBeAeYfc25njcm~t@p+r6y|lVpO?sbenzM}YuM5! z<$Q!w76)JnQhZ3cTskg*7!(*CNKW7{;Y4%aj()}RblQbJP|wRiNF>!g5%**w=)zp< z4LQZOY@(=j-B?SYL$4cRojT2z`e+LKfO|vnZS_w_f37G71Hbq66Xw6%q!&DER+t^Ie=lXHNCl+b~7)*DYG!Fdw+hx2t&n`-cm_6;TJ*Lh}#MGx4>0J}U`#!i1z* zcZ){vTkSemt%E=l!e-^_aicD>AG=-QvU#i%RD`NUJv|R4mHi`UDYN?e;Iza z-Ov|ZK82CxB4xVD`C!%swhT*$Dfr}|=J)X{=hAd$-5>@mhc;Ed><$OJz1@GI*+ETu4U|C@aJ*=DxGtGNhLHUFRzwNiD zR7ILwS@=TcTx(!RSiyL+@BW61z;0NY;eFhkwqYHUv>a3PD=OiDFKs|A!A;oB=An~<*j{n#l(4s!g5GCnD-MO(?o#E`*o&5ZM#^I# zE|jdj@7p|e->|P!7`*L6Htubt20*_>TSGp^@$;?d@tf}k{X=gM`~IuS_+M>h<1Qf} z{z*=!lM*<6W6^7YwONnt^Mn)C8qJ!gK`FmK)Z_HStwmA^Wz2h>a7IcGN6Lj54-+Y# zQoan(fe4)XEPi%gXF*U)Yx#kN&Ci!8ef_$@NG@kB^d|Zj{=rYYhB}%$5=2gHIgmOH z7!b>U$dV$4?V-_yrW2LkqwUf#s@pRsw4hs&f8nALbM|+J+Pp==={-unL5_?;Ax?WhY&8gwWUHnhuDmVhqbL`-wBl`+4_QaW5$tkU=etyRNeN}B&0iM5e4lOS+G@M!yU2tbS#MOm@U`5LIhBqEtm*i+kOl>M{V}!1 z`snN^CLqB^@?v-k)Fu=VpqR@tOc@IU(c@7*0?xTIqfs`VWubQ%jU);-;6ENf4zwqN z;qOPIhuK^*6Qz6;1-_u-)IQ+Wlt=`ta<`jM5fXA==!$2qpK@YG5qA*!H)d;9Sw^3I?2iWGR#{+|Xz?dSw<*KN5s zjODFG?vuTG&StkOh7cw^HR+v)-dFeVkr{rau#?CIMUZBk$Ws#h@QxhTK%^8INprIJ zqv!#FBfHN?X!vV^ZEkF_lm!@#pSe?QFwwIll>aAZtVt_!zmPZaa@Jkcc&hWC60`rI5j7R$ZXl{7&9vIb_ylmPx*>h@htCM7Ta(}>$)Z< zd5AJ+;iten5g({SI1ciTZZwrnH$&YY6|_LxshXHo*5Df8SH(Qu2!0OCyvOA#jL56Z zQF*fBd0%-Ps-gQWHt-3jIhe^V%cJdsoMC(-cDEo{Lg%HW3ww)#^Eia_L!QX>FaU7} z$TfPKUbC-ij2^jY84dkb6Nh?fEev%1J1F>F#J0%?ilVVCI}ESRWDi>0qu6_|fXC&b8^{fDFIzmzbkv zoy4!##-vO;ikGm=gQ6zs>jlRb3|)q>!hZn6P%;;r?dTsLvDl~4aMck%!7JY|yP1fLkeMk^u zRZZ;(?^fbLaix{|T6`}~NcU7E1bxfNJXK^s^7!-`_90%g@&W$&MDgXkoc@G`;j~&P z_(X&>nB%aP(bV){O9OFY4stiJV!y?0c3h zh)4VR)9!XX^Q3^1={rV6*3KqH{VdNkO?0@9m`$nO`2#1(wjx2irvyfCVhB^J!avx- zE2$Ahl-X!_?wd&RHYAKQIqgC74da-AK8i=bXTDW8eUfd}vA5k4gvK~3qMcW3CZr^J zw$d;(bzYs(`)V`8Ied>Ma!anl%dJ0~7AgBOO&TmE4>ImvGkv%a6MvL8oj$tvOe1}< znyD92T<9IpM{bzTrt_oTlCM1a0qVg?ZYt;|dK%(+Bl&^hv^`(xH30^z7hM@PJ=m5= zx7Q)En~k|Q+7^&*-mA3j0gJQ~z_{pFfE^?f|JD?Z(&mjOU}TJr1do|Uw2bY`dkSLH zZz)AgdBeDK4zGT$(EfBi=qUbs#KS?kWU*NFCrcfh_u=Z;EM94-*E?5wsWPT8UWo?8 zd8T>@>({=Y^;eQgjU#AkoF*xa-$YHMz-SZwH)Q`WMJ)QOfZ+a^IO+ zJ3J}DNKO3EYqh#G?L9t-`P{uxS?4UmG2{fVY#s65HPmjYiySIG9vW$jLrtJU93)2=m6=G6w}EV{UC}ThTkHJ2%H(^kqfw%3zdz_m!$K zPafCK*jrAXE#K~PG?p*PGj;>?kFZ02%M)Jz#=Y)VkY07%!sgiKOU|!ISZ>LbZpdUL zv2q(?b&Qe9D{yY{GsN0Mvq3t&D(@`qGQwxO#fRI~W)lAZ9>n|6MQsvSnIC9~dX|$f z2U#Moi@znwhp~M8mGRCfU4P}JFN^KB(HvQEoUB$c!*VL73a;k82Y z^zD%!j;ryv`&PTwxekteO|~;KW&5PDk0evt)QuL0M?1N#9VchVf%ce5hdF2aI$@jZ8a8k*EzRt)d;&{ePyah)iJk5AjD7Rj~CY5@gq^N&aX zZ|1x&o!;vXOn6mb3m%~6S4IMK8tU`Kf(b?R#yR9w)~z}5iQf}>mi{iNKb;vaDf4`6 zHLn4zln51{DI5}pWgJna_a`@fwtfTQ-__@hJBK0|Kugk)(lLHufa*z!zJ{ zGDR?yvY;tI)Pe|iK&*7AoIRD=X()PLjs&TR2IdMxd=Px{KXk9D>X&S*)aGIiH)LY> z8xyZ{b+70N8hb}<5PBU7m6d19NKJXu28{eQ38xC*H^4y92VsS_nmY@zVD8rCz;84WMd^)ldt21Ev`z|0!?E55w-Chk`QSQp zPGE|hnWtaNZka3N#>2M9HU3lBUA%#l)kFux{$0Y@J=V3e%7rtt-xWM$T(D6!XbKHl=H)eldNSMpBaeavdoJj+c5Rqy%k_z;=Vc;_{> zmia#bi*b;FcTg<{jmwU0-GfIGhLKhM{|nwgA-`cGW8`~itZ7jllmm#G1(VBq1C!lA zc=6vrIP>2?c<2uu1H#TI9mnH9klT+qmYPDAwHzfW86@}$4VkWubt+&k>vBYUWG0(0 zI{e3jX>9;xsa?Ph`UtcOT0d6s$kST6S9GdShJz9^+d|#JQ2}cMAt@Oo1KHjxn9H%N zqa7+^;rQ>!a3?wIa8!GRLFI0fdT-JiBUt|cW#{yDsV(fG?ydJ{sVMa`g0vu~*H4Hv z97p9%(AEC{PFfYLdOy6}rf8|daBVO&X0N}=w5A)OQl?LS8s$$<|rSwQ<9)ONukkGM&2%VO+ykM zq_SLK2_#2aWyNu#Lblt83LRZe?QL??aC+S8(&Cv`IMQ+yI}JuKpsdkMB#HzSXe>my zO@!cb9Jg6e@uDI+ zI#hRK_YASo9wh{5?LkxV0lO&!g5?pZisYKCL^1Sn) znTeAlD~>#u8$Ib!6c+w{mgvORBT60ax9fi}!Jv3vo|2rr#F&nEwyv=rFh$))hPb}Hs2a5w5x5x5h6@Cn;nFp;Q1a@6*$y6gFxW3-#~b%2v&P&Dx{}x zyl5Pf26(6eQj^_4xbuny#zEo8pgHd}0INIXVu0jw6aym}IG|i(yifwH?TQBR5^yLa zX}6qL3vH=KxD3!`?RLFQ71Yaz8bod#!-^aO-f={2u^6)I2F!g~J_H#l7)+Mk1grOw zHmWII^c{}RAJawY$?PcXsB9Gh=1JP3wU=jUwiUq5vQw%XW#{+@xDVEdYBd5QAMSRj zE%>Rj;rFXFQ(n~fmxetswmX_gY=5^D6kV4hLb#rN*AL=ZG!g81&=Iu*vp}d8Xb=R@ zC8Y>STZ>9l%#v|IMRV=kSQC-s+f@>`aJQ?~Or}qn#YIT21BJ$Og=2k=)EWCnNEiD{ zVl3n}afEhL5>du6;7;S&Lk!e-`YTrYwx3A0++6yxm+Q8(+>_3T%tGihQOO{`G9*-= zyrQLhhxMYf(Y{a7kV2+vWv{$97$@RXXfm<-4APBnB4p?MIKoH3P){eJeH`;DX_U!n zc-dj>kbH>VgG`yw4xsB*=vQcJYd)!PDJLFO6=ZZ3sUxyfZZQ_VNlJ6M?#bmr&U!%B zc5a3`Emuf0w7Xi4YxnfnqO$AiYB*R6<5?hOKolr)-xM9+M_KJxJv$QTF4bob%V7_e zbf2Ix2XJFv`>Si3PQTLT5> zRGq(fx`@zzSbZSd-ROCkuVQ(p4m%W5GeO9DGWkd7y%}z>rV0V{ zXKoDy1b`B?rM?nz`^Y%vm27-!qhiv!4?)^(?R0jS4@*_L8e_UAjxzji9uRZ& zR+2IeLE>MP5B{H*>mAMQVbDFX`?aD$Q?hJ`JW^q+Ir58xRN=7k}@bSoj0hQimQ{>sZn5|K`NCYr8d$@?BXK?_zmbVPqvm` zC8ZU$pHXCEhz{Bg$I#p>3(y|aQWTi-E$*dmNK?2|d^jME?ZpSvPJngI{{W>e7|x@+ zM2VUr(-so0CfrZj;q3>v6k%64He;iWyOf+7GDKC|K+Y-x7eM|$8VKB8duSp< zj-gyltw>PhQgB5Ee%-5eBM1@IC(~KRI;ltx5BF|8^cl!et+%0@mYRMS7x?hjLVWla zx46|rhe6t8J;86)P*hL&Z=S8e;e)n)v<$fEyMCWY^!PU!3dYL`4YYifzGyJ&)WRBw zw^|}Gj!Iffs{a6X-wJ|+-AjC#knO?V6ZTSgQ|=U2IK1M{R-#PvB=IPIWr252otxK4@x04*`*$8uJ2?x41->#w8<(4$HGMH~wErxXuQ_I6S| zWXgo+vDAKsfr7@V3L@Wei6gl@dU2X`-!Gg2p-h8l@J@6rqkTe2p<3_s&CAbDO_@2b<$5Yte?(- zp;+W7GIotiaUyORBKdSwX`ih%J?E8 zGtV@xEC=050FN94pKS!9Ne3J6K;a~f@_p13$Gu91c-UDO@7#Mu1Xuo&?v~pc)Z8fI zTd%bp3j~zqY6Cbv8QO!S^b4sh4Bg~vtzUMH1`Vo1%!l4kLJNZdAP&>@j@{KqVYvLP z{!bQ_DKffBfTbsxLR3ijR2oL1{{YknpzCs!)qyul^3lw&+FM7xXFm!JDu4c?R*zU( zx;2zfVUaX`Nfa4wj(%R2eNm{Hv+B(<)tQL^rKwH6RFtn70eS2^vq9K3KgyS(T_@GI zde%&J?jSfBjMAQ03GK>81qK2K%HN_bR?$_W^802a#*nngXdtO4KCs;FMR4zwhPBb% zGTby@N3bsFnE|`JnU^Fmt{-26g|!adxYt2byi1&23pilzS3EDJQczqP5%JgS2U#F(z8N zU((kbs#cY^X}0=9?x|V9`d+LzoF4xGZ4ss3h0Rh^F6kNOU6R^d0mPJ*3}6wS&8Rni zg8FgQ-3044H8z}-rNePaZsP%G=%urr(xKR)xeDeD2h&#iUa&o>^cJ^)bV95ex-o_p zwGX3l)P*z^+<1XW9fpIqI^aSJgy0V94SAUARnhs98)@kek;1jKlBV12PZl%dMRA{% z-^spb$`4Ih;o5aWc4>A;oOPDoX-baw7y$8bZjv#v!R^|E#Tu97H`4x#jn=z!vRq?G zA+;(BmbK)afJ)GlpDMU8?HvPrfmIMQd+A2#>k%AMTve&HVNU>Z1Eg#RfZn1#w+42X z({N+i;3_02u^UTFu-+1u&;|eiP->dvr7KOgI-@|8ACjSjA+w$6tp`nM8M?-Q&KF6_ zWiBfvAmv`p>ZV(cw&`1=E`%-dZ5OM+cI_*Lm=>m&*2|FMjuVvql#)lRtO|(Gx<<0O z0&7=h*lnin2?}+#_e)P>DoF6n2AWPN7*umXGOx@CuxeN%% zKWXKJ4~-Q#wCTvmrDFhxwVv1SC0V<$x9RcH@Az7e2TTm>H`U60Cs1FdJ zL4L{w#z!g%8oJzLCxMC58&7f_AW(HlmOC#=7V~nmw3zoqCoVJCv)(GBok-ETiYv}A zyIYCjJC)%8dr1`xnk)^llk<(@1Z8&@y2X2unku@Z>DJE@?$16)G_@Z9`&2~(gv~u4 zCA(AapYt}$*IUcL5Bl(G@QwZGDH+K?8&JLg-vWzwdu{fqoC^-A? zD6Qw#v)-kX`O#3Q8U@I0K#G(wb3ls0IRgTL5Dw=6&>*hNiU$Slfk4uvjn8cZQlOP_ zAdq{$(=r=agIvR!xZtg(vJ(Y5pTX#-duZ^x|o0JN%l}296PnUf#HHlGV z!gWl_Wo@V&dcvZi3h|&hpjpYFAe?4_vL{A@(RxdvAa-_|4r$UCN>)o$Ck*?uaob2B zGKFzC;qKuS9)7sr^@m9FSew!2m7acOTA%c$l2our8OphC2h|+tF>^Xu=}yk_n7S@Z zR}yffNQMwW_KE<{PWsuQ!ep@E?o&hbpvB(0SJk&?5$O}w(o%E4)=&?+gD)$kZEh`= z@#qSUhMePV=NKg~5y<6R9C%P{myJs&QuJ@9lP%X8lJtv*q;NPeqEwXy@D0b-BYt?G z<&Noz-Em2f(N1<*Cx03Yg|@@z&COG}A3Qcgt%N%&VS zb#Q~H+EkM4$3SgS*i+6VfxZxrSzC5cW@dB~(mjGHiKndEic8ZL0@pSoA-0f{vBI1x z&z>kcw?zF3BSlYZRBEZeI9|d>3L7Lc#zqLo+de0125Fv+dQQ;NBsZcuQ&pZb%X5WH zLP>ZwwPd8IpUQ(TW9ZLR#=0u$ed%LTT#L&Kkk=YpOXPMajFLPkIx3??b)v)3(Bo_q!Z!+1K~lRwrMRRWlB?O(^j~lJi4f;N%!J| zLh09{IGrNdQ$E2@%qhf|V8I4EP_;gX*_IT_Q>83qaar+<)i#fSD=IPEqQTe9Z>;3CB}% zd#@NP+z$$>p6x9RG6xS6i#S3@GHN3YE);uob?O_lX?4aBRLf*!Aot_Gs=Vvc2Tc7T zU1mK8O~P_JAdr=C?tEx8C>H*iZ%>exC(Ci;zX3m=Wd_+ROu*>=-Y>l{bct@$jRf)f zKU>zyaCapGB%fswq22yg8vg)SH13S~E%beLa3mI&d3HBnD3839?uPT;te+}OuU96q z^j*_Eo208&x#?2Hcub#7Ood3e`RTp+OX^?&oZuHwz8kfgEY>&nCR-Sw&<5u&ui$zgUM;Z zq6l7xx~lIQ^(r74V?J&9~2hzN&0T zm{{f(f}hTUxIOd^2NVFB1;^ZI2wek#@t`@{fyf100YQ?1r*Ahih&JgDc%`yOzJof? zMsvafu2*Y(Cl4mwcyW7iL5PE`wFDQE>lY=XgOU&eQ|=VV=#j40ZjaFBPP1RxEwiU2ULWdlIm;(*}z&`A`0y^+Z77O1MRY|foSMUr%2yJ^cu9)V zdx8+1fS~ku$-7DE3x2cE$AG$XCs9c#X2MJFXtHZ^x!0w>y`h!hbwLO*V1>PHO zM0q?Q1dO(O3U>8Kpy8UELeg zrFg_p9`Z3kvsn5D=?d_&xc?Je5g9b zzoccT2%4(f7V8}BR#U>@_SHnPbf%Ei3NL9Zpp7AW`Xseue&DK#)9+eR`C@;nxj=QU zE#Fr`W7wkRHxwFAA7Utju2V)}JtrsOvnO?3?=u#nhS z7Dn9p3JZGURrGbBV>@uTTN!akPIyq*$*MeW(;v%!S4&gYK8_h`#}wrqYbYIZypLr; zr1V$NF0$!<_|CBDMs>j!x0W1RvRqMmNauu=ZU>D~>>V%C=S000IKGWKJhwwjmZ<%G z!EK%h@RB*H_kPmP-5EYqS2*c|<$~qm=Cs>(cs2_<);R7rW{Z+uPv2>PGT0+{sx*{4U=Z_Bh$XoQimpgdbp0C(Kb9Qe>XWdIHh0SOq!XdKxn z%1-Gt1wakcwF4A0=Ro8HEMZ$HP##{X_ZkS*&Vb;KMuF`*>njd6>X4-F6qAA|Gx4>? zplPJBCR%2)Kf;!P>T&JdQ5KEoMqLrZbFjH@eDit=@{ej2`WgnOO53g@io>X($XVpW z=csf2+aG-uin!_9<*?#yvG&K_C?zMzji@oD3I_!{`A|<91j$PXjTy%jc?6`BL9^Ss zIn;8H9+w#qS>{}nu-N$TL1b98MWM`Ibt0vyJlcldeVj&!FKb;TZuK&#QjnzHANB*4Mh~F zl;VJc;Xs1UXc18q3ya2qXME;?yna*&xj+K*w$uQu8U&XIeFDyYu4pbowB6^P7*^>o z6U%|3Xf`WfMS9}n>kCSnN$qt(IQDa*WwFKRK6@$aFSp20QQAtmdsIa;23ZI;?u>M6 znUk&zmqFbOEUbHw59d;AgvYk0J-=6xpEli~#pJ^xISo7s3aK}&Sq1cQq z+jf|@TcNZvdd!t5_6*>Au}sYMS|V$lL(dw+q`gtn-l$HN)6S*YvYtyp#d@*gq?~0~ z<>k!QzG|*cDcU^Uq+7G1rfR#r&e7UFKjGAEJqT%ziDSl*HTSPNj#_x7TojN4Deh<- zl7L(FxYYpwLEXhJPbCsIFzC}5_`K%XE>#YN0=`? z2Wrhjc7E-2)NsR&oa}`hczIS6TrtXw50iEFRL%)niW5u&y3mCwSn@dh>CXwx4yU7j zPQ4x4Cb1&w#H-m(?j}pUTUZ;D!i*n{PHB|pdY4FFx-_?0S<<(C4GI;lgak6}y|R_F zp8d%^`ObU|T*;YaF1-=Aa*A(#9^P5Jz);Us#skgx5!>)JluIa_x!kBypvYWlaol0n z&J>3dQlNZ6s-%^0jpzt-J5W21Y6KTRi>JPV6|A2c0GC`r3PRMC4n!PKZFXLf^#!?s z25OqQL*!VNo-vOCcN7_6AEW(Uw0D-|w8F05AGmF_jSmOx;%#b~_9-QclNHw9l zG1IzLruY{U0t@?YUvP1{f1OIFRn9kWCk(}9l$@Rb3GNuLN5!?VO{#SE_sm9LHSm`M zxjNgNOHmUTnTli9XL)gNl;)^it42rD%sIm4D50<#DPV==HW5u^|!B9?av-B?AMD5(dM&0*LXZgG23zkfl1c8LT;coZ%#@ zN`gv|m3NGRx1B-GHQnavf4qyW)K~Aw$DsL52|YrQyXUCBfT#o$}0t`UU1zb@ zPZ~Yqpu{NP<1|tVpdoY;C1!!ebPVEBPTaew4-w};j!xs90xQqf=Rit<8_+PQr*6sw zm4iU=qlyBCTy;Pp1e4uCowVrvM{^*-wmhNtif}#VgL1XnZkk^Tyz3b7TKh}|_4ys# z8;YpN$=2N!rAh1b_oqBQyU1lN1$g415_OG^b0w`Ebd6+MhB~+71QY)N*a50E+x3H7 zJXJbdk6ao%^{hMAqaMyx6{$Mg%`bt}DJmSn8_*#@{E7yY0&_sXq^Qs$p`R)QBsj`~ z+EGf9atNU58at#&lcNncs?Z;b;NeS(87QiXTIZvet%>>G%)d&2;NnWtzC7qOEIQcX zQfer1lR&tQ0wnj)9nAstiNF*HpmKt*pgq z4t0ELb!rPB0F#aqp~aFj`c#>yVdGl5ch^waogV6lFr_veX?~IzNlE3iV5+1=Q@Ubh zZYRmYZ0tR$^|S9hcT!nwsAspDvvZ`K~8{l(yPbqI(LJxg*=XA|fH3H1p0Wc^37nXY{YA-nOFV zM5Cv`gM?NK>7B;&)CZR^q2%*|7oWqMTUCAJ@WLx<8OJg%xUEdol3 zJ0uR$PUO~o8N@Ji`YUq5qK>fDlFIcI)ufh2N@68gA%UN@gT$lktSX$bs&lhJc?WTl zYhGC6WYAFR5~Zo7gy)$D6$~~T0V|NBobUjCu5-BO;kJE-vV@kotG2mtp#&t`)*#~HnGyF> zws=;Bj43C$gH=rnpgeR9DxhIq04ks%WDax(ja2E(THex|az5p0&?kgba=9B5a8 zNub_f!m{f$6MNIH*`*$GW2fkpd&*BLxJ6l=56EpsOiRRhP=@8!2NIGzQ&4KRHuSZX z&Y@_ZIl-~|ttUSdR|2v%e0WL4r%yqSzQ-L;TEC9p3S?7jQMaicax~3bXL4H)B>dMg zz|e4|K|p65JiyyD%Hh|c^wloV!++ZSF>$tov6Rz&d}WUCvDr{>I&Y+1E2oX?hJzjwfh!HxZ@s|pBabS03o%{s`k!QYIbrrq4rIvGY zqdCcFJ-x(s9|{cYE|;J=a9)hMIIo;7>><`xqqD6Z&ous28!eON0@j;&A)*Qa-v)z4 z`AYPU3#ENE8tVl_Jqf2ENGBA7!O$G|( z70aV;MTW_BaCqKqr*fuA$sWVMw9souY7c&Vl73OoTl8I%OzH`xUWS!MZE8D&B}612 zA%X0oy&0%F2I$wN&2eh!PU!6MHWCnesL${-A!yG6)-m;fILPi48#Mlev?{HyF#^`M zWCqpMz1OKq3dY3b;O|6v@iT7L39iO&ON>1o#Wt`Mag>~HC^AD+!kz+emT2#j&DIB2 z=bqads4BBq?w|V=;3dQV0LPXCYW6FUpvPQv92J))Rez;bq)Q- zj{--qP+*rO*HxEYT8Zr^G*eIS8U~dpax@m8UM_aWL(nHa!iM=m$e`F0ucb{uZvAV4 zaU6==WFgPzP7MT&qe0z<1evyXk7CdsXdf|!d?*u|3h4R^wn;HAjRBm-bQNRwP7MYE zTnmH;jCru#VZ`%Gh#=5V2eeQkur{D#m!8T5g%lnjaA-H(57I;SbMs}`dQH9GRY+d= z6?&sHtNK5ObBJ{glV4oI{bjO~l7m6Z-M7qb)iS1B3I`lGIjF3izeZ|%ig@DLD^{{RcJ0+;-$$)*!5-M^yx|%iybWjKXg1hg7fNX$36{to{{RjEDj&q*Ka~bfFGdzrxz}ATH(Q#9 z;V;Wl8wd9r;U4_cD#B__cc%2+(OOSYrO1(FqpC}Z$PJ*30#u#A?pI}DEGK6q=_8_R z{jrs59X)V%_i9+{ID*REZ3lduePrXbRuxj4w4RUTBx^fKdK#slNJtC=g0*&U*g?Ve zRHtbko)QNWZl*%sS#e2jT80C!XVwoS@4${gp3&HGovfG*V%p!)_jvJ;hujdJct&|d zfx~yL{WsoS?irzj}|+4Mk+;EhCI^2UAlDX zBJfC6_t6_y*ox9y1KYa3H94k9nQJfJZj0e=_om?7)XkugtnG)Yphz%ab+w3aur4{pjEN$7%zePG&_IdnW{{X|L+pOIl zYA79LwmWv7XBBSY?JYl8>a8!Lp{=`OF~B%WidI~GUOmIWQX>l# zv04r%S$RgSzT8sQph`OEkg%1i6KdHmDJPQ1<(^p`wT)Rxp3vGZR%tWB<;`kDvQ}22 z^o-$0nJEV~hbtY}^;ljFae4wHv$w9>97p1!eiX(G8U34mv@OD3;X95rDCgPwQ85b< zH2ct`2bo}@Y5_&JS|PRI=Nas&(8Ff*%T?JewQ!89O~FKtu%DuFM4S+L0FjIhx2&R8 zB~syLnGmVRm|EJx08)2O4hm78SvdsN8twNDqlX|QFJs!X+w2<;eM#t&hQ+Dxx^_}# zSnhAba2(~eY_Q2mvdn0k+R_}9(@@&&T!%sc_H+1E!eueDX_y24A*d4i$JmJ3SH$2@ z1YI<&BgGoV^tBE~OV}Rd)KlEo6VM86u;IW?G4w2sFL~asQHzm%y3A2rsn0l1VOhtz zqMdKlH;Ys0lWw>?!qRh;EaTccXg0=m-Io4@#n!fFo;b&*M;s2P#DTG@h^F*s0`AeD zqcsB;29xV4hZVr`2bXOJkF}qFuu3^_`C@}(o2egGR+o+r<9^BvLN$EIGDF4AwX^ED(vPaN z_ZkZFWXGK0mulcxbP()iCoI-|RPzxja%Ew8$=&72Cv|cJuW~0)qje?^3NZqp;J9M^>@7Bb?BEJNYu| zi;de%GR&~DQjvuyc2Qm2zSd5!3;a!uBJ)t4qpb>$vqYbJuaoF;*#!H&uX=QIau1(oq>UB_&^#267&R)svDOMVa=He%qs~PrX>6$T8913yE&!sO^!~u1z7{ zwP(O30OpuR#cb%NBdSS68|BBe7NPYBKg8xJI=+w6w>u+URC^0B*-#u_ku$7W9!VIc zax>HPix&j~iC%^Gl^K);li;NTk98GY9UJMRgFDlwsB;_L|Bm%&cz0-xWIl0C8_fScyWzWyubwe@1J=y=RcEQ~g=Qd$Ibv zc$neV^N;FP{{ZVB<+EEX5OnRim{#T^#%-a$6GXY5(xR=2J@vO4byJ$BGS@ZVNtSIb z2}lwX#sS-4N$urZA~4D$4hL!JTj$MN=}g;xb(oB*GfFsSK0n1-J8k1dnRcBCXOz@+ zHuJ`Fw>Q&py3PpU>z(@ywy&&GJ=<1Mvl(_tY&{_?zadPpfY?e>ml6t<_=5Z^RLdDx za?%14>!?CN7(!Br>PbHR&y7IV?NSqkxfvktPy}=o`#4w6#)xZ{`6^Ev(Fd)Nmi-v0 zc?>!6#Y`w0e3sioZ$ej6qmM`QMV$E(qsz*eFtHK)m90e<((0N@R6Pz*APy{Nfk%*=t-U4ajc%6UYx^^C=8?iCvFhi?ZO6n`QCldL`@V*9)eGl6 z6#^C7HaLTz^{(T{?0uE8Du!8en})8`sZ!#*E$F2u3TX)qVD}IJrd5M_Y>6u3ko%4+ zy=o?kj41ceB7Eo^4k!;5049OJ=7MOlpN!yfhnCuhYEBIYmil{@sx*H|vu;{?^(rfO zR@<6TR!S7%7+JyFm_GUpMv>{CQOmnqts1#8ml8wiO5Z30Be_GCIZ%2}R!q|R_eL(k zscgyg_FLh%&yJ9i!2{Lv9u-x_^iHO~TA*rvjIJ`AXv~VHh7Z~srAhc2i1S^|GFq7H zQWUPSDG#dj8a!tJp3f&`w`zSFodcdqGnhL=xQJ~s%y^-f$OxJ-+ zX!W>{Z3fom@G`Egr#WrTfk<%3G49Pr3VnxCo-Q9%MDhwf+faKupiN_VX57OUSr*H4 z2tYhWSV-mDlSOv>Zn4t$i?cdPCrEaOFB1}_p@?n=Unoe*h~@{jh~*mo*AmOCCqRb$ zx40AwtCGGosbplF_i~`Xa}+}sPAH&;mgm(Opvzk{jsEbki+!p?++C?`3J6L!lpHiaFM>KIFe zdDIwdpQw5P52^juIzdbZu+Q2q*|M3WwLXA5%~b+v=ISR$gJ4iIojpm!Amf~XG{ zjQ}d3Aom&rngkDx0pg$mR1XG#=f;6-XaT_NpxRfgH0(e9pGU$-9F-+%bo;gyQ3Ks3 z>V6W`84STk-Hggx_<@>=!ppthF010WT;x2Wd9BiDJsHs~eKP{X8Ou8@8A@$Fl=6Ts zEq%N5#(4@3lH888WEu%lcIQAxvVdfc+61nA=ob+j=o(i*!lTZCg~QoF=dYARXCBlL zq?9YFO9@XFHpo#Y^R0DtYpbh>Ss5bs^FbV4+h+tFk1<%w7;g4yZ`r1?I?E~MBq2p8 zalcmp@=taS>LZsdiYu*J^?sA-3JVska|Sz800D_=2?^p?EGYd&5HrKe@~fP$R<)~o zHPluHj||n$gr#Ak#8r+C;;@2F-T(vMD&UxE4qKaOxLjIZT2t#Q(cf=?c%CEf+KGm_ zs;-aQprtSvE>4u7%vQ9$FuZ%V-MWt zu&GjqTQ$<6TzS*a9l&xGoKuLzICm~pdbzHyvgVqjq|4T+yC%h>VLdiervVLRTqC_o zDKE!TR6mQ5_S8jXKy7aIuEZtfC)90ejHHp_ zzQfy-KuwxEX-lreCH1G&WpS7xUAquZ3VpN{-?Q~(b@uMD#g9>w0VH{h6T(0J-k^!g zb8%7>NDt)5_TPjjNlIZQ5dU&ol|IH z?AtP01Jz_lbt(tLi+p>kVJ1A7DLQqTB~IBKImZ!-@xz0>1F%=X_)#(D!Z??hS!w3f zg{0u6Lj+L42MDMF!d-cgTjEeBfa5q(+XukzJ+#8*xarMTf71^M7D=xmC0q(f8AGZa zr6-WwZ|B`v(b9-6eh zc(`%(4C9M|1xJ4YLB{XDDR>3gv217z7i6UmzbS_%cG2*%&?a z9h;$^kiWDR{{ZaW)y!OArwA%PwQPQ(gD=trh2*~*S6fqu;QEr1o^CLEeA0LFp!3H_ zv0HCZNRr6)C1OCwFI%6-G#cb}dZj(o8T$*NO+6g*7*O)v?IU`W!L&J>Jg4y32`p!A1J zx_)b#-EP8&>&8cmQL zb7m3>y_u>Uy=S6qey*9)h;eD;^u=+XJXBUH=hA+yoRZsjA4O>Y0EZnR1K~leNx4my zvgD}?aYwaBG%&2CsGf8Ki1*M;#FyAuahFtuj!H?v6dA^~mBC01T^nZL$k?UynL@lAw!r@ z!Jv$P;(?_0 z&_<;l=pkBX%MEUygeFW;tAjA-UpZj@;()= zVXk8gvn)3FNK-)se$hC>vD?Fs0fFaH5NZV>c6lo{^sPxsxRP5?8Q7^n?Zd;eee{)P zIxp$U>!%|(ZN3-p76$U)t?NsMJFR?bC9p zquKUj^uI>ECiI~(6ZH&-Y__Kr#HXeb9a?_^59V+wGS`@gp)@Xu)7mQBEgC(Rl2wr+ zQ<9P$kfYj389p6@va#yIXDyk8Exs0Q%39M=;Vf?Nq$1cTy4m4 zmQkM4LBT!4J=FLBBPb56L3Q`OvYLL@+LVA#)=>xCGw!MIQ=GQ4&(ggGGT&bO4_WfG zXJAQ5Bo5)>*+C2OV>Ei#pr%}hf%P7?JI0a75(g0o9fJUEfmBnKY-J7lTdgwVr(~gV zBq2lDl0rw7L}K2E!$~f>wjFU<2u5VKt%o6#upVC;bQEq(211=&q%CVFgb9xz_5c7l z01o6Z!@D$gKIVLOnVb%`DzbMqAukB-v)b)|eE3B#M?i6riyKJO4JM5{iwR>G3Y zttp36f*eUn90)nW0Pj1GXz;D7uA~IxjU7se-9T)VZGbz;*zNa?6_nH3!%%5Q!tECo zwt_g8(*0(Fd5$1{(H-0DYGE7OdYeG>g4!6?HsNZy zGM}7P!jqm9r8vnr^AsGLPW00fT)D4ni(6@iu#`k^=AE!nh9lEx)at9DowuW z65A<%RZ)~;kbF{oQ|_SIdOXteEUrvkolNcSD{BaZT|8-TZ&^tso%hDn8@r9R`gk8< zw-QDOT0(*M&}$OQ3n_bC;aV7ZPs3*BerOL-}0D$2M= ziJp{RS8lLxaT8!(QhlxLF!f1?lSV_qw(L}2h1B)~RGzgyR1)2rLiUq}0`U!OwaLr~x>j8K7xBlo6aV_31*LP#mktx{54lThin*A|xBjDw#F zh|(lSm1kID;uh>g?3cjG+aHU(c=M?+#%b~kvXtee_QQp2xWa;5XOiDyo)P9~i^XSrta+CH?h^rGTv#Q0F(brBb zrHhnNOzUB^r6tv?_^xU+MY4F>{RI)x!=?;zlecVj+u zz4HrSw;|+9l^y0^ zZ1FcCEdZ^z97B@5zWSItGLGF{aynVQKtz@j07Rz21T3#5YsUEioOV+)9arFnR+TGd z60DL%!aB=~ZL|AA608-FHsqn*_I;T`pta~tQ+Pp>Y_iL7w5aeGTP>m0ee$K_BO`AD zXfsZ9SsudBo2#{Ts?3G%uC?H$tH@*yTVQ9EV>_bNhoxSOG&F$%)v9i{haCDo0G4zZRY z*sbe(1O+b#fjQ(e-{3c(?)P7j>h!*gXHoQxt?im^xsD;06DuKZxUvFLl>KEL@^Cg8 zCaC6GL!*nvp6OAo4||T2F4=bsz6R-95TLFgds3u~gWPHlaa}q^)Hf#*+t&rO%XkTC zuo935E?nr3C~3D%mtW0_PYG&spbri_{1;s#eC=N$`0Lbs4hl?Ge zfN^B+K#B37Sv~XtD-;>MF{&;aVsd3daYdkVw)6Q>9+S}8_TQ=DA;w-vdl^!5MQ-ur zhXx7Wg3SQWWdNWGppAT}F!#0#(TZ`LZ$YA5E#xSWDkELE+M0bTO4|e;V2X(6*PSym z)?Hn(N-^BeSsb5LS?y18sW6inYEf+s!*$6K-CBVlIEAMQLGcx?VquKzzrZn^0tljI z8r0)XHz^rQ%$_D)N{Uw6haw25u8Yy9P6)mOqq;d0y8TqzE!G$!pa!(2tyoSN%p#H4Oj zvBF0@XU>Eh8dPb|CLNu2aJK1c_s=Kl zx5b|<;2!EC`%(%LsCMGxQq}6ai^5hBle$sHa8-`nZJzoINRKMq3lULsMCAfjg{xJ@1e$RSQLFJ@woA5gcMaU1=m9f9AoIXLd1iKzEDE-Fa=(7PpS z3hGweWhK%FH58AkHXOn3p@R%tf*ka@pFD@0Acl+>>JE|3BLI+|e9y+61PY5OM9iG@ zwv={DE3~vyqu7SV=iBY7itaX&RU+V)BBTr@w=Be=Ir}Lj?xWx`Xfe-5VfC&|rs2m! zK}03%Tfqn&!ycPbBstc)%mxq%2uX_iK_7Wsl0Q!Ro~8;mmb=_k3~O5f zv>c@IINDX?jkflhM&=e|XxQ+P*Xn83%To#Uij^%uli&|OeK0D<)x8Z{CZ+@{w2Q2l zJzlAxpw@fJjsW-6*cZA?ZqQthrF6}r-0=$Q3%PN?I}ZpWImkH8Q!Qsvbcw5HS!Bk$ zU8T1BoNHoSRrPI@;1r{UI}P#`%ZIHI9w9pbxIva z(G1{lZSm(s%VDyxnD9`Ml2oM?5_cyxvYw3T=*cFsWOyVUs5mFeKH&wsj40ZROH7pw zw)o>rW!?nwAFP6Kapgh8t`~bn${bi4e$3nzvm>k^lH-mH40^}7gS7{9(;YJDB%Nb; z+wc3ZYrCnr+P0Q-Rm#-fRx`D+)waE?wvl?Pv05uv+uYi9zbD`SbL8FUIP&hsbzeA7 z?ouDCsSFFSDoN%;2{soh=4UDVQ@VMyeN2lfn|r$3$Vi9@AImKS=J-WbvN|kcRnWEy z&L_5j0Penb^9#YZT zFQbJZxmLIheQz#R$U3G9!R^=49qFlI+)q~wAlDi{`^pPKZVEW=(CSe|;!=RjN2e;` z6EoA{IrP#w7#;U(tXh~+f(3~MaZcr5B;tG@=RE#Q-4-9`ALx26rF&0obdp{Fj&td7AbfL_uNtb2bnvbjp^8WXTqjLnk|OX%u`pNi zdsN8_7=n`{E!&D317WI0oMgg459wokWHet0(GGZQeI%UUd8s4+L-kN`d$)<_3YcE0 zUKj6cpF{lYcGp@k_QF4zckMwsj+{xyLmFV8LEmH%Py8D<$xBhU1iUl-iM1U^XJr$} z%OF){bx2L>6zZ)#wFVkU532z2gs+|Dxd66waG?Q^x59a?mxR3HhF3fEvLKBH$j`6> z+X~R}E9yLJzutg@ia(vdiZu5b!Hs$eOgV#GKttd72Ik@pj77-r@^I49htfDiCnNZ* z*{whgnxSDTWW)^{F!XkiB1U*nW2D3fyW>q%TA(N0XpdfwmB0#)PP8oDkhfOwK(PMr z(tnu|Bb4gu)F!xSsAJnlIkoK7gXRlA+P&XF`#Ma4CSl9d^78_ogGpO6P$Rm0O($7U zS!rJdUWG*|x+L9hCM%Z&mNSv(h|+%Onh>{yloSiR-6ib;OAmV*7{tM=ku_U}&TMSu`oHI8M0?Ywm6BRzxT+IP zlaaJYcvf=l_R*GTNLkGF%N06xrGKYemY|P*k~OkD?+Y zozucMKwJtPI&G)$`w*&Q*dVgrgV-vTS@wbICo$Zqck9m0xI~fAS(GN%;)5 z@=AF}%OqW_|A!Pw_l9dUbp10_s$6KOV@{^iBjxgIW-Tn0CRG~?N7-Q-olZ-5f?c=cNI;G=SHha9&M&>l%A%%dGOS>!`x zJI2iTz8{;MveFro8d1t=?O`M(4-2b^Eo=im7O9+KkCsMF*OB+9)KOBFVs`9s$e&y$ zzY>H=+%uNFaS@Y%10-*d;YZW$x^vB$Zi2=_1U%GOhwIzsyY*xf%p@gnf$}1tiB{vR zXLt7a|By<+>2sX%>SKK9c0}}?n7=jM&tYLFx5DkG<dYw6Wc}y?f(v9{WSFqA`+RM%;A1x8dc80h3(`!`BDVBh@pG^Q|S;5c<>7Hbzb^ zO&>|jpk-lF(pY7+rEC?kt?0{a<$d@j0zAGs7$%3bo%mDBo{?4Ju2Q@|-KS>pRluvF zxX$+kL#6pEOKsvL6d&F_{mnM~q*5L-t8xjmmw?GQQVqnQeVlbPbcwrXU3$nm4vvW$2Sj_fe2Ia=V|J$miB_@gR>PFCMIqfgw6TbKWlLbR= zZjF4pc`GhelI>@v<4qP{SJ3Ws+=a(nwPK!$A49@(t~YxV*Z}Ldo?)dZU;L@aUxzqX z^olhD_urAmNNJIPqKObPYfqS&c?ME-XpbH)b6-dpIx-H-Og?1aca~?C3mv#~4imv= z3x!BGhZMj2QOVl@NGtq~;Pu#TpT$pN;emOlK02GGAbP-|7{gAXWe;I1>nv;qlS zYj^Q4d=r&F({BTc^vL7SeNCj306RvN)g-O9nG5wf;v(bzmrHIqmHP$nMFH?iWTR0j ze=uHn`A5FXH4l%SZuX+8Fp-9^V|GBd6;bqfw&+mt5P>2-rmc)&gXOm4c)n zQLwQq6VP5hfgbEdO*3n`5y0BxMAf?r4N29&OIG&qS`a)FD= zbBXLX1No^Yt#|%4g^AzEqTJ3sSkbD=kM|mp)-@wQ4;YNxOY1u~jOv;$t;$`r?a8?W1 z3hC!$&J{D0C!LHol9Hs2bfX#V&iEcz3a)yg%N*Z!r3^8&2$f^4ZvoT5wOzg;e=m;HM_~JAD0@ z-et?lbhhp{s)q)}_SHvKfp&ZC{ac9o_=-Ya*19uXqh(Cjxo#lZm=YV!3X^{AO2UR% zziKo;xNc{rPltg`@BWMX`^M(YoOnsB-96rfZP$U?kEt7hLQeF|mX0y?%wIiSd|myg(o4K{pVZ%TDrP-&@ALECC{&23 zj5bBjjW7~)P6h`3253pqV{eRmc?)G?6+qEv&z?mUZ93jq{dfn@w< z#*N_@N(D+j^df|uM9)&o8(oV0pp8*k-Y@2>&ga^bW{DJrP9 zRX`I)RDSY;x55dIR7`p`y(@diouA%VzKNmAcQ$V!11z z!Se7jWJQDFDLGlA1v9S5qkTD4*7x3?_LzJNUqQ;QCf8$+IB4RrWIf4ikv98I1slow(LmItsU=Y^ zfv3E!#f6UgSG_@Qa)3x?M9a5rB_Bx%IrGd+9-A|{Bq^2n;DinTB)8rqg!rmx&$y&q zF@sdS? zBUv@Vi=LUaa_w{NyfqcARpQFr`GnecWRGQ|2Q$G5rHCtBlE4x0(1Xqxkks51SrL>H z*aE-7Xd0o^v~b|(1iFpJe#@2`HO6Lc_`tT#=!0h$+&&YGl~vVHCP>b$Q7z*D);%Rz zOkl6|{8Fvt=Lh=s7AKx(I{GAC?}R(RBxWHmZN9)F7KcgQtLR(Z@95Cqx?D3;ClAmn zr}dS9^A^x-f4yFAN|1h-LVc30Ucv)qpiLbeUH`K&98{#C=7q*t{z z8C1u|nuAC>l#7ur&16fh0ZXsTuU6N!sXAU`5ciW7<`IO}4S!VV=$4%=$NWG;SUox~ z)wxA8tJMxiH(1wBj%y>?M$%(~$&^<5gGz@_S`?m|E_=4ln90q{#J@~`i!2Jud-;m& zm0#>?MJF5CTiW!O`dI=Njq-DDyjA#$F>8WPI_MZazii%F2UAu{O~*$snh>Q4fZ5{l zV>x$<<71$U=WW zcrcrTc-@%At9`-5 z!bmoWawu%Kkm+y9eS7>8^yCN;rH4lg)TJMRdX6u%Ta&62GKabZ%IF>TsH!DhCux8V zyoPgO4*^Ed?#&`ID;zZm)r~VcEZ(AP`IbX2{3o@0ZSwWGo?yA|o<~DAE}cJIxH~L( zp~Dl^O~TL8nIGfm<^rO2+1{z(LZ6V;@~x44#wA7zg%VA~!Fd6HGfffgfMsw(>d39_Oc$~8km7PoGQVOngfx= zlpqGWxlf1MjfA?p0{x;8r!EQ(%#CGMb)8R2-V~%QI}kHt(#rdM7Re;((DJy_SQE~h zT1Q%5Yft#$3fpAlRoAXi0QLEK&=zE;WuiMIM!+jV-MenQRX-Gc$jkB8h6X3s0AI7p zj!RqnMg%v$J>&*)xCh#cwKbH~Xj3Y5hllqsYC4|Bn5{JJCe_b;45uVe)sa}bDOZxS z?O05YqHuG^eyMvd+0o@1?B`R@beFqb_CHXaLNm=nkF>Lz4m*Ie<3N=q_k-IoMLQdf z3tp^Uq5*QE^~OTfV4LpZG|USs$Pd zLHS1zB~)T798+ooORi4gW7#@zrUn$1?WC3qMu^{Zdry?pjw;qR^&B|%OA{jfIDF#L zu?9zqsz?+h6S1<t)3k84b3+->mLym29Iq8AykrU0O&eiPKkn{yvuLVr_UfFmE4 za#n!NiIgVMEe42;R^#dCUDX;{C?g+%o9n%C<8Xf?u_N)F*&;=FeWHd%oTjdwnhd*A zN8r?e6l7o;HuriywstTDrwf=Eu`|#`=j204Q4Sx4V=WJbtHax+`~+Ff?+@)^`>?= z^Wc5t+{1|W)Ygpni82XNL^%M zp*MnUGMnsuZ6q0^q+8Nk6~GW8z5W^{>9#NouZ$5+p+?)-6YVQyG3<;9-*0d(7NPq8GAXFIM|x=ZdB95 z?V%{{D@4c`g``LQZUB_GT2dIjyp;)cPa9a!)=W<2MgrULq)jR7lbYfSL$*(ai-{8p zPs(VObATd_nPw%@!L>}QTrei{r&Ii*i`zsyV~2;$sdnqYx+3PUIea92(ly(y6KU>U zreM=>=_Aw@QRQ?yQQY2GLsQ? zabL+^ly`*)Tl{jJk_TRQJe^e;S_X%8L=9#Z5Yv%BQfl32{t2Zry#TtLc`7q70l>Ne z&^eVfWISnQq2Vg9*nVCC>KN`jnY|inknXH%h2A2m&eDQRDfpm)U={id8;TN$~3|T{hnHIV3 z{V=2~`hFYYDz>*90D$b@@b;JV?Tn##fI3bc8l;f=ceIJ113JGkD--i@dsU*2i2)st z)WT`m&Ki*^SskM>aka>{*?TG%q1J-1NMdb)U+2976Q)eRYIx5#nTZGVc9kjVF{+{z zHyL1}#cExew=u#L@L0xeQba{Ry)xkKVgPi2{hN_%(@>a>a^X8(e=vuHj>=tejA~~b zhI@)HCG3q-=pb9@IMjuMm6I)8&~e_kyZKupeh~Ww5wKY7xR=)SA~BXa>3otoS6=hM zjpcpyT!1%g4R5CqofbPqc)EI`$JspHzujhrVk@kZlikgpn9<7B8?{+7{Co;&S7if&1X@Q4wLU`hO<7XMnIi@F z8}58T;G_6;MZf!{rzWxE^gdJvG)G7Uf7?8i`;D^;ek`w(yPZ+CJx&~AHyr}~4~dGP z<0ymSyDC?FJp0$D5q0K*mIn?qC*2z+tv9Od>)X1aTJK z&Jj-mF7sHKAd^FrJ_rIGBZX(oItJt`4|NNv<`2yKrIntS77Y;ED0p`NHBCw2^^LqK z!=3aPd%_g_AChlhV_x7ORV1z+mDqjEOoGo>^Ek|r5^O=|-3viR501wJrb_;{#Ppew z;+^-wsmI$dEE~(yTPRa;7p>KdUH%V zvCpEovII{6WYqp*dB)S*|10K0%|uTP@jXUq!Oo|>K*52|HhkFZKj_j;O-$s_J66)- z8g9N=f}#89Zu6jQKydYxzVqyTH&5vsyi(+!7#u>iKl6d3Nc0oMdtz>KHU5?SO*Y!! zA)}f4)>i9-))q3ALK;n>aG2A%JDz1_X(da+^`bxv2{A5O9DY5D1Pw}c14QHk$U*H3 zbCDj#5PGd}UQ6PP^zUfnte+O_(h5-7VBjL)u9L#(1gai6NU~WW?GL-{EG7^5b8iP7 z%e>A{8cNH>FXBrQA0CLJFJ^F}aY|B3`;+R}GW#?8rtwn$dvT88P3mL)j>#kwar0I7 z_sr6xK23enN!Y4gx5(WxTV4_^A%FOZg!u@{H|-58OYDQOI6^h!F7spE(!h^RXQnb3 ze(4cCiW-ha_ZDv!6PgeR5i@+I@HtF==i&r1r{6)YfIshAMLEZ zNPQffm*rQOtGCa`NL?_r$M{9{^M<_rgM7SuN&#upHU0Ci@-_;W1%>;}a$!Di-#-$^ z>vzawVm}vFN2|}lT<>6hVJdQ`_oCOYqV5S6eb5CSp10}d&GQSF#&x*&_hBZah6C7_ zyQmB}XP4J;C4t^H5o<-$((Zpa?-3Idi_LsQ_uMXTM!RMk3Yap8FW&!~Gvl0fVmeNLb5Y&oh(WOX7fhRxG;o5f$cVTKz=y({BEj>_SoHF=mst z!1v|H>*gjGt`t)HO8%ocJlwD1MClzq?s15;=N_L`Eq`;i%5k5e<{CMd*Jp57yR-P3 z$zpuZXu5mHgcU;z7n;=kpY8omnZ>>M&u`bigi{q)x<1F3V?ozS-?>~5&pE~9*oP9! zKqcx|;wzR%1aLI}c_&i)m;2X4G5MrA)pOJAx~e?&vkNxg)00&{%s7}W;n{j}fXS+G zowwsK+un>_Vp1E+M{S3Lx*aZaM9~YI2mknw>>P8fI*! z-u)%)vt#MO@HTwf2{2VT2GFdPEmMhEAor@`g6^q&kkIQy^@fEUdZHkfG+1-=4qw$~ z!0ZV(ZNKX@S*XpnxQ2KWC=PQ&Xae1d+!PTyF@pbkHqi>GQAhVcgjg_2)Z8g#TINQG z1P_D<#SI;46nn&;e&G69=+o>l_)Yy8#^`jA)n5hlPhZL$YHBz>Zl??na95;wza>px zF7=K)KkKDdA%2)D9H>QQWd0M%Q0FBl*meOYFaPrU$3N3fYV48nkYE$SZy&>NH>P@!Ys+lXs>9J`noF>+ifOtT+X*Z&ggcos6xW z*RC!h^S_mBvWxT-Ac+!g`NZMo-}j*~!^{ z-6GQu`Ppn5h>o5RmgstGU73Xy<194>cYA0AM85qytn!W)cmYaCCmkpjY#18xfsKNr zQ`|U2^Oq8<>>;{VELPc4F-Lb^W%ECaYn&_WwSy#4e z8p|dfU9%Y%>X-*v+D^amvuw-y8dfrTR5^$jx%;F~^Nki~_W(5_HJnThY{bBd*T^-UWuUH)LwR{q48Gc6k17APd zWSkYBCGpBUt`nZBYWtjOdi0Z$eO8?duhx+#@@|Aw?T2h4lx9?4oMw)5e+Ajqk7#z1N`}`?De? z@y%fu?MqTtAqq*mOREEE_ELwD@pZK-YVmM#<}bC2kv0;yqG@6$6iEq}{AKy)e(IAK z!Ko4Hdl19^^?K)T{Kx(BnY5_kv;+b&l@dj;+{$cDm2N&2h)fpNXmi>-q|*X+mM>66 z780eyFi?RfOb>S*Xiq=_oM{dr;-EpC8wIx#F!w#w;9+E(QH0$!W?cl8oecqCg7BFh zZ34GdwnDAbr$ZU9yVjC^LqsU29A|Ln3TfkTf`|TxWU`>k;n<_t_H_o{TC)MF^_V0` z{+NGEF(xb|FjStU6#}V$OWyQ%I_%-gWqIW*`LJ2GL(PQ6$!o@MO}}KS9qse6>3?K@ z*zB_2@6~c?Bs5o`+7E={j`H{(4YYI^IBgUqn2+#C?<)?ahMWgsm^<63H?**~u`fNX zlyCQ#8h+zfs?eGb7`o4SJ@O^gEicj4VpVC4YJ#7vh7DIXr+l_;dC-%d53IT8N8dAf zFIk?bl=n$+i+$p~p>{=s#s^ln)9Z>i`>e}ZfJnSBZn(WQ(XN3QOxeW?Uj&c2IW!)~ z6joCk4w6^U-h^2Fir-PNv>BN8fem+}p>}zK2x0Bw+m%r{*zDG`X0d22=XuqWqO>el z00L7g%3(Bl#@%TSb_K0LkPy>ea~N>so)8~-RgbS>0A_9&3D};31jM0GF*k?#%*Eky zu7z&T(#~?B0_W|KjR0gH{2&(fLXGL+pgL7|wHXR1Xtc}tkL^^cTyKFrVJ`6Q>&&I@QT95R^`~|xIygwLt zd{nAI>&=!Gv8QJ9KP2Uh!6~=8Y#=5Aght62#`MI7Y*bSNUmj{6~I_RL+)XImn9qMXp0Z+Yh6fIFLk%0I)^;~ zrbDGnBa8WbUkwI{oGq}tzFUBSTsqNuJM#V+9A(CmjByjy)it%Xoy#T5`1n{=m#X+b z+(Tq_!&n%%Q~!cRE@#*!!goH(AEke;zJ~vkn z>vZcnd!Mv=;bvKn!0S+JDW0@q^82!=cuQxngkP7V33sV?n9>Ng4p)_jP!e!T&pJ|Q zdCk~-AeY<<>>`sil|@%!66N#UD*;5Ja!RWmcmK`19X&K zbCij0ds=MTV~k+0t91~4lWW{BvYkHiPY7W-S$VnJd?A>VxyOJmvqPYbGpYc*2`w|e zL8O}x6GZO=nqtx2chzX3jT{DdaTGtvFPl7?NNmn=5iB9e2^esu=4*%KTghYyG0Sh$gCaA5`tt)$R^5K4n^s(vag1 z*v`Z$Flk>E%9{qz&ZTJOcF=P?nLkvpU^gHDTDSo<$V=4ZsF(YsYGP`*(b_v^bQO{n z`z%_ds^us8^bd9PzUQ+B#r<8s-xbO2a~*Gh>K~^+YW3fXezfcS^+iehnxg1iFU%N? zGMWfcfdTND0eB`t9E|SuHz1AQ-;(qwSBX6ftC?x<-{N55srx{PiRdSAd4@-R!`h={ zxT0hJoLADv4PZ-MaGlY7zhPm4EL#GHcznrtt0`WTC5kHiKDa%V*w|@qF<5tt48*Va z$}d?_+ftNU>TFmusix-6x56g%#N4Xmm%dc9Xnd*joq03lKB@VSsbpmUj^#S1Jq36f zV?CoZBqO^E27qAk6po8&=%q`cY=~-ssQGxaKoi7-&(O%%HLZY$~p!sdp_PY~w1?ihf zx_XXdwo8gP{(3BTjtOW#>JcSZV0rnaoX!?Jli$*iy#Y?0-Hvj#9Cov&?PCI3k!ah+fy}P|~6a zj8xzN2!Lv_4um5}+yKl3Pnau%g9sXrH3Kzv8)}dbd6qvq`?Epys?x<9hmE-9@n%y4 zy7pn2#-GZ!7taQ#0q?L+AIM^_!1D%@(rE|1wSPj8|JK|##MHhJ2v&6*n13*AoztJP zjJ=OMehn~(IG4OpW{*Gn%|L6}6`xw5mNL=!%tAoo_a3;(a*25=<1i^mRaE3`=b)F5 z%&jIw6lO51Ll*flqG-cIf-%V5^xW((nMv;G=W%w2SjV+AQr+4Gon2+41@z_Y#M#oT zRP!&WyLm~(HRb<`+$Fc9)&14*HR4189tKq6s20GQzjS+@jC0!9fl0^o!~co2;xqN+ErNEd3HWa$ulL61V|RBcMT zl{9;<`mnCiNHi`bMswa7?wVd~#93uk{*cL@XBeM;o!eMo zfT-!0Wpb2Nks6tlNN*0jSb!xj85xkxRNDe3Y0O)u&eTWe-8RXc&7QbNN91m@Y8XnQ z7F!=)86DV6rL)N-9ls&e&HC4Mo+DhNI}2vHw+G3!OHm)jM*}nddH$o^w8BMn_-){e z2c;sol~1wdg2}Yd2NTs~>VAKa9BO;VvCEk^sko@v?uJi=WX+LaYY_8W@|wlfVoB)I zl$_sCP>z%Lgt|`7MW4?22kLoAZfDo?`teaO{I@VE)f3{Zp9}5mnb>}=^P7W$kfLAa zMwE1e>;vJ8<~UwpSO5{I*X);_j-xLT=?OLnPmywy7qxQ+Q4AV=)2rtt-JnV_zUsn` zX3&j-aX+?nuxY2EZPv-{RZw%U z_B1(h{!7baJd_7mEQ&=9ah|IGE{NAk;;k;R$37nXP{GMGDf_`L4I8#B`c^?IYyuC? zL0U{VSX;!~n~B$ISJF#oL(pDY=?nts8AE;rL5L3j9@82SdLwZmdc70Tg7i>DkVx}T zv=ULK1MLyERppee@HcMH!IFa=HT|9X^>8Em`p_XKQ#RZA6C$GYM*vLr&#RLS9Z7wI zx|i4Q%(;g`3p2&mp1JG6xb@ArzpHy^a-&01$y}Xwcja<_0;VYp#j?Kq@);y@SL4Fk zhV0C^uUkC_a*wS@#M%7xv0+u<+6ivZA9=!|-0h&d1v}}y^Qyuj*!iWe)7IT1q|*7@g=PdP=rcZ+4snnW$*R{b*r{h;5T zLs=3JNsd09^bc?S?)_zL<9b<5uSn<32T0ogeQ81qbAy~h`x&u!T?Bd z7=T%YR=Z3zTvrjIEaY>TxpMSC=(rPI1_>I39tV982-iU397f`Vp^KnKg}Tk6g$)`eFkt9^t-KgPh(yePV-PXEh?P!_K&R1@{Q@R& zU~Jx#yfI^EOi@n6SSs&QUd)4n)r~KCQ+yo;hti}Q9ZUS3OzyqwQkYLoRXQf$kGx|> zMW8Kdf{tBua)s|p59?F)Btqn)De3iSPxox$>-4(9#S#+>9km5|079albDC)9_T-zb z#MZNTkB5lNIin}Ak^+j%c+rcY$ra4D!X$W(!@Tz8uYstNihXjhr-^Y3Ia|hGJE90q zYssI&KL}4uITF+A8^HI%B&x08L?-S;&e(X!9-2EVm?rKw`;D5G*zY!q{;rr;w)e^5 zhWVsg8SP7~Bk$jp$e=gM2w@-GG!$q){&U$C%yP&ky0b$BdZ0RQg#{i;wVVLy19vlr zKJ>4uxDsIHHF|oo$$4$X$#jh$SELF4k0#}o1p{3w+R#$>>q~5z*+AU`lbcBi>%~0$ zQg0|3fWy;z_2dSIS$m^jfCHHDJR+;d%4o z$-sE;r0hKj0;N6CQ(AHx0u-*Qp*-|XnDRtuge!yId&S)aX07QsPP_7`0b& zg|AB$s2k0tXPAW3?`{6?({oAmMxC!TPv-Zo?*ZHw1)K!1(&S)tVC^hI(QW-Six@QQ zclobky$e7U!3;o0VERL0lA};$90WQau|k1&04x$oY83LoVToToY+he> zqwmwm>zfLLCF=o3O^y<~7~L%*x5?Gk?I~nOdNk1rS#%h8EoUfdh<#WfR6BrtnRM`n=S?hU~8kL zl+hvuVWW=a9jvyA?`7poFT72sE@u+v$_`sB-)B?shOecWdN&KGdWO#o1}$HRiaI=q zsO!fjsB2|=73jNITIh$N)+?!Fvyx)0vG1d1NSsgV_ zxOW#daOl%c}1Vl8i^o&SQXVWeRcrclceD*#F;L?jLo@Xm8k+aqhB0yrqYPKULGcZJo2 zGZk}n<>9z6g>}spO!yZ`#RStkDf<+AUELua@vxI>{99Xg=A%`J1 zco6CMfe^&zpyzu=9XKQ6dq<*DJ5`LP(7#Dh&VOojpRA4WVW`Q&A?@;35UENOwTQ<@ z`p6$)8?OJ^>_b~nJ?_x3R0K*Xdm{r*TD^mh4gMC3yI5u!AsJeY#I(;$utUyfYI|hX zVd_)9PH-pY^HN2XEL{VP;(Pbj8`ADsztQX3&;OPXgofR=A{PeBr%b=UyPpR;wlgg+ z^q!@yB@a@{&8Bg}blg+0Y3v3?ui@qHIRW5==A`CW^$3XqXPl@}jI5gKt?L0J3#bp5 zS;F09>QH7#2au(-F!Y?6vL3qLWWMIqLEDe5YIVKqYx!IzqCHA0(0ZLImfoy|xb#G6 z%nPPXISLl=P_FreQ{iTjIg=OE@H-wBuO_@a-cRyFo z`O$7R#pC=G^Z^PQZ^J4_m<8!dCM<61CBJqLc()K6n(vw(DamqmK(G91VNJP0gSJ`9r-WIXg+ zPti~pq(1}!mO=zo5a~iYbcBu`BFh2GY;^@Ww1$G*>$)AQ$Fu=xlG=o-Ph|QQWv&o& z=hU$!>VFN#S87f%2i=>U=+^?V8c7tXf8XU?O9JSFRtk|%>v$^cj8bZ*I+^2n*-!Er zV(I#<9NxAjXiumm*ksXkX5wpi~RT%x+RR zxKUJ!;;?lULEC+1nnN4B8oqknN+t4_Q@MkGN*u$M5o*QL$znC_FGTCoZjBN?U!(Gc zwaxi(xidwdz!y5>mDRZZsvB1qute1<3@xqh?`{r^!Irw$B1(jMVwBzET#P97LVyi8wj z97B>B(Rx%DdRlWYTFzwURZTLf;uV6SqXP?WD-<5r7k!Ml5LnsoSbU`(2!o5`NJ`Fp zhI)AGzo@l%FjM_0oXD6cja;Y>Sj)a!AD6QMy*V1vB zpud#S`=(5yLmsFK*V&QR1HC9%SsZe=&Mr+V3@ND36kT{PWJS(z{L-B~eUT;}BRMRs0{N@Rc z4K3%LF1*+Oe<~ac)Onl09}A=9`MV&-~OSKz7Uy5nMSD z%umngz!t=FlOWI&7{&{Pb zfZ*t*oT8|vzcf`#n0l9PDw?23$6GKCMQ>C}(rxF9{Q8Za( zkO!+mKyqtM62FDYO%lhOH+KfUzNlRA|J!*slw%j$d6Qi9#k|d`|GqBk$i|9KF!jyj z@0OIXG{F=d|9!fe!m?g5haXx#WV&SD_OEOs$T@_xB!E`Cc=nX@^M=0vA)$%3nJu24 z&pa-?t^FTT_Kb|LBU8NSXu;^*h@IpjxU-6b%e{ieyYz@w4Jk#o?U~+=@XG>KOqA-X zw@``AG761X&01F2_6ex)JDiQSG0%i_CQbctJF5zHsg^5BG zs<8|>xZqq&EwQjr2bhJXILw8EfCEbdHOw8v#lZ@KWwL}$hbheIz^s?Z^CJfV=$yp;AK!HH5j4ALedIGOQ?xlHOS`lo=}e*KG_Ld9*VJak$l*hL^uv(CxV)ooVWqtL za*34m&Q8(OrLtaX;2`XR(0E-|hziRaq>?p5Q9GHl;qMdvA?NoMYX8owZ(}c2MV;`P z)!#pZD<^Gwr_ejyrOf6wHeB98xVq36d6MLoyRCGU%#nVd@QqZb~=VHmA zvsl@9$W6R-Tx+RSwGCZuRu^cG$KDr=2-6i|j*9b6XKhOU%D*2>Hv4IE={cyz3nyAX zHYD%`VP?BRKV?)iNOefRqg)SxcOu}P3t;SoFjbsJgrX>L2HTx|-Gs@rKl_sAQ1Ex) z%!ksJgdYKBtk-COOje89^VY(Fd1>jMVVBsrJu-<0-Ejkp`GB`41U^M6L#E-UX1-Xb zAK2dx)bH)R=TEqnY!Uvf#Sq=&|9N4-t&g9=-SPfvEodu$K5u|`S^Bf8L=0WqK~Xes zZ+16mOjO^RgTtDdlTZph9LRw<2md?F5p#K!7>MWwX;x%bEHn!7i-Se#5!KojGa6kX z&htmeX(SkCp@FIzXitHUf)e@-A}a|CuxHgks|4t0rR$UDYekaTlor^AdCmSG^yXI^ zGR&z?>_yGuFlEn$rOe($J`Ls{Mlb~NkoCXyUhsV^$S+fMzPr_5`j~U+XO$;2vIU?I zSTdo}J+884gSAPG{8odxi}V9(o$UpXjSmn44P+d;MO>zkWGO_H5AWe1P(+3S^Du4y zOMd)cl+)tX(e0X6UZ5%9#`S|KN>Pz#mR=|iriy_wy)kGU)Lb7&+(JXPdsxQ z`#%G7vX7K$5%B1>*BVDKCgl~{$E3}dv%9Pk<-pJzoYv2$$oq)AQ8=zQ$gl^>HXmEZ{DLQ_mmPQi%J#mP<71lNxP^VOSI z$uWmu*+6sMIA?CRO#IAbS_W^f4#BeW9;mSNzkljg1uFt09}1vqVP`#NHk$24M1o^< zuS=219<@RzJTi-wLI#9PVs(#4!eKV`j=3bzq~x~&?*dO@tW9}x1eGtS(Cg~^WWdTPaoPVP?l$CM9xz&(k##$d+K z>xPPz0_QvOrkI(0`W|z@DQS@y zJzA7br9*N9C6(?D6;OX~Ki~g@|F!GdwI_RYpZh-NyyBdEd@5fW=1tWHa2ny}NP6rO zttvE}@X3?XpHQ*#h)ytmWHA*XLX?Hrt~X*=m!MKt%tpa!Goh+^K^pZw>IYQswF)On zk{GCYfZfEyaFm_$tX#hUNci^evOsrOOFEyhI} z1|rp;$hQi)3*|w?p%~-C2TFd6mkmGNOMe^Fg}+eQP>(|m=A#MmP@K8Mj$i_sMPLF8 z^*KsTlK543I0Xwat_t>Ad!~{fQ_}3A<|WK0s&z&NhN09bZQngbfDh6Ha23`iNxTQ} zc%b)^u291dX9^oyOY?K(h8n}n(xe>Fg4*`9aGZ&y52MA+ckDNZ_N+7(qk_8DS*uEX zo#u|Asj{DZN&B9hLvLdH{enBH$ts6GUxd{;?6|zsiCoVb;o-gda2dv;9`G~av_SNv ztDbuIgW8wMAC8kVA^aEK7xE)BvppS3mJ{$hzUR|Y6wE?KP@~1#&H$r@GzGb*-|Yjh zzI+sZd#&tL(xWGoRL&>!SxJdbH=yp#{Hua^beo#Ffn6P-u{fW{otq;O;E3m;|3w2^ zGO$XhbFRqcd%)J96dJ;O%*ajifdfrsp3q5Ek~uj}#*y!YxoLXrU}>_;$+UMi{=DUw zc1|jop)$zX(0CEr`Vp3^J^kY{!VE?-Nb-rx+`Y;F1;gRK{DLrMcFe+&?yRt0ox?y^ z$~>n|0_0j*^JC0h*rtQGnc*muo7@_O~WhsmkQ&@H$4u8860_QQ~c1Qf$j!;yd)e1TdW^zgl=!fl}OQn~oA z3*)#x%7DhVVovBC*TJoO#R^phdJLJq!1Z-qXy|#6ytl67b+BdLb|_OY%rl(0+;;QA zAuY=c2E&WcoM6>tI$2{rsiT)f=)HfI;4~C=wA(mYm?5KcZZ>YH>>j&Z@V>K5#o<#A zEiQMDTd41%y1KQ+ada7qQ)CXB&Fm2(tp56xZSeh|UzVJ!CEH$`-#aZ1k9Uis&pq=p zzMq}dzdgMHo6OCE)$aNS*HQUZx0~cv+}GS0cW2+|z2iAO5RtTB>Q{RhkNZ@R58aqa zNG9;lVGgwJ#Gz^t?(>ioV8Ij;h+-n(wnZfn2~4R~aV1E?wIpeTiti%uK^HUtq?L=? zD?m(wQe?vt$3zeSlw(5N8xmB0qdy?fi-77yG#LmXdB8;B33vjOcqrCiP!<9K0jrD4 z6d16<;MXt&%-zz|<{lwg-4bdP$e2V&gd1 z8tgr+u)wf80#$XWR1qAJ67mGI_uL#ZhIRU_F2bG=%$a z?i(-Oa@qb~kttuBLza(Ml?x8~SMfiXk^Xj{d#F5T^^4D?&MaIllrj`bG2x)AJR3>h z|80e>^EhR`H5&q44JlU zQeq4F7HVoUnAg6bB72$sZF5~SMsm{04C74ZW~N13z^r|T!ttrM^}|z1J5#z{D))~q zGq>|!S>}Ua$IKP^cO}-jtT*$nej4Xx`_Jwq(bU;4-)$xzF@k)PWW$Xo@x|3JQmGQ* zvN89Y*88s}yiZ2hjRdZeUEaSOMv1St_OC1~=0<=AppWVn?n=RF&`_~iLz00fAQr-x z%a1)*Q5In0$Q%OhAX3_dsyB`BCnLHDod^Ru9t1od6@Zc?3O@=KU~CSb5<%mjB+0^G zCQz_fPqSE?^}}^lot!lXsZUr%Th=m@hBj5UyL`{wu|{GWhkU1}fYHzuY!y?(Yila>2kp6iiAaa1C$V3a3&ij+8N zOvgWauiAC*l8xA8-frbkOFRUt*rNi-A*|l*G`K`X0dXfdI~?KOa6-^9~rR6W{fk5E~Y9}o=fE&IjWM# z_4|21oQ8ST8`fvMtTglyoALadTZ*?{FYz0h-bBkEv!0@ZT7(+?bi42_?tqw2((ajx z<`e!r#pe0HQ*UjpwX=l%C|CYda{Q}fg}H9`+4gAYc<%-H?ep)Y--}8G ze%d%momndPP#;(L-HA$t0UK=Nmopk>a>Y3{l4ShNX zlTq59aTxhvpZ}|xdXfYEGfyR>c0TT0dC_ruCVfd1LnhY~*Hn-+cW)1;uz{&E49RK%Lo3f-X{u3 zM9_=~Ghc0W_IDB7L%MoO=#08%TNxMA;@oolL~CPN7_A1|)SolXpdg1;$*);p91t@w ziIh%l(dPqX#V;rqK(GSoMWP9hC>)poLW!@7Pl-=I3Sb660ViNUBLJt6Lp=lFVDc!0 z!7BP?pDJ|%>IC~ZQXv|DCWVX?mP-nIfPj_y<&x40unVwDK#-Jp<9LTCZYGpEm;h+P zatR?SC{rNFu$T3!?GjefO)IsPZniOW(hsRmjz%`>-H?j=_3{L>?aDQ8T~#{G4b#7f zrIbQ$k=uvNVAf$zLU0DP+E-K)1NRJ^c2Ve`VRcz@TaEGU!c+(w@==Po(04y2nQ6;d zJ+U`34l@0(^H}T^3@$33VswRgizqWopL$^Mlbs9F-Tj9c->5dSLs%#UN8Ymm>s~YI z7IYtzaE)5{KaQ8>oQf$Tl210XsfGTBc=H6_-&cQkpas_cE9(&R_WC&SOkT?Cb4 z8tfAbP9aMLeUOL)eo2y4JqeX8Ob%i|Bt5(l?YMpowL22weN*2|%uk;ZqGCykU$gjz zcvXYIv2m6>abwCg*XmaD!jwSJf>+j>pJst7Ni#9Avo$mkc1||tvdrVnPTX&lTX`t7 zn`>y^kCm45)5Z=mwZ&~VF9%MAUwbz1$*$dT?8$$jFvw2T(br^9w|b##RF%IQH+k>2 zLx1Pug>sysSD~Yy=Y#9&Z@x!qn9`=ZN1xkdl^;{bCcn_QRB!z9xbFq`z|Xv`H>7@A{C5#RyMsD$BSftl3;xl= zs+uNU&;FV?dTUzM7P@RTSKFXWsH>9N@2D=Rp;F)O_o6%b1ErJQRxBKX=j2h#A9a7$ z)qK58%gJgZ;0t@@M&6LI5ch(bLg=5vs&5bOtKHKGX=l%k6s9Ip8SN$-UZNQ z5a~%(MbwnWYp6!5P;4nwLx2tPT~UA%_aA6Y-W%Z#rB>K`2|*L$0!&UmE-{!z^xclj z6BkC-ybT(>wXV-OBc$ubL6{M%U=9|_(6On@dTpK_9~hfn0!+L|ILWvJspwAZ-MwI01wjW%b(%Q_8IccpxSb#&t)>EXq6R|+Ka|3DCcefe z)QHpe1HWWB2a_K!A>)wf+l^y5j4Xpxh?!lA2R0~s3lqdsOhD0-5lfyY)e1jK(9%<3 z9U%Hq;XZD`40p1V!;yde*L#NsKiHY7jxBS2IaTn#@8n$k1;M6P`qtD`4?HskS8BSX=pVHC zF48|>lGS)@)IaZ4fm2}`kfo*tZ{nmIc-@RI%HeEi8G`siKMP-{*}pkB5G;RFoNEUF z5$|5u2u;Aw;ylkkmeJOU@<=}dqrAsmGlYlfOw8Yu+qSQVr5pnzm@E=p;)~xbuHNj5 zB+m+Fe>HG@Hv?NsMal76qoo!w9W`|igxBMnRzDpoQ#~-%IhQ(H4b!in+fEFze;-O# zPq$VdguS(D=_}auJdrSLek{To^5K{9>ma1_zTO5LA;<6)!|pn-E4^kuSyeL8GL1kt zA*9sT7U?dXs@@jN@bRDgh?y?Agx3o5k|xU9Is$H+zBNLpzGqhHQMK^Z|l~1z`T9$Vi4y z&@rz806HASB?-bqd+34#g7=7V;nWI9b|NHuZY1Pi>?+zuU_nITQ)Ku+-f;k0ad${3 z8t6daWZ_UC{TxK5Wj=k`Kw!@Ta+sk~$qh3mezssQgi(502=x^Eu$J=S;D@0%m7I%P zlCNLX3z-)!_owGPmdd;Qj5msgf3mVr>JzX54Z3YO)JRkCVdY|b#EwUqjU`syU}q7Y zGf^x&;lOW`ZtAPHSTDJ5jO246B~+Cg2D6H;lX~)0l4~+mioHXV462~ZGwmcvs%_R) zT6XIaFS|w`(=B|E@ApaF^*pqPpvl|Rp?B!+w)E6b2wltCm*-<8hn z{>m(99=P#vFRcv!pk5Q*G;X2m$@;oyLU~^_ML!HYE_t)uP4|a4&YjeE0cBsM4 zdUr>VqY|6tq_kZ%MFC;82nYJTI8EXE7nF`)0$d6a%d#_X3x(sT<;w?2H&m31>N(b0 z#~G?Fvnx(Ej=c&a7RopWk*}{NH?732s#vOKaHZ%GZmEq!tXQ&`KwLo9OSzgYo9V4f z7nfW|)MKp{lT(GBP`%Jt)wKDTmOl*hq6}J=iS?P2sXx`P*B{wBJ{|kfL#Z62M2Gxs z>4{;a;i4s5Yp-dYI=Iz5dI(;#V415+T!<@2@3f2ZTjkamcZpx#S6AX5Y*%+ere&5r zw#!cn&~A2Vuvlzk$_W}XcIwbEezL_o_kGgYTxD>MM5?waq(%QOvNDDeZ%_eAkAs5a zg|qtr_vF{q1Iyw#oBtp&Kwd3COcjWZIk}orh^dOBr#@n-;`D<*4zDVvgFudp*$Khl z1$bD|tc3UjxJ={t9Rl>gvrYgNL6Z#m;z^R*KtdYv!U05J^gn>xdqsddo8{ilSgf=J zjLE~swLQEP9q>|R@bcS8^H2b2N%BSETVB@cX^YVu(<9>Chj(92=6+D!Q@SJe2+J^J zTWR_o76bi$tQ9l(5|7H40FKWs$#CdT%sKjKrWnU4a5gGCXgVn3?QH9#5Su3zcV^X6 zW<;|sG|AI2p+(iM^a(i#RZ!DloNwkyYa}=m)GNnArFG$0S8Fymx;buO z*A(Ou@hTT8R+f>-Yk4cg%-!J$^$vaQQ&F;%o9!CX!r9IEQLE+o%XYBbVSW{tkEfg4 z!-~XpulQc0%uQJ`P1e{}14Pg>`9OUMA6KVXAK!Ck1MC{Ok_4Y?i6_sF=^IfE=$YkW zK6F5C3CUz}jOuV~?|7b-4Gnm!Ai@z4W(G=~P6~~R7Jp|+y5?a0BBXCkK?%#`&uj*U zHAI30;xblLX<(1JAdwQLkA%5g96o~yc@7$B-VfaiT>oaZ80m?`Vs7h-!x){;FMf8W zFs^#r)^9!cIo{p-Xm#k6yzaG5AhfdZC@s^G(oPU2cP6NKE8N;q-ao+FFhcXouWPuHKKijgP3##8Bf8&hfh?)VwoxaGw4`*_rUUgI;@7J6LyU69~+>6L(y zcg_mSU9W-(av7;cLwY+mt z2->BuFv-d`_jcn9CGcbU_4|fV@^ZZ6b$o1nQ_w5yfRCQdSCqv<)`C$qelB)lRQk9* zAzO}_D8d^M9Rkv;TwDkS(yOq;^p9i*zBB;UAvtk-oT8)wUU&rL07@-HjH3&M4B&^O zJeW}d>|hWWg!0&&2u}duFsgjKO$eF+Ng|1VH$2%C1t$uB23!kVh$?VNBuO|}$c%%f z%KzdJ8ZFMY#Wxs_x4iCG9P&H`v$`9+$i9cEsM5>p5&gy%$ft04SwwtN&AC1icTRKx z!wRRZ5MQOv!6D zZ!JGyP&wW#Nrh>j(~HJZ>@f`+CZ!Z-O({1$Ei?&Cw(EyF_|}i&tA6rbcMEmc{K9IO zREAz{YzD0g!5U6RDkH`meP(N=SkqbX7C#JF!5-r$^(rJJudM2jX9axJlo8bL;)4x> z&&PyK+2$2v!>gpJG?})L)WIqCc>0+{YRfhqmS8$|;e<&yq18LHuZnJ>N_qERb!jPh zH(4-0pYd5Kn7LZZ`4Po?Q6Ng&m2x4OBR)8h>_#bM>roND=D~kR)%82%GOqW&Gi`ML zHebyHcu3x_IUm5mYFDMBY7qa3W-!*o7gx76Et^_xl)%kp|%8kCg^fCN5}9>DnlTHhlihE0fhKwS}f9Ups(R8v;X$>xROJms{{Ytma&9_ zxmCuk7(GUMJK|58f10)w4fT9{CT~l2@2jtl_Fxh0g{xUTC{es*8BL;2`~@jS!`BJi zj8clU+#K{N(LpKXvww~^W%!O>wH|RFj0LWpIhedxMyC?$&%2!q)!W0*pW1EB&eNvq%Hvwfo85b>w2W;E zS^ZgMpMs-fUsZQw4JVU}pYeUp^G+N#)>G3=#eOwW!g~I|7s5^aUKu_n zix%C5X9?>M?pdRJy;-x;ypC|$W-r{R1_ag^PK{=VOl~^wgI6z+Rf|gnuS`~D75CeH zMqEb+QBkk27JXje{uZ7L$dYTMduRyy(RZF?rNrB?yvT>Y!lX=NEgxK-cZ7SlhNpoM z_u@4Bb$A(7kju=)X8AD{PFwTI<1-1P9!{g1sL~1xh8BdHvfrt<|r*JPq@m zl_?&$%Yx=>y+x);pq-3FVSUz{CORK_|0K`0h_LzS`~(SV8sN@-0CtOf8EjvEmax*C zhxpRi)+~kCtr}Ni@7<{{8)p2e)7_u9JqzXc5pEJXs;95SSj;TF_2=5XXI=hV@uH)t z_yjwbR$9X{we-1YVtpbKo|hnShGJj9(Z%_XCx{)5s0fOPLAwbNCKTBMA?LXY@j$Xc zRB?f)HWyET^}kQ_Xaay={$c@Y^M6GL@JO=|cHwR|;hm#Q0j>agpBpIJpCO6m@dR-B ziXZw#lT}$*VsWaSap;p5D9Vy|4x3@@WFN;(H2s(PgKYoeIA8dK0WkmRth)FvhnCR2 z7M^$0s|J%oAagO_&`PcM!mPqfXga?85jjWq3LreQABPv^IkDM8c)v+?xzv5{yHBVh| zmZhTi_KGXLjZnc7O*(Z<{=6bDW(^Q?yAztIc=ru&RFeMpNDv8X?>DQ^Gx1{yyyM3siB{q|+)8vJvW91q1q}M={_6-yVH*TUi^U z>jWQ5{nru$H`k@a{vs3oIeeVwFNAk3Wa1Xrqyu+{l0^kdPWc#y1gg7RUb5C&q}L#SWSA*zBse89E-bd zaOanC$;sz+bLH#F`I*zN6Il-hdVexv+x_`T`JeyfVEKXY`K%ZtK3(sk(KhU=^y+Ra zFkxT&ty`WUv+c50^2SL*AG>Q+vOqH!R+2{Yt#z)wf?{{sPJee4Xse;j~Y-E$Zly}^vd+{=2LNK=E`tC15pC;GVyT1ZEpS%+{+*jr4-Y+22 z;1WC^;coBTrd?xkRb_q6irL;cya-a5tJ-?g_rdJQzm~(3!R1=pjSAiKZY}go-+$A} zcdH^_Lxva!*W#77ip#0U@GRLbC=7pNX+(ab1v_{uUZZL%$(Ayq45IgY zQh7W&y z=)_HbD0SUo_J`T)ux>GJbxy@_0`f?<>*`lL_cc+!lq3@nF-HNzu<}K$J zMCDqLt4x>CAA`=eD<9+f;Kswfi_pklpq*-=g|~HXGw$!{BGHGD{V#s7vhz|Q&+3)Y z>n$z|pS}LPKdd=v7hKuc+_^)Pg~`>!bIo-gBAcgIUR84tdAa8K`DgB~n`Pa_xkcXC zhvH@Fb+8(juWqD7qEYj6zVey5aeZ891vk0gL1(J$f^mRyg)9BoV$fl6T`$IuhbPT6 zAVl48McH89fOq;Jd$7E*(87`JQq8yJ5imC9({M2mUj(jCBohQnpHAZ>2)6$77XTDc z1eDbuON5sE7q<{J8=U4PlobIK1VRxA;=ed6@P9uEyKsO?HW|P@rZ@pY$}$6Zc%hU# zfRP2zFeL%vis3`lFHCTYvM3s9)f~WH;mR{n1 zBdD6S5jnm{`nGiy{1o>l!b2tWw$s~cV-t{#Yn~wUywjm3>Y_WRXW=qGzD=jQfd02d zp=qoFtZ4b$fuv@X7qY=)_VsVRyUks0nf#^Zgf?`om8Ga8G4&zDdrZz5v4vX~ zn`H49N7F)W=IEyb$6T>v$xg}CRK{gu3F(Hqv%Y6NF5K3tn;twNGhmBmSh*n+;+5#W zptx5Wl1rh7!eouB5_*X05TcHvy$qcXVEQ(wP3>pLMZkV;53_7G^#>afd#we>cnYdd zeD1KFR@RmhCJRwUkP+a+H9*KxWc0i$8IC_7$OqaUK!0x=zJ`*mVgO)WpuJ}SXn9-| z_b3XX^6zn@z!FP6Ldl`502dTFJNUo&0caHfiyOyVh#%%{M{QB)vbw2 z(#r%Aur{i0y`np^eR;e4f?DJ=yAKVc=c397BxTtBC(ARx#@<^qo@0Exg1u z@O;9~%{iKXbH%oQweY9A!X%|{FCUfi^nusFnPBwOq-5j(VgljC^l3F?L^(t`pQz6p z7y3=#5nex9A|svVou2z{(8MKn)_yzv@Gs7?eky3C{`(oPbg1Gj)nGVc}2C&U9D^>`~EN{}ERAW38a zrA&9%!)5yY^Djy9Lr2G|8FgnF0v3`d%8IyDHHT5>p!e@Oob%S5-Ejo2U{7OEI|^G3 z_R;G2b}D%BhmtgvCMes?WFP9*O+ttI7D?(_AL`oRb5ykUktB_Xoo6m6X_{Dze8xLE z?dUqG4)!jxyFskh24{)WqWQ6t4~9BlUw_daw383|1KN+C5H&arfpIIm1R8}WE)X*^)?Zid#h$$%dt_Wj$3PdwYnA*W~9UqX=0TBf2D9bRCtqvYC!d{~Ao96y-nV zMLz!iaQgC-pzjxs=FU#_*U6T{)?PjGX=%$oN9bqXPLJkf8zvQZq|5WjabCjpKz@=i zU^Iv%q&O2eM0lzAHYO5lC9VlLBNQ z2ohk7;2d(;v*c~>brl*@#T_PN$iO|QTG^CZ6ZA=wZKQ?_BbNrKxyDVXSG(IMu*$6M zlNfaJA2;YkHoa;W2Iq?x!w1wi_sU+DN%t z<$OO@(RSB;ky&G%Rks=}H>llx*<65I`R0{iyF5`ZYKatjSvN=!|MxxjBl=tY85ykK>8R_F3R)C;GP{+cQv_5 z$pj!4l6c_yu0wJI2Q9S6MKsJvVDR)vk_}bj@@uADsai{_t&Zy2+g={{Rf1u5U<7j!icX?akVPVPZ(sIErHO2M4>klA9SkB`lfbx3LcFZLO zp8YYUwnn|C1(RU3{(J#=PrgEqxwTP(W1aiONuHHT`+e4SMa-KmGGlqgc z>nn-^O4-Hkb?$8X^&8OpmOcrAaD$ zxaKp?QnltNVta^-mM<9bO9)`oofC)S%-hT7ao{((t%3WU_Nx>tb|zzIAI{}Ej$g=> zggo{L3{5`!IfVeqmtP^!A5(kWp1E_M8Cy|Kn~;GbQB;u~=hRbsl;mD22qKHB^ITS; z{Wy*f79trY@TwBhcgS9A?~h4c*VfdUDoQO{Ldr_bmhSFjY?sQ)6&o*yowOzmduE9n z{oy;w(dD9NJYrF;5-lMHP48Esn+AqkXgG1sIKUGD(25ZLSP)?85&(`JPJ~kGK#%|w o&%XliAB!{v3>(P>bTqka+Oq#E{vb(s2SBR`__ .. |reach-ur10-link| replace:: `Isaac-Reach-UR10-v0 `__ @@ -154,6 +158,7 @@ for the lift-cube environment: .. |stack-cube-link| replace:: `Isaac-Stack-Cube-Franka-v0 `__ .. |stack-cube-bp-link| replace:: `Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0 `__ .. |gr1_pick_place-link| replace:: `Isaac-PickPlace-GR1T2-Abs-v0 `__ +.. |gr1_pp_waist-link| replace:: `Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0 `__ .. |cube-shadow-link| replace:: `Isaac-Repose-Cube-Shadow-Direct-v0 `__ .. |cube-shadow-ff-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0 `__ diff --git a/docs/source/overview/teleop_imitation.rst b/docs/source/overview/teleop_imitation.rst index a8c3c347f9b9..67cddf03ec18 100644 --- a/docs/source/overview/teleop_imitation.rst +++ b/docs/source/overview/teleop_imitation.rst @@ -333,7 +333,7 @@ Collect human demonstrations Data collection for the GR-1 humanoid robot environment requires use of an Apple Vision Pro headset. If you do not have access to an Apple Vision Pro, you may skip this step and continue on to the next step: `Generate the dataset`_. - A pre-recorded annotated dataset is provided in the next step . + A pre-recorded annotated dataset is provided in the next step. .. tip:: The GR1 scene utilizes the wrist poses from the Apple Vision Pro (AVP) as setpoints for a differential IK controller (Pink-IK). @@ -380,6 +380,9 @@ Collect five demonstrations by running the following command: --dataset_file ./datasets/dataset_gr1.hdf5 \ --num_demos 5 --enable_pinocchio +.. note:: + We also provide a GR-1 pick and place task with waist degrees-of-freedom enabled ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` (see :ref:`environments` for details on the available environments, including the GR1 Waist Enabled variant). The same command above applies but with the task name changed to ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0``. + .. tip:: If a demo fails during data collection, the environment can be reset using the teleoperation controls panel in the XR teleop client on the Apple Vision Pro or via voice control by saying "reset". See :ref:`teleoperate-apple-vision-pro` for more details. From aac30f9b443ef0f57833a17b65194d422032ee45 Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Mon, 18 Aug 2025 13:08:22 -0700 Subject: [PATCH 04/12] Moving pink ik controller components into sub-directory, updating tests and cleaning up --- .../{ => pink_ik}/null_space_posture_task.py | 0 .../controllers/{ => pink_ik}/pink_ik.py | 4 +- .../controllers/{ => pink_ik}/pink_ik_cfg.py | 0 .../envs/mdp/actions/pink_actions_cfg.py | 2 +- .../mdp/actions/pink_task_space_actions.py | 4 +- source/isaaclab/isaaclab/utils/string.py | 8 ++-- .../isaaclab/test/controllers/test_pink_ik.py | 8 ++-- source/isaaclab/test/utils/test_string.py | 21 +++++++-- .../exhaustpipe_gr1t2_pink_ik_env_cfg.py | 3 +- .../nutpour_gr1t2_pink_ik_env_cfg.py | 3 +- .../pick_place/pickplace_gr1t2_env_cfg.py | 3 +- .../pickplace_gr1t2_waist_enabled_env_cfg.py | 43 ------------------- 12 files changed, 32 insertions(+), 67 deletions(-) rename source/isaaclab/isaaclab/controllers/{ => pink_ik}/null_space_posture_task.py (100%) rename source/isaaclab/isaaclab/controllers/{ => pink_ik}/pink_ik.py (98%) rename source/isaaclab/isaaclab/controllers/{ => pink_ik}/pink_ik_cfg.py (100%) diff --git a/source/isaaclab/isaaclab/controllers/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py similarity index 100% rename from source/isaaclab/isaaclab/controllers/null_space_posture_task.py rename to source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py diff --git a/source/isaaclab/isaaclab/controllers/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py similarity index 98% rename from source/isaaclab/isaaclab/controllers/pink_ik.py rename to source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index 7619a5ea55d6..e197845d20a4 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -18,9 +18,9 @@ from pinocchio.robot_wrapper import RobotWrapper from isaaclab.assets import ArticulationCfg -from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask from isaaclab.utils.string import resolve_matching_names_values +from .null_space_posture_task import NullSpacePostureTask from .pink_ik_cfg import PinkIKControllerCfg @@ -51,7 +51,7 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: # Use resolve_matching_names_values to match Pink joint names to joint_pos values indices, names, values = resolve_matching_names_values( - joint_pos_dict, pink_joint_names, preserve_order=False, require_all_keys_matched=False + joint_pos_dict, pink_joint_names, preserve_order=False, strict=False ) if len(indices) != len(pink_joint_names): unmatched = [name for name in pink_joint_names if name not in names] diff --git a/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py similarity index 100% rename from source/isaaclab/isaaclab/controllers/pink_ik_cfg.py rename to source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py index d8da1e421983..834d23d955a0 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -5,7 +5,7 @@ from dataclasses import MISSING -from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.controllers.pink_ik import PinkIKControllerCfg from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg from isaaclab.utils import configclass diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index bbdfd68f1fb4..6af3029797c9 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -60,9 +60,7 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma self._ik_controllers = [] for _ in range(env.num_envs): self._ik_controllers.append( - PinkIKController( - cfg=copy.deepcopy(self.cfg.controller), robot_cfg=env.scene.cfg.robot, device=self.device - ) + PinkIKController(cfg=self.cfg.controller.copy(), robot_cfg=env.scene.cfg.robot, device=self.device) ) # Create tensors to store raw and processed actions diff --git a/source/isaaclab/isaaclab/utils/string.py b/source/isaaclab/isaaclab/utils/string.py index ca6a5cca8886..43a2fa0b3106 100644 --- a/source/isaaclab/isaaclab/utils/string.py +++ b/source/isaaclab/isaaclab/utils/string.py @@ -275,7 +275,7 @@ def resolve_matching_names_values( data: dict[str, Any], list_of_strings: Sequence[str], preserve_order: bool = False, - require_all_keys_matched: bool = True, + strict: bool = True, ) -> tuple[list[int], list[str], list[Any]]: """Match a list of regular expressions in a dictionary against a list of strings and return the matched indices, names, and values. @@ -296,7 +296,7 @@ def resolve_matching_names_values( data: A dictionary of regular expressions and values to match the strings in the list. list_of_strings: A list of strings to match. preserve_order: Whether to preserve the order of the query keys in the returned values. Defaults to False. - require_all_keys_matched: Whether to require that all keys in the dictionary are matched. Defaults to True. + strict: Whether to require that all keys in the dictionary get matched. Defaults to True. Returns: A tuple of lists containing the matched indices, names, and values. @@ -304,7 +304,7 @@ def resolve_matching_names_values( Raises: TypeError: When the input argument :attr:`data` is not a dictionary. ValueError: When multiple matches are found for a string in the dictionary. - ValueError: When not all regular expressions in the data keys are matched (if require_all_keys_matched is True). + ValueError: When not all regular expressions in the data keys are matched (if strict is True). """ # check valid input if not isinstance(data, dict): @@ -358,7 +358,7 @@ def resolve_matching_names_values( names_list = names_list_reorder values_list = values_list_reorder # check that all regular expressions are matched - if require_all_keys_matched and not all(keys_match_found): + if strict and not all(keys_match_found): # make this print nicely aligned for debugging msg = "\n" for key, value in zip(data.keys(), keys_match_found): diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index 1a1c448885a7..28edf3c3ff58 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -4,12 +4,10 @@ # SPDX-License-Identifier: BSD-3-Clause """Launch Isaac Sim Simulator first.""" -import json -import os -import sys - # Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim # pinocchio is required by the Pink IK controller +import sys + if sys.platform != "win32": import pinocchio # noqa: F401 @@ -22,7 +20,9 @@ import contextlib import gymnasium as gym +import json import numpy as np +import os import torch import pytest diff --git a/source/isaaclab/test/utils/test_string.py b/source/isaaclab/test/utils/test_string.py index 6b4a50bea457..f697509586b2 100644 --- a/source/isaaclab/test/utils/test_string.py +++ b/source/isaaclab/test/utils/test_string.py @@ -162,15 +162,28 @@ def test_resolve_matching_names_values_with_basic_strings(): query_names = {"a|c": 1, "b": 0, "f": 2} with pytest.raises(ValueError): _ = string_utils.resolve_matching_names_values(query_names, target_names) - # test require_all_keys_matched=False + + +def test_resolve_matching_names_values_with_strict_false(): + """Test resolving matching names with strict=False parameter.""" + # list of strings + target_names = ["a", "b", "c", "d", "e"] + # test strict=False data = {"a|c": 1, "b": 2, "f": 3} - index_list, names_list, values_list = string_utils.resolve_matching_names_values( - data, target_names, require_all_keys_matched=False - ) + index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names, strict=False) assert index_list == [0, 1, 2] assert names_list == ["a", "b", "c"] assert values_list == [1, 2, 1] + # test failure case: multiple matches for a string (should still raise ValueError even with strict=False) + data = {"a|c": 1, "a": 2, "b": 3} + with pytest.raises(ValueError, match="Multiple matches for 'a':"): + _ = string_utils.resolve_matching_names_values(data, target_names, strict=False) + + # test failure case: invalid input type (should still raise TypeError even with strict=False) + with pytest.raises(TypeError, match="Input argument `data` should be a dictionary"): + _ = string_utils.resolve_matching_names_values("not_a_dict", target_names, strict=False) + def test_resolve_matching_names_values_with_basic_strings_and_preserved_order(): """Test resolving matching names with a basic expression.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index 2424599cebb5..90f402f0de12 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -6,8 +6,7 @@ from pink.tasks import DampingTask, FrameTask import isaaclab.controllers.utils as ControllerUtils -from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask -from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg from isaaclab.devices import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index ea074f230435..994c0fe1ac8b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -6,8 +6,7 @@ from pink.tasks import DampingTask, FrameTask import isaaclab.controllers.utils as ControllerUtils -from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask -from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg from isaaclab.devices import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index e4d08c712180..7c120a1353a6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -13,8 +13,7 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask -from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py index e19b339b8862..a86583e631d0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -4,7 +4,6 @@ # SPDX-License-Identifier: BSD-3-Clause import tempfile -import torch import isaaclab.controllers.utils as ControllerUtils from isaaclab.devices.device_base import DevicesCfg @@ -43,48 +42,6 @@ class PickPlaceGR1T2WaistEnabledEnvCfg(ManagerBasedRLEnvCfg): # Temporary directory for URDF files temp_urdf_dir = tempfile.gettempdir() - # Idle action to hold robot in default pose - # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), - # left hand joint pos (11), right hand joint pos (11)] - idle_action = torch.tensor([ - -0.22878, - 0.2536, - 1.0953, - 0.5, - 0.5, - -0.5, - 0.5, - 0.22878, - 0.2536, - 1.0953, - 0.5, - 0.5, - -0.5, - 0.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ]) - def __post_init__(self): """Post initialization.""" # general settings From c98f32a68e9b3098a8052e32a7126cd6d8af070d Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Mon, 18 Aug 2025 13:56:36 -0700 Subject: [PATCH 05/12] Update documentation --- docs/source/api/lab/isaaclab.controllers.rst | 16 +++++ .../isaaclab/isaaclab/controllers/__init__.py | 1 + .../isaaclab/controllers/pink_ik/pink_ik.py | 70 +++++++++++++------ 3 files changed, 66 insertions(+), 21 deletions(-) diff --git a/docs/source/api/lab/isaaclab.controllers.rst b/docs/source/api/lab/isaaclab.controllers.rst index 07adbfcc0cda..fd476c9e27b0 100644 --- a/docs/source/api/lab/isaaclab.controllers.rst +++ b/docs/source/api/lab/isaaclab.controllers.rst @@ -11,6 +11,8 @@ DifferentialIKControllerCfg OperationalSpaceController OperationalSpaceControllerCfg + PinkIKController + PinkIKControllerCfg Differential Inverse Kinematics ------------------------------- @@ -39,3 +41,17 @@ Operational Space controllers :inherited-members: :show-inheritance: :exclude-members: __init__, class_type + +Differential Inverse Kinematics Controllers (Based on Pink) +----------------------------------------------------------- + +.. autoclass:: isaaclab.controllers.pink_ik.PinkIKController + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: isaaclab.controllers.pink_ik.PinkIKControllerCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type diff --git a/source/isaaclab/isaaclab/controllers/__init__.py b/source/isaaclab/isaaclab/controllers/__init__.py index ffc5a5fb9a77..6a5b884b78ac 100644 --- a/source/isaaclab/isaaclab/controllers/__init__.py +++ b/source/isaaclab/isaaclab/controllers/__init__.py @@ -15,3 +15,4 @@ from .differential_ik_cfg import DifferentialIKControllerCfg from .operational_space import OperationalSpaceController from .operational_space_cfg import OperationalSpaceControllerCfg +from .pink_ik import NullSpacePostureTask, PinkIKController, PinkIKControllerCfg diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index e197845d20a4..fba3a0b81b27 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -7,10 +7,16 @@ This module provides integration between Pink inverse kinematics solver and IsaacLab. Pink is a differentiable inverse kinematics solver framework that provides task-space control capabilities. + +Reference: + Pink IK Solver: https://github.com/stephane-caron/pink """ +from __future__ import annotations + import numpy as np import torch +from typing import TYPE_CHECKING from pink import solve_ik from pink.configuration import Configuration @@ -21,21 +27,38 @@ from isaaclab.utils.string import resolve_matching_names_values from .null_space_posture_task import NullSpacePostureTask -from .pink_ik_cfg import PinkIKControllerCfg + +if TYPE_CHECKING: + from .pink_ik_cfg import PinkIKControllerCfg class PinkIKController: """Integration of Pink IK controller with Isaac Lab. - The Pink IK controller is available at: https://github.com/stephane-caron/pink + The Pink IK controller solves differential inverse kinematics through weighted tasks. Each task is defined + by a residual function e(q) that is driven to zero (e.g., e(q) = p_target - p_ee(q) for end-effector positioning). + The controller computes joint velocities v satisfying J_e(q)v = -αe(q), where J_e(q) is the task Jacobian. + Multiple tasks are resolved through weighted optimization, formulating a quadratic program that minimizes + weighted task errors while respecting joint velocity limits. + + It supports user defined tasks, and we have provided a NullSpacePostureTask for maintaining desired joint configurations. + + Reference: + Pink IK Solver: https://github.com/stephane-caron/pink """ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str): """Initialize the Pink IK Controller. Args: - cfg: The configuration for the controller. - device: The device to use for computations (e.g., 'cuda:0'). + cfg: The configuration for the Pink IK controller containing task definitions, solver parameters, + and joint configurations. + robot_cfg: The robot articulation configuration containing initial joint positions and robot + specifications. + device: The device to use for computations (e.g., 'cuda:0', 'cpu'). + + Raises: + KeyError: When Pink joint names cannot be matched to robot configuration joint positions. """ # Initialize the robot model from URDF and mesh files self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None) @@ -63,6 +86,7 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: # Set the default targets for each task from the configuration for task in cfg.variable_input_tasks: + # If task is a NullSpacePostureTask, set the target to the initial joint positions if isinstance(task, NullSpacePostureTask): task.set_target(self.init_joint_positions) continue @@ -91,30 +115,29 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: self.cfg = cfg self.device = device - def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: + def _reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: """Reorder the input array based on the provided ordering. + This utility method is used to convert between Isaac Lab and Pink joint ordering conventions. + Args: - input_array: The array to reorder. - reordering_array: The indices to use for reordering. + input_array: The array to reorder, typically joint positions or velocities. + reordering_array: The indices to use for reordering, mapping from source to target ordering. Returns: - Reordered array. + Reordered array following the target joint ordering convention. """ return [input_array[i] for i in reordering_array] - def initialize(self): - """Initialize the internals of the controller. - - This method is called during setup but before the first compute call. - """ - pass - def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): """Update the null space joint targets. + This method updates the target joint positions for null space posture tasks based on the current + joint configuration. This is useful for maintaining desired joint configurations when the primary + task allows redundancy. + Args: - curr_joint_pos: The current joint positions. + curr_joint_pos: The current joint positions of shape (num_joints,). """ for task in self.cfg.variable_input_tasks: if isinstance(task, NullSpacePostureTask): @@ -127,15 +150,20 @@ def compute( ) -> torch.Tensor: """Compute the target joint positions based on current state and tasks. + Performs inverse kinematics using the Pink solver to compute target joint positions that satisfy + the defined tasks. The solver uses quadratic programming to find optimal joint velocities that + minimize task errors while respecting constraints. + Args: - curr_joint_pos: The current joint positions. - dt: The time step for computing joint position changes. + curr_joint_pos: The current joint positions of shape (num_joints,). + dt: The time step for computing joint position changes in seconds. Returns: - The target joint positions as a tensor. + The target joint positions as a tensor of shape (num_joints,) on the specified device. + If the IK solver fails, returns the current joint positions unchanged to maintain stability. """ # Initialize joint positions for Pink, including the root and universal joints - joint_positions_pink = np.array(self.reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering)) + joint_positions_pink = np.array(self._reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering)) # Update Pink's robot configuration with the current joint positions self.pink_configuration.update(joint_positions_pink) @@ -165,7 +193,7 @@ def compute( # Reorder the joint angle changes back to Isaac Lab conventions joint_vel_isaac_lab = torch.tensor( - self.reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering), + self._reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering), device=self.device, dtype=torch.float, ) From 625ec50ac372bddcb35c143b337f8aad401fbd4b Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Mon, 18 Aug 2025 23:09:23 -0700 Subject: [PATCH 06/12] Fixed bug with NullSpacePostureTask. Simplified robot articulation configuration by moving some config parameters to a common configclass. Removed unused file. --- source/isaaclab/docs/CHANGELOG.rst | 4 +- .../isaaclab/controllers/pink_ik/__init__.py | 13 ++ .../pink_ik/null_space_posture_task.py | 52 +++---- .../controllers/pink_ik/pink_ik_cfg.py | 4 +- .../isaaclab/test/controllers/test_pink_ik.py | 1 + .../isaaclab_assets/robots/fourier.py | 52 +++++++ .../pickplace_gr1t2_mimic_env.py | 6 +- .../pickplace_gr1t2_mimic_env_cfg.py | 2 +- ...pickplace_gr1t2_waist_enabled_mimic_env.py | 129 ------------------ ...place_gr1t2_waist_enabled_mimic_env_cfg.py | 2 +- .../exhaustpipe_gr1t2_base_env_cfg.py | 54 +------- .../exhaustpipe_gr1t2_pink_ik_env_cfg.py | 4 +- .../pick_place/nutpour_gr1t2_base_env_cfg.py | 54 +------- .../nutpour_gr1t2_pink_ik_env_cfg.py | 4 +- .../pick_place/pickplace_gr1t2_env_cfg.py | 77 ++--------- .../pickplace_gr1t2_waist_enabled_env_cfg.py | 17 +-- 16 files changed, 136 insertions(+), 339 deletions(-) create mode 100644 source/isaaclab/isaaclab/controllers/pink_ik/__init__.py delete mode 100644 source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 4d3fc656bb6a..ac82933a757f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -19,8 +19,8 @@ Changed improvements, our unit tests pass position and orientation accuracy test within **(1 mm, 1 degree)**. Previously, the position accuracy tolerances were set to **(30 mm, 10 degrees)**. * Including a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` - to control whether the IK controller should fail if robot joint limits are exceeded. This adds to stability of the controller and avoids - operator experiencing sudden large delays in control. + to control whether the IK controller raise an exception if robot joint limits are exceeded. In the case of an exception, the controller will hold the + last joint position. This adds to stability of the controller and avoids operator experiencing what is perceived as sudden large delays in robot control. 0.45.7 (2025-08-21) diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py new file mode 100644 index 000000000000..005645f97985 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pink IK controller package for IsaacLab. + +This package provides integration between Pink inverse kinematics solver and IsaacLab. +""" + +from .null_space_posture_task import NullSpacePostureTask +from .pink_ik import PinkIKController +from .pink_ik_cfg import PinkIKControllerCfg diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py index bb3ce2278abd..33d3f195bd31 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py @@ -151,31 +151,29 @@ def compute_error(self, configuration: Configuration) -> np.ndarray: _, root_nv = get_root_joint_dim(configuration.model) # Compute configuration difference - diff = pin.difference( + return pin.difference( configuration.model, self.target_q, configuration.q, )[root_nv:] - return diff - def compute_jacobian(self, configuration: Configuration) -> np.ndarray: - r"""Compute the posture task Jacobian (null space projector). - - This method computes the sum of the left and right null space projectors, - each defined as :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse - of the corresponding end-effector Jacobian. The null space projectors are - masked to only affect the selected controlled joints for each side. - - The final Jacobian returned is: - :math:`J(q) = N_{\text{left}}(q) + N_{\text{right}}(q)` - - Args: - configuration: Robot configuration :math:`q`. - - Returns: - Posture task Jacobian :math:`J(q)`, which is the sum of the left and right - null space projectors, each masked to their respective controlled joints. + r"""Computes the product of left and right null space projectors + :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse of the + corresponding end-effector Jacobian. The null space projector ensures + joint velocities in the null space produce zero end-effector velocity + (:math:`J \cdot \dot{q}_{\text{null}} = 0`), allowing posture control + without affecting end-effector motion. + + The final Jacobian returned is: + :math:`J(q) = N_{\text{task_1}}(q) * N_{\text{task_2}}(q)` + + Args: + configuration: Robot configuration :math:`q`. + + Returns: + Posture task Jacobian :math:`J(q)`, which is the sum of the left and right + null space projectors, each masked to their respective controlled joints. """ # Initialize joint mapping if needed if self._joint_name_to_index is None: @@ -199,19 +197,23 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray: self._jacobian_dim = J_frame_tasks[0].shape[1] # Initialize null space task matrix - null_space_task = np.zeros((self._jacobian_dim, self._jacobian_dim)) + null_space_task = np.eye(self._jacobian_dim) - # Compute null space projectors efficiently - for i, (J_frame_task, selected_indices) in enumerate(zip(J_frame_tasks, self._selected_joint_indices or [])): - # Create mask for selected joints - mask = np.zeros(J_frame_task.shape[1], dtype=bool) + # Create mask for all controlled joints + mask = np.zeros(self._jacobian_dim, dtype=bool) + for selected_indices in self._selected_joint_indices or []: mask[selected_indices] = True + # Compute null space projectors efficiently + for J_frame_task, selected_indices in zip(J_frame_tasks, self._selected_joint_indices or []): # Compute pseudoinverse and null space projector J_pinv = np.linalg.pinv(J_frame_task) null_space_full = np.eye(J_frame_task.shape[1]) - J_pinv @ J_frame_task # Only update rows corresponding to selected joints - null_space_task[mask, :] += null_space_full[mask, :] + null_space_task @= null_space_full + + # Apply mask to the final result + null_space_task = null_space_task * mask[:, np.newaxis] return null_space_task diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py index 824db01a3cbf..5add83a59168 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py @@ -59,4 +59,6 @@ class PinkIKControllerCfg: """Show warning if IK solver fails to find a solution.""" fail_on_joint_limit_violation: bool = True - """Fail the IK solver if a joint limit is violated.""" + """If True, the Pink IK solver will fail and raise an error if any joint limit is violated during optimization. PinkIKController + will handle the error by setting the last joint positions. If False, the solver will ignore joint limit violations and return the + closest solution found.""" diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index 28edf3c3ff58..3485f367e373 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -360,3 +360,4 @@ def print_debug_info(errors, test_counter): for hand in ["left", "right"]: print(f"Measured {hand} hand position error:", errors[f"{hand}_pos_error"]) print(f"Measured {hand} hand rotation error:", errors[f"{hand}_rot_error"]) + print(f"Measured {hand} hand PD error:", errors[f"{hand}_pd_error"]) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py index 42a2aa63885d..6e346e8ace83 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py @@ -8,6 +8,7 @@ The following configuration parameters are available: * :obj:`GR1T2_CFG`: The GR1T2 humanoid. +* :obj:`GR1T2_PICK_PLACE_CFG`: The GR1T2 humanoid configured for pick-place manipulation tasks. Reference: https://www.fftai.com/products-gr1 """ @@ -123,3 +124,54 @@ }, ) """Configuration for the GR1T2 Humanoid robot.""" + + +GR1T2_PICK_PLACE_CFG = GR1T2_CFG.replace( + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=4400, + damping=40.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the GR1T2 Humanoid robot configured for pick-place manipulation tasks.""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py index 5a70a3746191..31c08e9197a0 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py @@ -39,7 +39,7 @@ def target_eef_pose_to_action( target_eef_pose_dict: dict, gripper_action_dict: dict, action_noise_dict: dict | None = None, - env_id: int = 0, + env_id: int = 0, # Unused, but required to conform to interface ) -> torch.Tensor: """ Takes a target pose and gripper action for the end effector controller and returns an action @@ -49,8 +49,8 @@ def target_eef_pose_to_action( Args: target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. gripper_action_dict: Dictionary of gripper actions for each end-effector. - noise: Noise to add to the action. If None, no noise is added. - env_id: Environment index to get the action for. + action_noise_dict: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. (Unused in this implementation, present for interface compatibility.) Returns: An action torch.Tensor that's compatible with env.step(). diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py index 2e53e20c89d2..14c0ebd2a9d9 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py @@ -17,7 +17,7 @@ def __post_init__(self): super().__post_init__() # Override the existing values - self.datagen_config.name = "demo_src_gr1t2_demo_task_D0" + self.datagen_config.name = "gr1t2_pick_place_D0" self.datagen_config.generation_guarantee = True self.datagen_config.generation_keep_failed = False self.datagen_config.generation_num_trials = 1000 diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py deleted file mode 100644 index 8b12863de17d..000000000000 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py +++ /dev/null @@ -1,129 +0,0 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -import torch -from collections.abc import Sequence - -import isaaclab.utils.math as PoseUtils -from isaaclab.envs import ManagerBasedRLMimicEnv - - -class PickPlaceGR1T2WaistEnabledMimicEnv(ManagerBasedRLMimicEnv): - - def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: - """ - Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. - - Args: - eef_name: Name of the end effector. - env_ids: Environment indices to get the pose for. If None, all envs are considered. - - Returns: - A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) - """ - if env_ids is None: - env_ids = slice(None) - - eef_pos_name = f"{eef_name}_eef_pos" - eef_quat_name = f"{eef_name}_eef_quat" - - target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] - target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) - - return PoseUtils.make_pose(target_wrist_position, target_rot_mat) - - def target_eef_pose_to_action( - self, - target_eef_pose_dict: dict, - gripper_action_dict: dict, - action_noise_dict: dict | None = None, - env_id: int = 0, - ) -> torch.Tensor: - """ - Takes a target pose and gripper action for the end effector controller and returns an action - (usually a normalized delta pose action) to try and achieve that target pose. - Noise is added to the target pose action if specified. - - Args: - target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. - gripper_action_dict: Dictionary of gripper actions for each end-effector. - noise: Noise to add to the action. If None, no noise is added. - env_id: Environment index to get the action for. - - Returns: - An action torch.Tensor that's compatible with env.step(). - """ - - # target position and rotation - target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"]) - target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"]) - - target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) - target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) - - # gripper actions - left_gripper_action = gripper_action_dict["left"] - right_gripper_action = gripper_action_dict["right"] - - if action_noise_dict is not None: - pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) - pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) - quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) - quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) - - target_left_eef_pos += pos_noise_left - target_right_eef_pos += pos_noise_right - target_left_eef_rot_quat += quat_noise_left - target_right_eef_rot_quat += quat_noise_right - - return torch.cat( - ( - target_left_eef_pos, - target_left_eef_rot_quat, - target_right_eef_pos, - target_right_eef_rot_quat, - left_gripper_action, - right_gripper_action, - ), - dim=0, - ) - - def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: - """ - Converts action (compatible with env.step) to a target pose for the end effector controller. - Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses - from a demonstration trajectory using the recorded actions. - - Args: - action: Environment action. Shape is (num_envs, action_dim). - - Returns: - A dictionary of eef pose torch.Tensor that @action corresponds to. - """ - target_poses = {} - - target_left_wrist_position = action[:, 0:3] - target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) - target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) - target_poses["left"] = target_pose_left - - target_right_wrist_position = action[:, 7:10] - target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) - target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) - target_poses["right"] = target_pose_right - - return target_poses - - def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: - """ - Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). - - Args: - actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). - - Returns: - A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. - """ - return {"left": actions[:, 14:25], "right": actions[:, 25:]} diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py index 5dd25d235384..d5b033bf7371 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py @@ -19,7 +19,7 @@ def __post_init__(self): super().__post_init__() # Override the existing values - self.datagen_config.name = "demo_src_gr1t2_demo_task_D0" + self.datagen_config.name = "gr1t2_pick_place_waist_enabled_D0" self.datagen_config.generation_guarantee = True self.datagen_config.generation_keep_failed = False self.datagen_config.generation_num_trials = 1000 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index 9675c3030c30..0ed0afabd800 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -9,7 +9,6 @@ import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.devices.openxr import XrCfg from isaaclab.envs import ManagerBasedRLEnvCfg @@ -29,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip ## @@ -80,7 +79,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ) # Humanoid robot w/ arms higher - robot: ArticulationCfg = GR1T2_CFG.replace( + robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), @@ -135,52 +134,6 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), - actuators={ - "trunk": ImplicitActuatorCfg( - joint_names_expr=[ - "waist_.*", - ], - effort_limit=None, - velocity_limit=None, - stiffness=9000, - damping=160.0, - armature=0.01, - ), - "right-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "right_shoulder_.*", - "right_elbow_.*", - "right_wrist_.*", - ], - stiffness=9000.0, - damping=160.0, - armature=0.01, - ), - "left-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "left_shoulder_.*", - "left_elbow_.*", - "left_wrist_.*", - ], - stiffness=9000.0, - damping=160.0, - armature=0.01, - ), - "right-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "R_.*", - ], - stiffness=400, - damping=8, - ), - "left-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "L_.*", - ], - stiffness=400, - damping=8, - ), - }, ) # Set table view camera @@ -310,6 +263,9 @@ class ExhaustPipeGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg): anchor_rot=(1.0, 0.0, 0.0, 0.0), ) + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + # Temporary directory for URDF files temp_urdf_dir = tempfile.gettempdir() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index 90f402f0de12..24e4255258cf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -198,8 +198,8 @@ def __post_init__(self): retargeters=[ GR1T2RetargeterCfg( enable_visualization=True, - # OpenXR hand tracking has 26 joints per hand - num_open_xr_hand_joints=2 * 26, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.gr1_action.hand_joint_names, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index 004fb501a800..6a822dcd5383 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -9,7 +9,6 @@ import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.devices.openxr import XrCfg from isaaclab.envs import ManagerBasedRLEnvCfg @@ -29,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip ## @@ -101,7 +100,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ), ) - robot: ArticulationCfg = GR1T2_CFG.replace( + robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), @@ -156,52 +155,6 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), - actuators={ - "trunk": ImplicitActuatorCfg( - joint_names_expr=[ - "waist_.*", - ], - effort_limit=None, - velocity_limit=None, - stiffness=9000, - damping=160.0, - armature=0.01, - ), - "right-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "right_shoulder_.*", - "right_elbow_.*", - "right_wrist_.*", - ], - stiffness=9000.0, - damping=160.0, - armature=0.01, - ), - "left-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "left_shoulder_.*", - "left_elbow_.*", - "left_wrist_.*", - ], - stiffness=9000.0, - damping=160.0, - armature=0.01, - ), - "right-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "R_.*", - ], - stiffness=400, - damping=8, - ), - "left-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "L_.*", - ], - stiffness=400, - damping=8, - ), - }, ) # Set table view camera @@ -345,6 +298,9 @@ class NutPourGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg): anchor_rot=(1.0, 0.0, 0.0, 0.0), ) + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + # Temporary directory for URDF files temp_urdf_dir = tempfile.gettempdir() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index 994c0fe1ac8b..0f6b66731f5b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -196,8 +196,8 @@ def __post_init__(self): retargeters=[ GR1T2RetargeterCfg( enable_visualization=True, - # OpenXR hand tracking has 26 joints per hand - num_open_xr_hand_joints=2 * 26, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.gr1_action.hand_joint_names, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index 7c120a1353a6..e1791fd420fa 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -11,7 +11,6 @@ import isaaclab.controllers.utils as ControllerUtils import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg from isaaclab.devices.device_base import DevicesCfg @@ -31,7 +30,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip ## @@ -60,8 +59,8 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ), ) - # Humanoid robot w/ arms higher - robot: ArticulationCfg = GR1T2_CFG.replace( + # Humanoid robot configured for pick-place manipulation tasks + robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), @@ -94,52 +93,6 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): }, joint_vel={".*": 0.0}, ), - actuators={ - "trunk": ImplicitActuatorCfg( - joint_names_expr=[ - "waist_.*", - ], - effort_limit=None, - velocity_limit=None, - stiffness=4400, - damping=40.0, - armature=0.01, - ), - "right-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "right_shoulder_.*", - "right_elbow_.*", - "right_wrist_.*", - ], - stiffness=4400.0, - damping=40.0, - armature=0.01, - ), - "left-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "left_shoulder_.*", - "left_elbow_.*", - "left_wrist_.*", - ], - stiffness=4400.0, - damping=40.0, - armature=0.01, - ), - "right-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "R_.*", - ], - stiffness=None, - damping=None, - ), - "left-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "L_.*", - ], - stiffness=None, - damping=None, - ), - }, ) # Ground plane @@ -280,16 +233,13 @@ class ActionsCfg: cost=0.5, # [cost] * [s] / [rad] ), NullSpacePostureTask( - cost=0.1, + cost=0.2, lm_damping=1, frame_task_controlled_joints={ "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", - "left_wrist_yaw_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", "waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint", @@ -298,9 +248,6 @@ class ActionsCfg: "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", - "right_wrist_yaw_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", "waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint", @@ -308,14 +255,7 @@ class ActionsCfg: }, ), ], - fixed_input_tasks=[ - # COMMENT OUT IF LOCKING WAIST/HEAD - # FrameTask( - # "GR1T2_fourier_hand_6dof_head_yaw_link", - # position_cost=1.0, # [cost] / [m] - # orientation_cost=0.05, # [cost] / [rad] - # ), - ], + fixed_input_tasks=[], ), ) @@ -414,6 +354,9 @@ class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg): anchor_rot=(1.0, 0.0, 0.0, 0.0), ) + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + # Temporary directory for URDF files temp_urdf_dir = tempfile.gettempdir() @@ -486,8 +429,8 @@ def __post_init__(self): retargeters=[ GR1T2RetargeterCfg( enable_visualization=True, - # OpenXR hand tracking has 26 joints per hand - num_open_xr_hand_joints=2 * 26, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py index a86583e631d0..636f347109f4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -39,6 +39,9 @@ class PickPlaceGR1T2WaistEnabledEnvCfg(ManagerBasedRLEnvCfg): anchor_rot=(1.0, 0.0, 0.0, 0.0), ) + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + # Temporary directory for URDF files temp_urdf_dir = tempfile.gettempdir() @@ -52,12 +55,10 @@ def __post_init__(self): self.sim.render_interval = 2 # Add waist joint to pink_ik_cfg - self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_yaw_joint") - self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_pitch_joint") - self.actions.pink_ik_cfg.pink_controlled_joint_names.append("waist_roll_joint") - self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_yaw_joint") - self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_pitch_joint") - self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove("waist_roll_joint") + waist_joint_names = ["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"] + for joint_name in waist_joint_names: + self.actions.pink_ik_cfg.pink_controlled_joint_names.append(joint_name) + self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove(joint_name) # Convert USD to URDF and change revolute joints to fixed temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( @@ -77,8 +78,8 @@ def __post_init__(self): retargeters=[ GR1T2RetargeterCfg( enable_visualization=True, - # OpenXR hand tracking has 26 joints per hand - num_open_xr_hand_joints=2 * 26, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, ), From a54b496ac354cb105724a7c1323e7add6fe832c5 Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Tue, 19 Aug 2025 10:24:26 -0700 Subject: [PATCH 07/12] optimized joint re-ordering in pink with Numpy array indexing. Renamed GR1T2 robot config. --- .../isaaclab/controllers/pink_ik/pink_ik.py | 33 ++++++------------- .../isaaclab_assets/robots/fourier.py | 4 +-- .../exhaustpipe_gr1t2_base_env_cfg.py | 4 +-- .../pick_place/nutpour_gr1t2_base_env_cfg.py | 4 +-- .../pick_place/pickplace_gr1t2_env_cfg.py | 4 +-- 5 files changed, 18 insertions(+), 31 deletions(-) diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index fba3a0b81b27..f37ebe163e19 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -97,6 +97,7 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: # Map joint names from Isaac Lab to Pink's joint conventions self.pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints self.isaac_lab_joint_names = cfg.joint_names + assert cfg.joint_names is not None, "cfg.joint_names cannot be None" # Frame task link names self.frame_task_link_names = [] @@ -105,30 +106,16 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: self.frame_task_link_names.append(task.frame) # Create reordering arrays for joint indices - self.isaac_lab_to_pink_ordering = [ - self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names - ] - self.pink_to_isaac_lab_ordering = [ - self.pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in self.isaac_lab_joint_names - ] + self.isaac_lab_to_pink_ordering = np.array( + [self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names] + ) + self.pink_to_isaac_lab_ordering = np.array( + [self.pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in self.isaac_lab_joint_names] + ) self.cfg = cfg self.device = device - def _reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: - """Reorder the input array based on the provided ordering. - - This utility method is used to convert between Isaac Lab and Pink joint ordering conventions. - - Args: - input_array: The array to reorder, typically joint positions or velocities. - reordering_array: The indices to use for reordering, mapping from source to target ordering. - - Returns: - Reordered array following the target joint ordering convention. - """ - return [input_array[i] for i in reordering_array] - def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): """Update the null space joint targets. @@ -162,8 +149,8 @@ def compute( The target joint positions as a tensor of shape (num_joints,) on the specified device. If the IK solver fails, returns the current joint positions unchanged to maintain stability. """ - # Initialize joint positions for Pink, including the root and universal joints - joint_positions_pink = np.array(self._reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering)) + # Initialize joint positions for Pink, change from isaac_lab to pink/pinocchio joint ordering. + joint_positions_pink = curr_joint_pos[self.isaac_lab_to_pink_ordering] # Update Pink's robot configuration with the current joint positions self.pink_configuration.update(joint_positions_pink) @@ -193,7 +180,7 @@ def compute( # Reorder the joint angle changes back to Isaac Lab conventions joint_vel_isaac_lab = torch.tensor( - self._reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering), + pink_joint_angle_changes[self.pink_to_isaac_lab_ordering], device=self.device, dtype=torch.float, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py index 6e346e8ace83..96b50ffbbfb0 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py @@ -8,7 +8,7 @@ The following configuration parameters are available: * :obj:`GR1T2_CFG`: The GR1T2 humanoid. -* :obj:`GR1T2_PICK_PLACE_CFG`: The GR1T2 humanoid configured for pick-place manipulation tasks. +* :obj:`GR1T2_HIGH_PD_CFG`: The GR1T2 humanoid configured with high PD gains on upper body joints for pick-place manipulation tasks. Reference: https://www.fftai.com/products-gr1 """ @@ -126,7 +126,7 @@ """Configuration for the GR1T2 Humanoid robot.""" -GR1T2_PICK_PLACE_CFG = GR1T2_CFG.replace( +GR1T2_HIGH_PD_CFG = GR1T2_CFG.replace( actuators={ "trunk": ImplicitActuatorCfg( joint_names_expr=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index 0ed0afabd800..b7aed2d1f4e9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -28,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip ## @@ -79,7 +79,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ) # Humanoid robot w/ arms higher - robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( + robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index 6a822dcd5383..f8166188ed9c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -28,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip ## @@ -100,7 +100,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ), ) - robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( + robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index e1791fd420fa..ed40f4b2b6eb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -30,7 +30,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_PICK_PLACE_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip ## @@ -60,7 +60,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ) # Humanoid robot configured for pick-place manipulation tasks - robot: ArticulationCfg = GR1T2_PICK_PLACE_CFG.replace( + robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), From e2681cfa4b20f5f8d2e01f35b07cd791097d8dac Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Thu, 21 Aug 2025 19:11:58 -0700 Subject: [PATCH 08/12] Changing null space task to use simultaneous formulation (prioritizing all frame tasks equally). Also, adding unit test for null space task. --- .../pink_ik/null_space_posture_task.py | 247 +++++++------ .../controllers/simplified_test_robot.urdf | 191 ++++++++++ .../test_null_space_posture_task.py | 331 ++++++++++++++++++ .../exhaustpipe_gr1t2_pink_ik_env_cfg.py | 41 +-- .../nutpour_gr1t2_pink_ik_env_cfg.py | 41 +-- .../pick_place/pickplace_gr1t2_env_cfg.py | 37 +- 6 files changed, 715 insertions(+), 173 deletions(-) create mode 100644 source/isaaclab/test/controllers/simplified_test_robot.urdf create mode 100644 source/isaaclab/test/controllers/test_null_space_posture_task.py diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py index 33d3f195bd31..5fb248be50a9 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py @@ -12,15 +12,14 @@ class NullSpacePostureTask(Task): - r"""Pink-based task that enforces a postural constraint on the robot using null space projection. + r"""Pink-based task that enforces a postural constraint using null space projection. - This task implements a null space posture control strategy within the Pink inverse kinematics framework. - It enforces a desired joint configuration while operating in the null space of higher priority tasks - (typically end-effector pose tasks). The mathematical formulation follows the Pink optimization framework. + This task implements posture control in the null space of higher priority tasks + (typically end-effector pose tasks) within the Pink inverse kinematics framework. **Mathematical Formulation:** - The Pink inverse kinematics framework solves the following constrained optimization problem: + The Pink inverse kinematics framework solves: .. math:: @@ -37,18 +36,58 @@ class NullSpacePostureTask(Task): - :math:`\mathcal{C}` is the set of feasible velocities (joint velocity limits) - :math:`\mathbf{v}_{\min}(\mathbf{q})`, :math:`\mathbf{v}_{\max}(\mathbf{q})` are the lower and upper joint velocity bounds + **Null Space Posture Task Implementation:** - This NullSpacePostureTask implements one of the residual functions + This task consists of two components: + + 1. **Error Function**: The posture error is computed as: .. math:: - e(q) = N^+(q) \cdot (q^* - q) + \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) where: - - :math:`N^+(q)` is the pseudoinverse of the null space projector :math:`N(q) = I - J^+J` - :math:`J` is the Jacobian of the primary task - - :math:`q^*` is the target joint configuration - - :math:`q` is the current joint configuration + - :math:`\mathbf{q}^*` is the target joint configuration + - :math:`\mathbf{q}` is the current joint configuration + - :math:`\mathbf{M}` is a joint selection mask matrix + + 2. **Jacobian Matrix**: The task Jacobian is the null space projector: + + .. math:: + + \mathbf{J}_{\text{posture}}(\mathbf{q}) = \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}} + + where: + - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all higher priority tasks + - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian + - :math:`\mathbf{N}(\mathbf{q})` is the null space projector matrix + + For example, if there are two frame tasks (e.g., controlling the pose of two end-effectors), the combined Jacobian + :math:`\mathbf{J}_{\text{primary}}` is constructed by stacking the individual Jacobians for each frame vertically: + + .. math:: + + \mathbf{J}_{\text{primary}} = + \begin{bmatrix} + \mathbf{J}_1(\mathbf{q}) \\ + \mathbf{J}_2(\mathbf{q}) + \end{bmatrix} + + where :math:`\mathbf{J}_1(\mathbf{q})` and :math:`\mathbf{J}_2(\mathbf{q})` are the Jacobians for the first and second frame tasks, respectively. + + The null space projector ensures that joint velocities in the null space produce zero velocity + for the primary tasks: :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`. + + **Task Integration:** + + When integrated into the Pink framework, this task contributes to the optimization as: + + .. math:: + + \left\| \mathbf{N}(\mathbf{q}) \mathbf{v} + \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) \right\|_{W_{\text{posture}}}^2 + + This formulation allows the robot to maintain a desired posture while respecting the constraints + imposed by higher priority tasks (e.g., end-effector positioning). """ @@ -57,63 +96,68 @@ def __init__( cost: float, lm_damping: float = 0.0, gain: float = 1.0, - frame_task_controlled_joints: dict[str, list[str]] | None = None, + controlled_frames: list[str] | None = None, + controlled_joints: list[str] | None = None, ) -> None: - r"""Create task. + r"""Create a null space posture task. Args: - cost: value used to cast joint angle differences to a homogeneous - cost, in :math:`[\mathrm{cost}] / [\mathrm{rad}]`. - lm_damping: Unitless scale of the Levenberg-Marquardt (only when - the error is large) regularization term, which helps when - targets are infeasible. Increase this value if the task is too - jerky under infeasible targets, but beware that too large a - damping can slow down the task. - gain: Task gain :math:`\alpha \in [0, 1]` for additional low-pass - filtering. Defaults to 1.0 (no filtering). - frame_task_controlled_joints: Dictionary of frame link names to controlled joint names. + cost: Weighting factor for the posture task in the optimization objective, + in :math:`[\mathrm{cost}] / [\mathrm{rad}]`. + lm_damping: Unitless scale of the Levenberg-Marquardt regularization term. + Increase if the task is too jerky under infeasible targets. + gain: Task gain :math:`\alpha \in [0, 1]` for low-pass filtering. + Defaults to 1.0 (no filtering). + controlled_frames: List of frame names whose Jacobians define the primary tasks + for null space projection. If None or empty, no null space projection is applied. + controlled_joints: List of joint names to control in the posture task. + If None or empty, all actuated joints are controlled. """ super().__init__(cost=cost, gain=gain, lm_damping=lm_damping) self.target_q: np.ndarray | None = None - self.frame_task_controlled_joints: dict[str, list[str]] = frame_task_controlled_joints or {} - self._joint_name_to_index: dict[str, int] | None = None - self._selected_joint_indices: list[np.ndarray] | None = None - self._jacobian_dim: int = 0 + self.controlled_frames: list[str] = controlled_frames or [] + self.controlled_joints: list[str] = controlled_joints or [] + self._joint_mask: np.ndarray | None = None + self._frame_names: list[str] | None = None def __repr__(self) -> str: """Human-readable representation of the task.""" return ( f"NullSpacePostureTask(cost={self.cost}, gain={self.gain}, lm_damping={self.lm_damping}," - f" frame_task_controlled_joints={self.frame_task_controlled_joints})" + f" controlled_frames={self.controlled_frames}, controlled_joints={self.controlled_joints})" ) def _build_joint_mapping(self, configuration: Configuration) -> None: - """Build efficient joint name to index mapping. + """Build joint mask and cache frequently used values. + + Creates a binary mask that selects which joints should be controlled + in the posture task. Args: - configuration: Robot configuration. + configuration: Robot configuration containing the model and joint information. """ - if self._joint_name_to_index is not None: - return - - # Create O(1) lookup dictionary for joint names - joint_names = configuration.model.names.tolist()[1:] # Skip root joint - self._joint_name_to_index = {name: idx for idx, name in enumerate(joint_names)} - - # Build selected joint indices efficiently - self._selected_joint_indices = [] - for controlled_joints in self.frame_task_controlled_joints.values(): - indices = [self._joint_name_to_index[joint] for joint in controlled_joints] - self._selected_joint_indices.append(np.array(sorted(indices), dtype=int)) + # Create joint mask for full configuration size + self._joint_mask = np.zeros(configuration.model.nq) + + # Create dictionary for joint names to indices (exclude root joint) + joint_names = configuration.model.names.tolist()[1:] + + # Build joint mask efficiently + for i, joint_name in enumerate(joint_names): + if joint_name in self.controlled_joints: + self._joint_mask[i] = 1.0 + + # Cache frame names for performance + self._frame_names = list(self.controlled_frames) def set_target(self, target_q: np.ndarray) -> None: - """Set target posture. + """Set target posture configuration. Args: target_q: Target vector in the configuration space. If the model has a floating base, then this vector should include floating-base coordinates (although they have no effect on the - posture task). + posture task since only actuated joints are controlled). """ self.target_q = target_q.copy() @@ -121,26 +165,29 @@ def set_target_from_configuration(self, configuration: Configuration) -> None: """Set target posture from a robot configuration. Args: - configuration: Robot configuration. + configuration: Robot configuration whose joint angles will be used + as the target posture. """ self.set_target(configuration.q) def compute_error(self, configuration: Configuration) -> np.ndarray: r"""Compute posture task error. - This method computes the posture error for the null space posture task. - The error is the difference between the target and current configuration, - excluding the floating base coordinates (if present). + The error computation follows: + + .. math:: - Note: - The actual null space projection is applied in the Jacobian (see `compute_jacobian`). - This function only returns the configuration difference in the actuated joint space. + \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) + + where :math:`\mathbf{M}` is the joint selection mask and :math:`\mathbf{q}^* - \mathbf{q}` + is computed using Pinocchio's difference function to handle joint angle wrapping. Args: - configuration: Robot configuration :math:`q`. + configuration: Robot configuration :math:`\mathbf{q}`. Returns: - Posture task error :math:`e(q) = q^* - q`, where only actuated joints are considered. + Posture task error :math:`\mathbf{e}(\mathbf{q})` with the same dimension + as the configuration vector, but with zeros for non-controlled joints. Raises: ValueError: If no posture target has been set. @@ -148,72 +195,60 @@ def compute_error(self, configuration: Configuration) -> np.ndarray: if self.target_q is None: raise ValueError("No posture target has been set. Call set_target() first.") - _, root_nv = get_root_joint_dim(configuration.model) + # Initialize joint mapping if needed + if self._joint_mask is None: + self._build_joint_mapping(configuration) - # Compute configuration difference - return pin.difference( + # Compute configuration difference using Pinocchio's difference function + # This handles joint angle wrapping correctly + err = pin.difference( configuration.model, self.target_q, configuration.q, - )[root_nv:] + ) + + # Apply pre-computed joint mask to select only controlled joints + return self._joint_mask * err def compute_jacobian(self, configuration: Configuration) -> np.ndarray: - r"""Computes the product of left and right null space projectors - :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse of the - corresponding end-effector Jacobian. The null space projector ensures - joint velocities in the null space produce zero end-effector velocity - (:math:`J \cdot \dot{q}_{\text{null}} = 0`), allowing posture control - without affecting end-effector motion. - - The final Jacobian returned is: - :math:`J(q) = N_{\text{task_1}}(q) * N_{\text{task_2}}(q)` - - Args: - configuration: Robot configuration :math:`q`. - - Returns: - Posture task Jacobian :math:`J(q)`, which is the sum of the left and right - null space projectors, each masked to their respective controlled joints. - """ - # Initialize joint mapping if needed - if self._joint_name_to_index is None: - self._build_joint_mapping(configuration) + r"""Compute the null space projector Jacobian. - # Early return if no frame tasks defined - if not self.frame_task_controlled_joints: - print("No frame tasks defined") - # Return identity matrix if no frame tasks defined - n_joints = len(configuration.model.names) - 1 # Exclude root joint - return np.eye(n_joints) + The null space projector is defined as: - # Get frame names - frame_names = list(self.frame_task_controlled_joints.keys()) + .. math:: - # Get Jacobians for all frame tasks - J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in frame_names] + \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}} - # Cache Jacobian dimension - if self._jacobian_dim == 0: - self._jacobian_dim = J_frame_tasks[0].shape[1] + where: + - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all controlled frames + - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian + - :math:`\mathbf{I}` is the identity matrix - # Initialize null space task matrix - null_space_task = np.eye(self._jacobian_dim) + The null space projector ensures that joint velocities in the null space produce + zero velocity for the primary tasks: :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`. - # Create mask for all controlled joints - mask = np.zeros(self._jacobian_dim, dtype=bool) - for selected_indices in self._selected_joint_indices or []: - mask[selected_indices] = True + If no controlled frames are specified, returns the identity matrix. + + Args: + configuration: Robot configuration :math:`\mathbf{q}`. - # Compute null space projectors efficiently - for J_frame_task, selected_indices in zip(J_frame_tasks, self._selected_joint_indices or []): - # Compute pseudoinverse and null space projector - J_pinv = np.linalg.pinv(J_frame_task) - null_space_full = np.eye(J_frame_task.shape[1]) - J_pinv @ J_frame_task + Returns: + Null space projector matrix :math:`\mathbf{N}(\mathbf{q})` with dimensions + :math:`n_q \times n_q` where :math:`n_q` is the number of configuration variables. + """ + # Initialize joint mapping if needed + if self._frame_names is None: + self._build_joint_mapping(configuration) - # Only update rows corresponding to selected joints - null_space_task @= null_space_full + # If no frame tasks are defined, return identity matrix (no null space projection) + if not self._frame_names: + return np.eye(configuration.model.nq) - # Apply mask to the final result - null_space_task = null_space_task * mask[:, np.newaxis] + # Get Jacobians for all frame tasks and combine them + J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in self._frame_names] + J_combined = np.concatenate(J_frame_tasks, axis=0) + + # Compute null space projector: N = I - J^+ * J + N_combined = np.eye(J_combined.shape[1]) - np.linalg.pinv(J_combined) @ J_combined - return null_space_task + return N_combined diff --git a/source/isaaclab/test/controllers/simplified_test_robot.urdf b/source/isaaclab/test/controllers/simplified_test_robot.urdf new file mode 100644 index 000000000000..b66ce68324bb --- /dev/null +++ b/source/isaaclab/test/controllers/simplified_test_robot.urdf @@ -0,0 +1,191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab/test/controllers/test_null_space_posture_task.py b/source/isaaclab/test/controllers/test_null_space_posture_task.py new file mode 100644 index 000000000000..331691c7ca05 --- /dev/null +++ b/source/isaaclab/test/controllers/test_null_space_posture_task.py @@ -0,0 +1,331 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""Launch Isaac Sim Simulator first.""" +# Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim +# pinocchio is required by the Pink IK controller +import sys + +if sys.platform != "win32": + import pinocchio # noqa: F401 + import pinocchio as pin # noqa: F401 +else: + import pinocchio # noqa: F401 + import pinocchio as pin # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Unit tests for NullSpacePostureTask with simplified robot configuration using Pink library directly.""" + +import numpy as np +import pytest + +from pinocchio.robot_wrapper import RobotWrapper +from pink.configuration import Configuration +from pink.tasks import FrameTask +from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask + + +class TestNullSpacePostureTaskSimplifiedRobot: + """Test cases for NullSpacePostureTask with simplified robot configuration.""" + + @pytest.fixture + def num_joints(self): + """Number of joints in the simplified robot.""" + return 20 + + @pytest.fixture + def joint_configurations(self, num_joints): + """Pre-generated joint configurations for testing.""" + return { + "random_1": np.array([0.1, 0.2, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4]), + "random_2": np.array([0.5, -0.3, 0.2, -0.4, 0.1, 0.2, -0.1, 0.3, 0.2, 0.1, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4]), + "random_3": np.array([0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1]), + "controlled_only": np.array([0.5, 0.5, 0.5, 0.5, 0.5] + [0.0] * 15), # Non-zero for controlled joints only + "mixed": np.array([-0.3, 0.2, -0.4, 0.1, 0.5] + [0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3]), + "sequential": np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2.0]), + } + + @pytest.fixture + def robot_urdf(self): + """Load the simplified test robot URDF file.""" + import os + current_dir = os.path.dirname(os.path.abspath(__file__)) + urdf_path = os.path.join(current_dir, "simplified_test_robot.urdf") + return urdf_path + + @pytest.fixture + def robot_configuration(self, robot_urdf): + """Simplified robot wrapper.""" + wrapper = RobotWrapper.BuildFromURDF(robot_urdf, None, root_joint=None) + return Configuration(wrapper.model, wrapper.data, wrapper.q0) + + @pytest.fixture + def tasks(self): + """pink tasks.""" + return [ + FrameTask("left_hand_pitch_link", position_cost=1.0, orientation_cost=1.0), + NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=[ + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + ] + ) + ] + + def test_null_space_jacobian_zero_end_effector_velocity(self, robot_configuration, tasks, joint_configurations, num_joints): + """Test that velocities projected through null space Jacobian result in zero end-effector velocity.""" + # Set specific joint configuration + robot_configuration.q = joint_configurations["random_1"] + + # Set frame task target to a specific position in workspace + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.5, 0.3, 0.8]) # x, y, z + quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + # Get the null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Get the end-effector Jacobian + frame_task_jacobian = frame_task.compute_jacobian(robot_configuration) + + # Test multiple random velocities in null space + for _ in range(10): + # Generate random joint velocity + random_velocity = np.random.randn(num_joints) * 0.1 + + # Project through null space Jacobian + null_space_velocity = null_space_jacobian @ random_velocity + + # Compute resulting end-effector velocity + ee_velocity = frame_task_jacobian @ null_space_velocity + + # The end-effector velocity should be approximately zero + assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), \ + f"End-effector velocity not zero: {ee_velocity}" + + def test_null_space_jacobian_properties(self, robot_configuration, tasks, joint_configurations, num_joints): + """Test mathematical properties of the null space Jacobian.""" + # Set specific joint configuration + robot_configuration.q = joint_configurations["random_2"] + + # Set frame task target + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.3, 0.4, 0.6]) + quaternion = pin.Quaternion(0.707, 0.0, 0.0, 0.707) # w, x, y, z (90-degree rotation around X) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + target_posture[0:5] = [0.1, -0.1, 0.2, -0.2, 0.0] # Set first 5 joints (controlled joints) + null_space_task.set_target(target_posture) + + # Get Jacobians + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + + # Test: N * J^T should be approximately zero (null space property) + # where N is the null space projector and J is the end-effector Jacobian + null_space_projection = null_space_jacobian @ ee_jacobian.T + assert np.allclose(null_space_projection, np.zeros_like(null_space_projection), atol=1e-7), \ + f"Null space projection of end-effector Jacobian not zero: {null_space_projection}" + + def test_null_space_jacobian_identity_when_no_frame_tasks(self, robot_configuration, joint_configurations, num_joints): + """Test that null space Jacobian is identity when no frame tasks are defined.""" + # Create null space task without frame task controlled joints + null_space_task = NullSpacePostureTask(cost=1.0, controlled_frames=[], controlled_joints=[]) + + # Set specific joint configuration + robot_configuration.q = joint_configurations["random_3"] + + # Set target + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + # Get the null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Should be identity matrix + expected_identity = np.eye(num_joints) + assert np.allclose(null_space_jacobian, expected_identity), \ + f"Null space Jacobian should be identity when no frame tasks defined: {null_space_jacobian}" + + def test_null_space_jacobian_consistency_across_configurations(self, robot_configuration, tasks, joint_configurations, num_joints): + """Test that null space Jacobian is consistent across different joint configurations.""" + # Test multiple joint configurations + test_configs = [ + np.zeros(num_joints), # Zero configuration + joint_configurations["controlled_only"], # Non-zero for controlled joints + joint_configurations["mixed"], # Mixed configuration + ] + + # Set frame task target + frame_task = tasks[0] + # Create pin.SE3 from position and quaternion + position = np.array([0.3, 0.3, 0.5]) + quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) + target_pose = pin.SE3(quaternion, position) + frame_task.set_target(target_pose) + + # Set null space posture task target + null_space_task = tasks[1] + target_posture = np.zeros(num_joints) + null_space_task.set_target(target_posture) + + jacobians = [] + for config in test_configs: + robot_configuration.q = config + jacobian = null_space_task.compute_jacobian(robot_configuration) + jacobians.append(jacobian) + + # Verify that velocities through this Jacobian result in zero end-effector velocity + ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + + # Test with random velocity + random_velocity = np.random.randn(num_joints) * 0.1 + null_space_velocity = jacobian @ random_velocity + ee_velocity = ee_jacobian @ null_space_velocity + + assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), \ + f"End-effector velocity not zero for configuration {config}: {ee_velocity}" + + def test_compute_error_without_target(self, robot_configuration, joint_configurations): + """Test that compute_error raises ValueError when no target is set.""" + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"] + ) + + robot_configuration.q = joint_configurations["sequential"] + + # Should raise ValueError when no target is set + with pytest.raises(ValueError, match="No posture target has been set"): + null_space_task.compute_error(robot_configuration) + + def test_joint_masking(self, robot_configuration, joint_configurations, num_joints): + """Test that joint mask correctly filters only controlled joints.""" + + controlled_joint_names = ["waist_pitch_joint", "left_shoulder_pitch_joint", "left_elbow_pitch_joint"] + + # Create task with specific controlled joints + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=controlled_joint_names + ) + + # Find the joint indexes in robot_configuration.model.names.tolist()[1:] + joint_names = robot_configuration.model.names.tolist()[1:] + joint_indexes = [joint_names.index(jn) for jn in controlled_joint_names] + + # Set configurations + current_config = joint_configurations["sequential"] + target_config = np.zeros(num_joints) + + robot_configuration.q = current_config + null_space_task.set_target(target_config) + + # Compute error + error = null_space_task.compute_error(robot_configuration) + + # Only controlled joints should have non-zero error + # Joint indices: waist_yaw_joint=0, waist_pitch_joint=1, waist_roll_joint=2, left_shoulder_pitch_joint=3, left_shoulder_roll_joint=4, etc. + expected_error = np.zeros(num_joints) + for i in joint_indexes: + expected_error[i] = current_config[i] + + assert np.allclose(error, expected_error, atol=1e-7), \ + f"Joint mask not working correctly: expected {expected_error}, got {error}" + + def test_empty_controlled_joints(self, robot_configuration, joint_configurations, num_joints): + """Test behavior when controlled_joints is empty.""" + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=[] + ) + + current_config = joint_configurations["sequential"] + target_config = np.zeros(num_joints) + + robot_configuration.q = current_config + null_space_task.set_target(target_config) + + # Error should be all zeros + error = null_space_task.compute_error(robot_configuration) + expected_error = np.zeros(num_joints) + assert np.allclose(error, expected_error), \ + f"Error should be zero when no joints controlled: {error}" + + def test_set_target_from_configuration(self, robot_configuration, joint_configurations): + """Test set_target_from_configuration method.""" + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"] + ) + + # Set a specific configuration + test_config = joint_configurations["sequential"] + robot_configuration.q = test_config + + # Set target from configuration + null_space_task.set_target_from_configuration(robot_configuration) + + # Verify target was set correctly + assert null_space_task.target_q is not None + assert np.allclose(null_space_task.target_q, test_config) + + def test_multiple_frame_tasks(self, robot_configuration, joint_configurations, num_joints): + """Test null space projection with multiple frame tasks.""" + # Create task with multiple controlled frames + null_space_task = NullSpacePostureTask( + cost=1.0, + controlled_frames=["left_hand_pitch_link", "right_hand_pitch_link"], + controlled_joints=["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"] + ) + + current_config = joint_configurations["sequential"] + robot_configuration.q = current_config + + # Get null space Jacobian + null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) + + # Get Jacobians for both frames + jacobian_left_hand = robot_configuration.get_frame_jacobian("left_hand_pitch_link") + jacobian_right_hand = robot_configuration.get_frame_jacobian("right_hand_pitch_link") + + # Test that null space velocities result in zero velocity for both frames + for _ in range(5): + random_velocity = np.random.randn(num_joints) * 0.1 + null_space_velocity = null_space_jacobian @ random_velocity + + # Check both frames + ee_velocity_left = jacobian_left_hand @ null_space_velocity + ee_velocity_right = jacobian_right_hand @ null_space_velocity + + assert np.allclose(ee_velocity_left, np.zeros(6), atol=1e-7), \ + f"Left hand velocity not zero: {ee_velocity_left}" + assert np.allclose(ee_velocity_right, np.zeros(6), atol=1e-7), \ + f"Right hand velocity not zero: {ee_velocity_right}" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index 24e4255258cf..8b35bf2c3cb9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -144,30 +144,23 @@ def __post_init__(self): NullSpacePostureTask( cost=0.2, lm_damping=1, - frame_task_controlled_joints={ - "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_wrist_yaw_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_wrist_yaw_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - }, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], ), ], fixed_input_tasks=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index 0f6b66731f5b..d18b4866d155 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -142,30 +142,23 @@ def __post_init__(self): NullSpacePostureTask( cost=0.2, lm_damping=1, - frame_task_controlled_joints={ - "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_wrist_yaw_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_wrist_yaw_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - }, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], ), ], fixed_input_tasks=[ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index ed40f4b2b6eb..4d0871fcb8a0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -233,26 +233,25 @@ class ActionsCfg: cost=0.5, # [cost] * [s] / [rad] ), NullSpacePostureTask( - cost=0.2, + cost=0.5, lm_damping=1, - frame_task_controlled_joints={ - "GR1T2_fourier_hand_6dof_left_hand_pitch_link": [ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - "GR1T2_fourier_hand_6dof_right_hand_pitch_link": [ - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], - }, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], ), ], fixed_input_tasks=[], From 3d31865a7599e09ea8038b35a5f35af2e250d1a8 Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Thu, 21 Aug 2025 19:54:03 -0700 Subject: [PATCH 09/12] fixing linting. Also, reducing pink unit test accuracy requirement to 1 mm. --- docs/index.rst | 2 +- .../pink_ik/null_space_posture_task.py | 11 +- .../pink_ik_gr1_test_configs.json | 7 +- .../test_null_space_posture_task.py | 176 +++++++++--------- 4 files changed, 101 insertions(+), 95 deletions(-) diff --git a/docs/index.rst b/docs/index.rst index d5e85bc8cab5..ffd94c35857e 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -46,7 +46,7 @@ For more information about the framework, please refer to the `paper None: """ # Create joint mask for full configuration size self._joint_mask = np.zeros(configuration.model.nq) - + # Create dictionary for joint names to indices (exclude root joint) joint_names = configuration.model.names.tolist()[1:] - + # Build joint mask efficiently for i, joint_name in enumerate(joint_names): if joint_name in self.controlled_joints: @@ -206,7 +205,7 @@ def compute_error(self, configuration: Configuration) -> np.ndarray: self.target_q, configuration.q, ) - + # Apply pre-computed joint mask to select only controlled joints return self._joint_mask * err @@ -247,7 +246,7 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray: # Get Jacobians for all frame tasks and combine them J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in self._frame_names] J_combined = np.concatenate(J_frame_tasks, axis=0) - + # Compute null space projector: N = I - J^+ * J N_combined = np.eye(J_combined.shape[1]) - np.linalg.pinv(J_combined) @ J_combined diff --git a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json index 9d28a942316a..b033b95b81f6 100644 --- a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json +++ b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json @@ -1,11 +1,10 @@ { "tolerances": { - "position": 0.002, + "position": 0.001, "pd_position": 0.001, "rotation": 0.02, "check_errors": true }, - "num_iterations": 12, "tests": { "stay_still": { "left_hand_pose": [ @@ -64,7 +63,7 @@ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 70, + "allowed_steps_per_motion": 30, "repeat": 3 }, "rotation_movements": { @@ -80,7 +79,7 @@ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0] ], - "allowed_steps_per_motion": 25, + "allowed_steps_per_motion": 20, "repeat": 2 } } diff --git a/source/isaaclab/test/controllers/test_null_space_posture_task.py b/source/isaaclab/test/controllers/test_null_space_posture_task.py index 331691c7ca05..97fc7221748a 100644 --- a/source/isaaclab/test/controllers/test_null_space_posture_task.py +++ b/source/isaaclab/test/controllers/test_null_space_posture_task.py @@ -22,11 +22,12 @@ """Unit tests for NullSpacePostureTask with simplified robot configuration using Pink library directly.""" import numpy as np -import pytest -from pinocchio.robot_wrapper import RobotWrapper +import pytest from pink.configuration import Configuration from pink.tasks import FrameTask +from pinocchio.robot_wrapper import RobotWrapper + from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask @@ -39,21 +40,22 @@ def num_joints(self): return 20 @pytest.fixture - def joint_configurations(self, num_joints): + def joint_configurations(self): """Pre-generated joint configurations for testing.""" + # Set random seed for reproducible tests + np.random.seed(42) + return { - "random_1": np.array([0.1, 0.2, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4]), - "random_2": np.array([0.5, -0.3, 0.2, -0.4, 0.1, 0.2, -0.1, 0.3, 0.2, 0.1, -0.3, 0.4, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.4]), - "random_3": np.array([0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1]), - "controlled_only": np.array([0.5, 0.5, 0.5, 0.5, 0.5] + [0.0] * 15), # Non-zero for controlled joints only - "mixed": np.array([-0.3, 0.2, -0.4, 0.1, 0.5] + [0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1, 0.2, 0.1, -0.3]), - "sequential": np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2.0]), + "random": np.random.uniform(-0.5, 0.5, 20), + "controlled_only": np.array([0.5] * 5 + [0.0] * 15), # Non-zero for controlled joints only + "sequential": np.linspace(0.1, 2.0, 20), } @pytest.fixture def robot_urdf(self): """Load the simplified test robot URDF file.""" import os + current_dir = os.path.dirname(os.path.abspath(__file__)) urdf_path = os.path.join(current_dir, "simplified_test_robot.urdf") return urdf_path @@ -70,7 +72,7 @@ def tasks(self): return [ FrameTask("left_hand_pitch_link", position_cost=1.0, orientation_cost=1.0), NullSpacePostureTask( - cost=1.0, + cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=[ "waist_yaw_joint", @@ -78,15 +80,17 @@ def tasks(self): "waist_roll_joint", "left_shoulder_pitch_joint", "left_shoulder_roll_joint", - ] - ) + ], + ), ] - def test_null_space_jacobian_zero_end_effector_velocity(self, robot_configuration, tasks, joint_configurations, num_joints): + def test_null_space_jacobian_zero_end_effector_velocity( + self, robot_configuration, tasks, joint_configurations, num_joints + ): """Test that velocities projected through null space Jacobian result in zero end-effector velocity.""" # Set specific joint configuration - robot_configuration.q = joint_configurations["random_1"] - + robot_configuration.q = joint_configurations["random"] + # Set frame task target to a specific position in workspace frame_task = tasks[0] # Create pin.SE3 from position and quaternion @@ -94,15 +98,15 @@ def test_null_space_jacobian_zero_end_effector_velocity(self, robot_configuratio quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) target_pose = pin.SE3(quaternion, position) frame_task.set_target(target_pose) - + # Set null space posture task target null_space_task = tasks[1] target_posture = np.zeros(num_joints) null_space_task.set_target(target_posture) - + # Get the null space Jacobian null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) - + # Get the end-effector Jacobian frame_task_jacobian = frame_task.compute_jacobian(robot_configuration) @@ -110,22 +114,21 @@ def test_null_space_jacobian_zero_end_effector_velocity(self, robot_configuratio for _ in range(10): # Generate random joint velocity random_velocity = np.random.randn(num_joints) * 0.1 - + # Project through null space Jacobian null_space_velocity = null_space_jacobian @ random_velocity - + # Compute resulting end-effector velocity ee_velocity = frame_task_jacobian @ null_space_velocity - + # The end-effector velocity should be approximately zero - assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), \ - f"End-effector velocity not zero: {ee_velocity}" + assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), f"End-effector velocity not zero: {ee_velocity}" def test_null_space_jacobian_properties(self, robot_configuration, tasks, joint_configurations, num_joints): """Test mathematical properties of the null space Jacobian.""" # Set specific joint configuration - robot_configuration.q = joint_configurations["random_2"] - + robot_configuration.q = joint_configurations["random"] + # Set frame task target frame_task = tasks[0] # Create pin.SE3 from position and quaternion @@ -133,52 +136,58 @@ def test_null_space_jacobian_properties(self, robot_configuration, tasks, joint_ quaternion = pin.Quaternion(0.707, 0.0, 0.0, 0.707) # w, x, y, z (90-degree rotation around X) target_pose = pin.SE3(quaternion, position) frame_task.set_target(target_pose) - + # Set null space posture task target null_space_task = tasks[1] target_posture = np.zeros(num_joints) target_posture[0:5] = [0.1, -0.1, 0.2, -0.2, 0.0] # Set first 5 joints (controlled joints) null_space_task.set_target(target_posture) - + # Get Jacobians null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") - + # Test: N * J^T should be approximately zero (null space property) # where N is the null space projector and J is the end-effector Jacobian null_space_projection = null_space_jacobian @ ee_jacobian.T - assert np.allclose(null_space_projection, np.zeros_like(null_space_projection), atol=1e-7), \ - f"Null space projection of end-effector Jacobian not zero: {null_space_projection}" + assert np.allclose( + null_space_projection, np.zeros_like(null_space_projection), atol=1e-7 + ), f"Null space projection of end-effector Jacobian not zero: {null_space_projection}" - def test_null_space_jacobian_identity_when_no_frame_tasks(self, robot_configuration, joint_configurations, num_joints): + def test_null_space_jacobian_identity_when_no_frame_tasks( + self, robot_configuration, joint_configurations, num_joints + ): """Test that null space Jacobian is identity when no frame tasks are defined.""" # Create null space task without frame task controlled joints null_space_task = NullSpacePostureTask(cost=1.0, controlled_frames=[], controlled_joints=[]) - + # Set specific joint configuration - robot_configuration.q = joint_configurations["random_3"] - + robot_configuration.q = joint_configurations["sequential"] + # Set target target_posture = np.zeros(num_joints) null_space_task.set_target(target_posture) - + # Get the null space Jacobian null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) - + # Should be identity matrix expected_identity = np.eye(num_joints) - assert np.allclose(null_space_jacobian, expected_identity), \ - f"Null space Jacobian should be identity when no frame tasks defined: {null_space_jacobian}" + assert np.allclose( + null_space_jacobian, expected_identity + ), f"Null space Jacobian should be identity when no frame tasks defined: {null_space_jacobian}" - def test_null_space_jacobian_consistency_across_configurations(self, robot_configuration, tasks, joint_configurations, num_joints): + def test_null_space_jacobian_consistency_across_configurations( + self, robot_configuration, tasks, joint_configurations, num_joints + ): """Test that null space Jacobian is consistent across different joint configurations.""" # Test multiple joint configurations test_configs = [ np.zeros(num_joints), # Zero configuration joint_configurations["controlled_only"], # Non-zero for controlled joints - joint_configurations["mixed"], # Mixed configuration + joint_configurations["random"], # Random configuration ] - + # Set frame task target frame_task = tasks[0] # Create pin.SE3 from position and quaternion @@ -186,39 +195,40 @@ def test_null_space_jacobian_consistency_across_configurations(self, robot_confi quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0) # w, x, y, z (identity quaternion) target_pose = pin.SE3(quaternion, position) frame_task.set_target(target_pose) - + # Set null space posture task target null_space_task = tasks[1] target_posture = np.zeros(num_joints) null_space_task.set_target(target_posture) - + jacobians = [] for config in test_configs: robot_configuration.q = config jacobian = null_space_task.compute_jacobian(robot_configuration) jacobians.append(jacobian) - + # Verify that velocities through this Jacobian result in zero end-effector velocity ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link") - + # Test with random velocity random_velocity = np.random.randn(num_joints) * 0.1 null_space_velocity = jacobian @ random_velocity ee_velocity = ee_jacobian @ null_space_velocity - - assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), \ - f"End-effector velocity not zero for configuration {config}: {ee_velocity}" + + assert np.allclose( + ee_velocity, np.zeros(6), atol=1e-7 + ), f"End-effector velocity not zero for configuration {config}: {ee_velocity}" def test_compute_error_without_target(self, robot_configuration, joint_configurations): """Test that compute_error raises ValueError when no target is set.""" null_space_task = NullSpacePostureTask( - cost=1.0, + cost=1.0, controlled_frames=["left_hand_pitch_link"], - controlled_joints=["waist_yaw_joint", "waist_pitch_joint"] + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"], ) - + robot_configuration.q = joint_configurations["sequential"] - + # Should raise ValueError when no target is set with pytest.raises(ValueError, match="No posture target has been set"): null_space_task.compute_error(robot_configuration) @@ -230,69 +240,65 @@ def test_joint_masking(self, robot_configuration, joint_configurations, num_join # Create task with specific controlled joints null_space_task = NullSpacePostureTask( - cost=1.0, - controlled_frames=["left_hand_pitch_link"], - controlled_joints=controlled_joint_names + cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=controlled_joint_names ) # Find the joint indexes in robot_configuration.model.names.tolist()[1:] joint_names = robot_configuration.model.names.tolist()[1:] joint_indexes = [joint_names.index(jn) for jn in controlled_joint_names] - + # Set configurations current_config = joint_configurations["sequential"] target_config = np.zeros(num_joints) - + robot_configuration.q = current_config null_space_task.set_target(target_config) - + # Compute error error = null_space_task.compute_error(robot_configuration) - + # Only controlled joints should have non-zero error # Joint indices: waist_yaw_joint=0, waist_pitch_joint=1, waist_roll_joint=2, left_shoulder_pitch_joint=3, left_shoulder_roll_joint=4, etc. expected_error = np.zeros(num_joints) for i in joint_indexes: expected_error[i] = current_config[i] - assert np.allclose(error, expected_error, atol=1e-7), \ - f"Joint mask not working correctly: expected {expected_error}, got {error}" + assert np.allclose( + error, expected_error, atol=1e-7 + ), f"Joint mask not working correctly: expected {expected_error}, got {error}" def test_empty_controlled_joints(self, robot_configuration, joint_configurations, num_joints): """Test behavior when controlled_joints is empty.""" null_space_task = NullSpacePostureTask( - cost=1.0, - controlled_frames=["left_hand_pitch_link"], - controlled_joints=[] + cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=[] ) - + current_config = joint_configurations["sequential"] target_config = np.zeros(num_joints) - + robot_configuration.q = current_config null_space_task.set_target(target_config) - + # Error should be all zeros error = null_space_task.compute_error(robot_configuration) expected_error = np.zeros(num_joints) - assert np.allclose(error, expected_error), \ - f"Error should be zero when no joints controlled: {error}" + assert np.allclose(error, expected_error), f"Error should be zero when no joints controlled: {error}" def test_set_target_from_configuration(self, robot_configuration, joint_configurations): """Test set_target_from_configuration method.""" null_space_task = NullSpacePostureTask( cost=1.0, controlled_frames=["left_hand_pitch_link"], - controlled_joints=["waist_yaw_joint", "waist_pitch_joint"] + controlled_joints=["waist_yaw_joint", "waist_pitch_joint"], ) - + # Set a specific configuration test_config = joint_configurations["sequential"] robot_configuration.q = test_config - + # Set target from configuration null_space_task.set_target_from_configuration(robot_configuration) - + # Verify target was set correctly assert null_space_task.target_q is not None assert np.allclose(null_space_task.target_q, test_config) @@ -303,29 +309,31 @@ def test_multiple_frame_tasks(self, robot_configuration, joint_configurations, n null_space_task = NullSpacePostureTask( cost=1.0, controlled_frames=["left_hand_pitch_link", "right_hand_pitch_link"], - controlled_joints=["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"] + controlled_joints=["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"], ) - + current_config = joint_configurations["sequential"] robot_configuration.q = current_config - + # Get null space Jacobian null_space_jacobian = null_space_task.compute_jacobian(robot_configuration) - + # Get Jacobians for both frames jacobian_left_hand = robot_configuration.get_frame_jacobian("left_hand_pitch_link") jacobian_right_hand = robot_configuration.get_frame_jacobian("right_hand_pitch_link") - + # Test that null space velocities result in zero velocity for both frames for _ in range(5): random_velocity = np.random.randn(num_joints) * 0.1 null_space_velocity = null_space_jacobian @ random_velocity - + # Check both frames ee_velocity_left = jacobian_left_hand @ null_space_velocity ee_velocity_right = jacobian_right_hand @ null_space_velocity - - assert np.allclose(ee_velocity_left, np.zeros(6), atol=1e-7), \ - f"Left hand velocity not zero: {ee_velocity_left}" - assert np.allclose(ee_velocity_right, np.zeros(6), atol=1e-7), \ - f"Right hand velocity not zero: {ee_velocity_right}" + + assert np.allclose( + ee_velocity_left, np.zeros(6), atol=1e-7 + ), f"Left hand velocity not zero: {ee_velocity_left}" + assert np.allclose( + ee_velocity_right, np.zeros(6), atol=1e-7 + ), f"Right hand velocity not zero: {ee_velocity_right}" From 2ea0e0da5368be011e82a3169f7c9772610bdc20 Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Mon, 25 Aug 2025 16:55:40 -0700 Subject: [PATCH 10/12] Updating documentations to include Null Space Posture Task --- docs/source/api/lab/isaaclab.controllers.rst | 15 +++---- source/isaaclab/docs/CHANGELOG.rst | 2 +- .../pink_ik/null_space_posture_task.py | 41 +++++++------------ source/isaaclab_tasks/docs/CHANGELOG.rst | 2 +- 4 files changed, 23 insertions(+), 37 deletions(-) diff --git a/docs/source/api/lab/isaaclab.controllers.rst b/docs/source/api/lab/isaaclab.controllers.rst index fd476c9e27b0..24bfa0b7f836 100644 --- a/docs/source/api/lab/isaaclab.controllers.rst +++ b/docs/source/api/lab/isaaclab.controllers.rst @@ -13,6 +13,7 @@ OperationalSpaceControllerCfg PinkIKController PinkIKControllerCfg + pink_ik.NullSpacePostureTask Differential Inverse Kinematics ------------------------------- @@ -45,13 +46,9 @@ Operational Space controllers Differential Inverse Kinematics Controllers (Based on Pink) ----------------------------------------------------------- -.. autoclass:: isaaclab.controllers.pink_ik.PinkIKController - :members: - :inherited-members: - :show-inheritance: +For detailed documentation of Pink IK controllers and tasks, see: -.. autoclass:: isaaclab.controllers.pink_ik.PinkIKControllerCfg - :members: - :inherited-members: - :show-inheritance: - :exclude-members: __init__, class_type +.. toctree:: + :maxdepth: 1 + + isaaclab.controllers.pink_ik diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index ac82933a757f..ee1f3590896b 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -11,7 +11,7 @@ Added to specify the target end-effector link names for the pink inverse kinematics controller. Changed -^^^^^ +^^^^^^^ * Updated pink inverse kinematics controller configuration for the following tasks (Isaac-PickPlace-GR1T2, Isaac-NutPour-GR1T2, Isaac-ExhaustPipe-GR1T2) to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom. diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py index 58a9b3d7b916..212071c904e8 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py @@ -11,29 +11,14 @@ class NullSpacePostureTask(Task): - r"""Pink-based task that enforces a postural constraint using null space projection. + r"""Pink-based task that adds a posture objective that is in the null space projection of other tasks. This task implements posture control in the null space of higher priority tasks (typically end-effector pose tasks) within the Pink inverse kinematics framework. **Mathematical Formulation:** - The Pink inverse kinematics framework solves: - - .. math:: - - \begin{align*} - &\min_{\mathbf{v} \in \mathcal{C}} \sum_{\text{task } e} \left\| J_e(\mathbf{q}) \mathbf{v} + \alpha_e(\mathbf{q}) \right\|_{W_e}^2 \\ - &\text{subject to} \quad \mathbf{v}_{\min}(\mathbf{q}) \leq \mathbf{v} \leq \mathbf{v}_{\max}(\mathbf{q}) - \end{align*} - - where: - - :math:`\mathbf{v}` is the joint velocity vector (optimization variable) - - :math:`J_e(\mathbf{q})` is the task Jacobian for task :math:`e` - - :math:`\alpha_e(\mathbf{q})` is the task error/residual for task :math:`e` - - :math:`W_e` is the task weighting matrix - - :math:`\mathcal{C}` is the set of feasible velocities (joint velocity limits) - - :math:`\mathbf{v}_{\min}(\mathbf{q})`, :math:`\mathbf{v}_{\max}(\mathbf{q})` are the lower and upper joint velocity bounds + For details on Pink Inverse Kinematics optimization formulation visit: https://github.com/stephane-caron/pink **Null Space Posture Task Implementation:** @@ -98,19 +83,23 @@ def __init__( controlled_frames: list[str] | None = None, controlled_joints: list[str] | None = None, ) -> None: - r"""Create a null space posture task. + r"""Initialize the null space posture task. + + This task maintains a desired joint posture in the null space of higher-priority + frame tasks. Joint selection allows excluding specific joints (e.g., wrist joints + in humanoid manipulation) to prevent large rotational ranges from overwhelming + errors in critical joints like shoulders and waist. Args: - cost: Weighting factor for the posture task in the optimization objective, - in :math:`[\mathrm{cost}] / [\mathrm{rad}]`. - lm_damping: Unitless scale of the Levenberg-Marquardt regularization term. - Increase if the task is too jerky under infeasible targets. + cost: Task weighting factor in the optimization objective. + Units: :math:`[\text{cost}] / [\text{rad}]`. + lm_damping: Levenberg-Marquardt regularization scale (unitless). Defaults to 0.0. gain: Task gain :math:`\alpha \in [0, 1]` for low-pass filtering. Defaults to 1.0 (no filtering). - controlled_frames: List of frame names whose Jacobians define the primary tasks - for null space projection. If None or empty, no null space projection is applied. - controlled_joints: List of joint names to control in the posture task. - If None or empty, all actuated joints are controlled. + controlled_frames: Frame names whose Jacobians define the primary tasks for + null space projection. If None or empty, no projection is applied. + controlled_joints: Joint names to control in the posture task. If None or + empty, all actuated joints are controlled. """ super().__init__(cost=cost, gain=gain, lm_damping=lm_damping) self.target_q: np.ndarray | None = None diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 07728e84697b..c33f645b33d2 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -11,7 +11,7 @@ Added Changed -^^^^^ +^^^^^^^ * Updated pink inverse kinematics controller configuration for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``) to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom without From c7792e55a2855c26da861bba28884c2cf0629e5d Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Tue, 26 Aug 2025 09:57:17 -0700 Subject: [PATCH 11/12] nit changes on documentation and code in fourier robot definition --- CONTRIBUTORS.md | 2 +- source/isaaclab/docs/CHANGELOG.rst | 4 +-- .../isaaclab_assets/robots/fourier.py | 26 +++++-------------- 3 files changed, 9 insertions(+), 23 deletions(-) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 3816e0f20d14..bde83712b642 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -131,7 +131,6 @@ Guidelines for modifications: * Wei Yang * Xavier Nal * Xinpeng Liu -* Yan Chang * Yang Jin * Yanzi Zhu * Yijie Guo @@ -150,4 +149,5 @@ Guidelines for modifications: * Gavriel State * Hammad Mazhar * Marco Hutter +* Yan Chang * Yashraj Narang diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index ee1f3590896b..10ed470e018f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog Added ^^^^^ -* Added :attr:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg.target_eef_link_names` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` +* Created :attr:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg.target_eef_link_names` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` to specify the target end-effector link names for the pink inverse kinematics controller. Changed @@ -18,7 +18,7 @@ Changed * Improved the test_pink_ik script to more comprehensive test on controller accuracy. Also, migrated to use pytest. With the current IK controller improvements, our unit tests pass position and orientation accuracy test within **(1 mm, 1 degree)**. Previously, the position accuracy tolerances were set to **(30 mm, 10 degrees)**. -* Including a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` +* Included a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg` to control whether the IK controller raise an exception if robot joint limits are exceeded. In the case of an exception, the controller will hold the last joint position. This adds to stability of the controller and avoids operator experiencing what is perceived as sudden large delays in robot control. diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py index 96b50ffbbfb0..b2d87b1ee8f3 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py @@ -129,9 +129,7 @@ GR1T2_HIGH_PD_CFG = GR1T2_CFG.replace( actuators={ "trunk": ImplicitActuatorCfg( - joint_names_expr=[ - "waist_.*", - ], + joint_names_expr=["waist_.*"], effort_limit=None, velocity_limit=None, stiffness=4400, @@ -139,39 +137,27 @@ armature=0.01, ), "right-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "right_shoulder_.*", - "right_elbow_.*", - "right_wrist_.*", - ], + joint_names_expr=["right_shoulder_.*", "right_elbow_.*", "right_wrist_.*"], stiffness=4400.0, damping=40.0, armature=0.01, ), "left-arm": ImplicitActuatorCfg( - joint_names_expr=[ - "left_shoulder_.*", - "left_elbow_.*", - "left_wrist_.*", - ], + joint_names_expr=["left_shoulder_.*", "left_elbow_.*", "left_wrist_.*"], stiffness=4400.0, damping=40.0, armature=0.01, ), "right-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "R_.*", - ], + joint_names_expr=["R_.*"], stiffness=None, damping=None, ), "left-hand": ImplicitActuatorCfg( - joint_names_expr=[ - "L_.*", - ], + joint_names_expr=["L_.*"], stiffness=None, damping=None, ), }, ) -"""Configuration for the GR1T2 Humanoid robot configured for pick-place manipulation tasks.""" +"""Configuration for the GR1T2 Humanoid robot configured for with high PD gains for pick-place manipulation tasks.""" From ae2ac9564ac1edd643a45e958914fe02c1fb7139 Mon Sep 17 00:00:00 2001 From: Michael Lin Date: Tue, 26 Aug 2025 15:53:37 -0700 Subject: [PATCH 12/12] More nit changes. Also reverted high PD gain controller for nut pour and exhaust pipe envs. --- .../isaaclab/envs/mdp/actions/pink_task_space_actions.py | 2 +- .../envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py | 2 +- .../manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py | 4 ++-- .../manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index 6af3029797c9..f1e9fd7a819c 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -230,7 +230,7 @@ def apply_actions(self): all_envs_joint_pos_des = torch.cat((all_envs_joint_pos_des, self._target_hand_joint_positions), dim=1) self._processed_actions = all_envs_joint_pos_des - self._asset.set_joint_position_target(all_envs_joint_pos_des, self._joint_ids) + self._asset.set_joint_position_target(self._processed_actions, self._joint_ids) def reset(self, env_ids: Sequence[int] | None = None) -> None: """Reset the action term for specified environments. diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py index 31c08e9197a0..6bede8c0897c 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py @@ -50,7 +50,7 @@ def target_eef_pose_to_action( target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. gripper_action_dict: Dictionary of gripper actions for each end-effector. action_noise_dict: Noise to add to the action. If None, no noise is added. - env_id: Environment index to get the action for. (Unused in this implementation, present for interface compatibility.) + env_id: Environment index to get the action for. Returns: An action torch.Tensor that's compatible with env.step(). diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index b7aed2d1f4e9..ed1f0f06130c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -28,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip ## @@ -79,7 +79,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ) # Humanoid robot w/ arms higher - robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( + robot: ArticulationCfg = GR1T2_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index f8166188ed9c..ffa7929c9539 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -28,7 +28,7 @@ from . import mdp -from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip ## @@ -100,7 +100,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): ), ) - robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( + robot: ArticulationCfg = GR1T2_CFG.replace( prim_path="/World/envs/env_.*/Robot", init_state=ArticulationCfg.InitialStateCfg( pos=(0, 0, 0.93),