diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index 6ef9788869c6..9ed0fc201eec 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -25,7 +25,7 @@ ["Isaac-Lift-Cable-Franka-Camera", "rsl_rl", "newton_mjwarp_vbd_proxy", "isaacsim_rtx,newton_renderer,ovrtx", "ik,joint", "tasks/manipulation/franka_lift_cable.jpg", false, {"*": ["joint"]}], ["Isaac-Lift-Cloth-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy", "", "ik,joint", "tasks/manipulation/franka_lift_cloth.jpg", false, {"*": ["joint"]}], ["Isaac-Lift-Cloth-Franka-Camera", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy", "isaacsim_rtx,newton_renderer,ovrtx", "ik,joint", "tasks/manipulation/franka_lift_cloth.jpg", false, {"*": ["joint"]}], - ["Isaac-Lift-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "", "cube,shapes", "", false, {"*": ["shapes"]}], + ["Isaac-Lift-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "", "arm_collisions,cube,shapes", "", false, {"*": ["shapes"]}], ["Isaac-Lift-KukaAllegro", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "", "cube,shapes", "tasks/manipulation/kuka_allegro_lift.jpg", false, {"*": ["shapes"]}], ["Isaac-Lift-KukaAllegro-Camera", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "isaacsim_rtx,newton_renderer,ovrtx", "albedo128,albedo256,albedo64,cube,depth128,depth256,depth64,duo_camera,raycaster_depth128,raycaster_depth256,raycaster_depth64,rgb128,rgb256,rgb64,semantic_segmentation128,semantic_segmentation256,semantic_segmentation64,shapes,simple_shading_constant_diffuse128,simple_shading_constant_diffuse256,simple_shading_constant_diffuse64,simple_shading_diffuse_mdl128,simple_shading_diffuse_mdl256,simple_shading_diffuse_mdl64,simple_shading_full_mdl128,simple_shading_full_mdl256,simple_shading_full_mdl64,single_camera", "tasks/manipulation/kuka_allegro_lift.jpg", false, {"*": ["rgb64", "shapes", "single_camera"]}], ["Isaac-Lift-Soft-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy", "", "ik,joint", "newton/franka-mjwarp-vbd-coupling.png", false, {"*": ["joint"]}, {}, "tetrahedralization"], diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-franka-lift-soft.major.rst b/source/isaaclab_tasks/changelog.d/maximiliank-franka-lift-soft.major.rst new file mode 100644 index 000000000000..5b671ff64cf7 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/maximiliank-franka-lift-soft.major.rst @@ -0,0 +1,11 @@ +Changed +^^^^^^^ + +* Migrated Franka rigid and deformable Lift tasks to the shared flat asset, with backend-specific physics + and gripper-only collisions by default. Select ``arm_collisions`` to enable primitive arm contacts. + +Fixed +^^^^^ + +* Corrected rigid Lift reset sampling and success-driven motion regularization without changing + Kuka-Allegro rewards. Requalify existing Franka Lift checkpoints because the reset distribution changed. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py index 2d136cd8c31d..d7821a35ee28 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py @@ -7,6 +7,7 @@ from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import RewardTermCfg as RewTerm @@ -14,23 +15,16 @@ from isaaclab.sensors import ContactSensorCfg from isaaclab.sim import MeshCapsuleCfg, MeshCuboidCfg, MeshSphereCfg from isaaclab.utils import clone, configclass, replace -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR -from isaaclab_assets.robots import FRANKA_PANDA_CFG +from isaaclab_tasks.utils import preset + +from isaaclab_assets.robots import FRANKA_PANDA_FLAT_CFG from ... import lift_env_cfg as lift from ... import mdp -## -# Scene assets -## - -# The lift tasks run the menagerie-converted asset (identified inertials, authored finger coupling) with -# actuators calibrated for it, while the other Franka tasks keep the stock asset. -FRANKA_PANDA_LIFT_CFG = clone(FRANKA_PANDA_CFG) -FRANKA_PANDA_LIFT_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda.usda" -# Reset clearance was calibrated for these arm meshes; the asset's primitive colliders intersect the ground. -FRANKA_PANDA_LIFT_CFG.spawn.variants = {"Colliders": "convex_hulls"} +# Lift uses task-specific actuators calibrated for contact-rich manipulation. +FRANKA_PANDA_LIFT_CFG = clone(FRANKA_PANDA_FLAT_CFG) FRANKA_PANDA_LIFT_CFG.actuators = { # inspired by libfranka's joint_impedance_control.cpp; ``actuator_velocity_limit`` is the soft task # limit and ``joint_velocity_limit`` the separate solver request @@ -51,6 +45,7 @@ "panda_joint6": 25.0, "panda_joint7": 15.0, }, + viscous_friction=0.0, armature={ "panda_joint[1-2]": 0.6057, "panda_joint[3-4]": 0.4625, @@ -64,6 +59,7 @@ joint_velocity_limit=2.0, stiffness=350.0, damping=175.0, + viscous_friction=0.0, armature=0.1, ), "panda_finger2_passive": ImplicitActuatorCfg( @@ -73,6 +69,7 @@ joint_velocity_limit=2.0, stiffness=0.0, damping=0.0, + viscous_friction=0.0, armature=0.1, ), } @@ -102,6 +99,12 @@ class FrankaSceneCfg(lift.SceneCfg): def __post_init__(self): super().__post_init__() + # Lift only requires hand-object and fingertip-object contacts; the asset's + # complete primitive colliders remain available for general robot use. + self.robot.spawn.variants = { + "Physics": preset(default="mujoco", isaacsim_physx="physx", physx="physx", ovphysx="physx"), + "Colliders": preset(default="gripper_only", arm_collisions="primitives"), + } self.robot.spawn.activate_contact_sensors = True # the base is rotated by 180 degrees about z so the workspace lies at positive x self.robot.init_state.rot = (0.0, 0.0, 1.0, 0.0) @@ -157,9 +160,12 @@ class FrankaRelJointPosActionCfg: @configclass -class FrankaReorientRewardCfg(lift.RewardsCfg): +class FrankaLiftRewardCfg(lift.RewardsCfg): """Reward terms for the MDP, with the Franka finger contact sensors filled in.""" + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-4) + joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-1e-4, params={"asset_cfg": SceneEntityCfg("robot")}) + good_finger_contact = RewTerm( func=mdp.contacts, weight=0.75, @@ -186,6 +192,28 @@ def __post_init__(self): self.success.params["finger_names"] = FINGER_SENSORS +@configclass +class FrankaLiftCurriculumCfg(lift.CurriculumCfg): + """Franka-specific motion regularization as grasp difficulty increases.""" + + action_rate = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "rewards.action_rate.weight", + "modify_fn": mdp.difficulty_interpolate_float, + "modify_params": {"initial_value": -1e-4, "final_value": -1e-1, "difficulty_term_str": "adr"}, + }, + ) + joint_vel = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "rewards.joint_vel.weight", + "modify_fn": mdp.difficulty_interpolate_float, + "modify_params": {"initial_value": -1e-4, "final_value": -1e-1, "difficulty_term_str": "adr"}, + }, + ) + + @configclass class FrankaEventCfg(lift.EventCfg): """Franka-specific event configuration.""" @@ -243,10 +271,10 @@ def __post_init__(self): @configclass class FrankaMixinCfg: - """Franka-specific scene, observation, action, reward and event terms, mixed into the task configurations.""" + """Franka scene, observation, action, reward and event terms for the lift task.""" scene: FrankaSceneCfg = FrankaSceneCfg(num_envs=4096, env_spacing=3, replicate_physics=True) - rewards: FrankaReorientRewardCfg = FrankaReorientRewardCfg() + rewards: FrankaLiftRewardCfg = FrankaLiftRewardCfg() observations: StateObservationCfg = StateObservationCfg() actions: FrankaRelJointPosActionCfg = FrankaRelJointPosActionCfg() events: FrankaEventCfg = FrankaEventCfg() @@ -256,6 +284,9 @@ def __post_init__(self): self.commands.object_pose.body_name = "panda_hand" # Franka base is rotated 180 deg about z, so the workspace mirrors to positive x. self.commands.object_pose.ranges.pos_x = (0.3, 0.7) + # The actuator limits are nominal policy limits, not evidence of unstable physics. + # Reserve the abnormal-state termination for velocities beyond the solver contract. + self.terminations.abnormal_robot.func = mdp.abnormal_robot_state self.terminations.abnormal_robot.params["asset_cfg"] = SceneEntityCfg("robot", joint_names="panda_joint.*") @@ -263,6 +294,27 @@ def __post_init__(self): class FrankaLiftEnvCfg(FrankaMixinCfg, lift.LiftEnvCfg): """Franka object lifting environment.""" + curriculum: FrankaLiftCurriculumCfg | None = FrankaLiftCurriculumCfg() + + def __post_init__(self): + super().__post_init__() + reset = self.events.conditional_reset.params + terms = reset["terms"] + # The aligned opening must be written after the generic gripper-width reset. + pregrasp = terms.pop("reset_object_to_target") + terms["reset_object_to_target"] = pregrasp + pregrasp.func = mdp.reset_to_grasp + pregrasp.params.update( + probability=0.75, + gripper_cfg=SceneEntityCfg("robot", joint_names="panda_finger_joint.*"), + pose_range={"x": (-0.002, 0.002), "y": (-0.002, 0.002), "z": (0.1, 0.1)}, + gripper_joint_positions=[0.026, 0.026, 0.0135, 0.026, 0.021, 0.026, 0.026, 0.011], + asset_orientations=[(0.0, 0.0, 0.0, 1.0)] * 5 + [(0.0, 2.0**-0.5, 0.0, 2.0**-0.5)] * 3, + ) + pregrasp.params.pop("velocity_range") + # Farthest-point thinning discards valid near-grasp starts. + reset["diversity_feature"] = None + def play_mode(self): super().play_mode() # evaluate at the datasheet gripper speed: without the closing-speed randomization the hand diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py index cd668dd3ec47..3ad14de79289 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py @@ -48,7 +48,7 @@ from isaaclab_tasks.utils import PresetCfg, preset from isaaclab_tasks.utils.presets import MultiBackendRendererCfg -from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG +from isaaclab_assets.robots.franka import FRANKA_PANDA_FLAT_CFG from ... import mdp @@ -56,7 +56,7 @@ # Scene assets ## -# shared volume material parameters; the Newton configuration uses the equivalent Lame parameters +# Shared volume material parameters. The Newton config below uses the equivalent Lame parameters. YOUNGS_MODULUS = 2e5 POISSONS_RATIO = 0.3 @@ -190,12 +190,12 @@ class PhysicsCfg(PresetCfg): class FrankaSoftBaseSceneCfg(InteractiveSceneCfg): """Scene for the Franka deformable environment, also the base of the cloth and cable scenes.""" - robot: ArticulationCfg = replace(FRANKA_PANDA_MENAGERIE_CFG, prim_path="{ENV_REGEX_NS}/Robot") - robot.spawn.variants = preset( - default={"Physics": "mujoco"}, - isaacsim_physx={"Physics": "physx"}, - physx={"Physics": "physx"}, - ) + robot: ArticulationCfg = replace(FRANKA_PANDA_FLAT_CFG, prim_path="{ENV_REGEX_NS}/Robot") + # Deformable contact is restricted to the hand and fingertips for throughput. + robot.spawn.variants = { + "Physics": preset(default="mujoco", isaacsim_physx="physx", physx="physx", ovphysx="physx"), + "Colliders": "gripper_only", + } # end-effector frame for reward shaping ee_frame: FrameTransformerCfg = FrameTransformerCfg( @@ -257,6 +257,7 @@ def __post_init__(self): "panda_joint6": 25.0, "panda_joint7": 15.0, }, + viscous_friction=0.0, armature={ "panda_joint[1-2]": 0.6057, "panda_joint[3-4]": 0.4625, @@ -270,6 +271,7 @@ def __post_init__(self): joint_velocity_limit=2.0, stiffness=350.0, damping=175.0, + viscous_friction=0.0, armature=0.1, ), "panda_finger2_passive": ImplicitActuatorCfg( @@ -279,6 +281,7 @@ def __post_init__(self): joint_velocity_limit=2.0, stiffness=0.0, damping=0.0, + viscous_friction=0.0, armature=0.1, ), } diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi index e5826e6db8f4..fcb2979c51bf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi @@ -31,6 +31,7 @@ __all__ = [ "deformable_ee_distance", "deformable_lifting", "deformable_outside_bounds", + "difficulty_interpolate_float", "ee_below_minimum", "fingers_contact_force_b", "get_reset_state", @@ -51,6 +52,7 @@ __all__ = [ "reset_cable_state_uniform", "reset_deformable_over_support", "reset_joints_shared_offset", + "reset_to_grasp", "reset_to_target", "set_reset_state", "slab_clearance", @@ -61,7 +63,7 @@ __all__ = [ from isaaclab_tasks.utils.success_monitor import SuccessMonitor, SuccessMonitorCfg from .commands import CableUniformPoseCommandCfg, DeformableUniformPoseCommandCfg, ObjectUniformPoseCommandCfg -from .curriculums import gravity_range_linear +from .curriculums import difficulty_interpolate_float, gravity_range_linear from .events import ( conditional_reset, grasp_travel_distance, @@ -69,6 +71,7 @@ from .events import ( reset_cable_state_uniform, reset_deformable_over_support, reset_joints_shared_offset, + reset_to_grasp, reset_to_target, slab_clearance, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py index 4dfad1e07dd7..587adeb972c7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py @@ -14,6 +14,20 @@ from isaaclab.envs import ManagerBasedRLEnv +def difficulty_interpolate_float( + env: ManagerBasedRLEnv, + _env_ids: Sequence[int], + _data: float, + initial_value: float, + final_value: float, + difficulty_term_str: str, +) -> float: + """Interpolate a scalar continuously with an ADR term's success-driven difficulty.""" + difficulty_term = getattr(env.curriculum_manager.cfg, difficulty_term_str).func + fraction = min(max(difficulty_term.difficulty_frac, 0.0), 1.0) + return initial_value + fraction * (final_value - initial_value) + + def gravity_range_linear( env: ManagerBasedRLEnv, _env_ids: Sequence[int], diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py index c2a39d721d16..fdbdd48c4eb4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py @@ -18,7 +18,7 @@ from isaaclab import cloner from isaaclab.managers import EventTermCfg, ManagerTermBase, ManagerTermBaseCfg, SceneEntityCfg from isaaclab.utils import instantiate -from isaaclab.utils.math import quat_apply, random_orientation, sample_uniform +from isaaclab.utils.math import quat_apply, quat_mul, random_orientation, sample_uniform from isaaclab_tasks.utils.success_monitor import SuccessMonitor, SuccessMonitorCfg @@ -147,6 +147,96 @@ def reset_to_target( asset.write_root_velocity_to_sim_index(root_velocity=velocities, env_ids=picked) +class reset_to_grasp(ManagerTermBase): + """Place selected object variants in aligned parallel-gripper pre-grasps.""" + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + gripper_cfg: SceneEntityCfg = cfg.params["gripper_cfg"] + object_cfg = getattr(env.cfg.scene, asset_cfg.name) + plan = env.scene.clone_plan + object_prototypes = cloner.path.get_asset_prototypes(plan, object_cfg.prim_path) + if not len(object_prototypes): + raise ValueError(f"Could not find clone-plan prototypes for asset '{asset_cfg.name}'.") + variant_ids = np.full(env.num_envs, -1, dtype=np.int64) + for variant_id, prototype_id in enumerate(object_prototypes): + world_ids, _ = cloner.query.get_asset_prototype_unique_world_index(plan.topology, int(prototype_id)) + world_ids = world_ids[world_ids >= 0] + if (variant_ids[world_ids] >= 0).any(): + raise ValueError(f"Multiple object variants are assigned to asset '{asset_cfg.name}' in one world.") + variant_ids[world_ids] = variant_id + if (variant_ids < 0).any(): + raise ValueError(f"No object variant is assigned to asset '{asset_cfg.name}' in some worlds.") + self._variant_ids = torch.as_tensor(variant_ids, device=env.device) + self._gripper_joint_ids = env.scene[gripper_cfg.name].find_joints(gripper_cfg.joint_names)[0] + gripper_joint_positions = cfg.params["gripper_joint_positions"] + asset_orientations = cfg.params["asset_orientations"] + if len(gripper_joint_positions) < len(object_prototypes) or len(asset_orientations) < len(object_prototypes): + raise ValueError(f"reset_to_grasp requires one gripper pose per object variant ({len(object_prototypes)}).") + self._gripper_joint_positions = torch.as_tensor(gripper_joint_positions, device=env.device) + self._asset_orientations = torch.as_tensor(asset_orientations, device=env.device) + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + probability: float, + target_cfg: SceneEntityCfg, + gripper_cfg: SceneEntityCfg, + gripper_joint_positions: list[float], + asset_orientations: list[tuple[float, float, float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("object"), + ) -> None: + """Reset a fraction of environments to configured pre-grasps. + + Args: + env: The environment. + env_ids: Environments to reset. + pose_range: Object-position offsets in the target frame [m]. + probability: Per-environment probability of applying the pre-grasp. + target_cfg: Body defining the pre-grasp frame. + gripper_cfg: Parallel gripper joints to pose. + gripper_joint_positions: Joint position for each object variant [m or rad, depending on joint type]. + asset_orientations: Object orientation in the target frame for each variant, in ``(x, y, z, w)`` order. + asset_cfg: Object asset to reset. + """ + picked = env_ids[torch.rand(len(env_ids), device=env.device) < probability] + if len(picked) == 0: + return + + asset = env.scene[asset_cfg.name] + target = env.scene[target_cfg.name] + gripper = env.scene[gripper_cfg.name] + variant_ids = self._variant_ids[picked] + + target_pos = target.data.body_pos_w.torch[picked][:, target_cfg.body_ids, :].reshape(len(picked), -1)[:, :3] + target_quat = target.data.body_quat_w.torch[picked][:, target_cfg.body_ids, :].reshape(len(picked), -1)[:, :4] + offset_ranges = torch.tensor( + [tuple(pose_range.get(axis, (0.0, 0.0))) for axis in ("x", "y", "z")], device=asset.device + ) + local_offsets = sample_uniform(offset_ranges[:, 0], offset_ranges[:, 1], (len(picked), 3), device=asset.device) + positions = target_pos + quat_apply(target_quat, local_offsets) + local_orientations = self._asset_orientations[variant_ids] + orientations = quat_mul(target_quat, local_orientations) + + joint_positions = self._gripper_joint_positions[variant_ids, None].expand(-1, len(self._gripper_joint_ids)) + gripper.write_joint_position_to_sim_index( + position=joint_positions, joint_ids=self._gripper_joint_ids, env_ids=picked + ) + gripper.write_joint_velocity_to_sim_index( + velocity=torch.zeros_like(joint_positions), joint_ids=self._gripper_joint_ids, env_ids=picked + ) + gripper.set_joint_position_target_index( + target=joint_positions, joint_ids=self._gripper_joint_ids, env_ids=picked + ) + asset.write_root_pose_to_sim_index(root_pose=torch.cat((positions, orientations), dim=-1), env_ids=picked) + asset.write_root_velocity_to_sim_index( + root_velocity=torch.zeros((len(picked), 6), device=asset.device), env_ids=picked + ) + + class conditional_reset(ManagerTermBase): """Run wrapped reset terms and guarantee the resulting states satisfy a criterion. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py index 5f38e5d6ea9a..683307b69612 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py @@ -21,7 +21,7 @@ class out_of_bound(ManagerTermBase): - """Termination condition for when the object falls out of bound. + """Terminate when the rigid object state is non-finite or its position leaves the workspace bounds. The world-space bounds are cached and rebuilt per axis only when the corresponding ``in_bound_range`` entry changes. This keeps the hot path free of host-to-device @@ -58,7 +58,12 @@ def __call__( self._cached_axis[i] = bounds pos_w = self._object.data.root_pos_w.torch - return ((pos_w < self._lower) | (pos_w > self._upper)).any(dim=1) + quat_w = self._object.data.root_quat_w.torch + vel_w = self._object.data.root_vel_w.torch + invalid = ( + ~torch.isfinite(pos_w).all(dim=1) | ~torch.isfinite(quat_w).all(dim=1) | ~torch.isfinite(vel_w).all(dim=1) + ) + return invalid | ((pos_w < self._lower) | (pos_w > self._upper)).any(dim=1) def abnormal_robot_state(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: @@ -67,8 +72,8 @@ def abnormal_robot_state(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = Sce Such violations indicate unstable physics, typically caused by aggressive actions. """ robot: Articulation = env.scene[asset_cfg.name] - joint_vel = robot.data.joint_vel.torch - joint_vel_limits = robot.data.joint_vel_limits.torch + joint_vel = robot.data.joint_vel.torch[:, asset_cfg.joint_ids] + joint_vel_limits = robot.data.joint_vel_limits.torch[:, asset_cfg.joint_ids] return (joint_vel.abs() > (joint_vel_limits * 2)).any(dim=1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py index 96926a118a82..8cc4b8235813 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py @@ -311,7 +311,11 @@ def collect_collision_meshes(root_prim, owner_frame_fn: Callable) -> dict[int, t mesh_types = PRIMITIVE_MESH_TYPES + ["Mesh"] mesh_prims = sim_utils.get_all_matching_child_prims( root_prim.GetPath(), - lambda prim: prim.GetTypeName() in mesh_types and prim.HasAPI(UsdPhysics.CollisionAPI), + lambda prim: ( + prim.GetTypeName() in mesh_types + and prim.HasAPI(UsdPhysics.CollisionAPI) + and UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get() + ), ) meshes_by_owner: dict[int, list[trimesh.Trimesh]] = {} diff --git a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py index 2da93775565c..803491c47bf6 100644 --- a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py +++ b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py @@ -6,27 +6,38 @@ """Behavioral tests for the dexterous Lift tasks.""" from types import SimpleNamespace +from unittest.mock import Mock import gymnasium as gym import pytest import torch import warp as wp -from pxr import Usd +from pxr import Usd, UsdGeom, UsdPhysics +from isaaclab.cloner import make_clone_plan from isaaclab.managers import CommandTerm, ObservationTermCfg, SceneEntityCfg -from isaaclab.sim import select_usd_variants +from isaaclab.sim import select_usd_variants, use_stage from isaaclab.utils.warp import ProxyArray from isaaclab_tasks.core.lift import mdp from isaaclab_tasks.core.lift.config.franka.franka_env_cfg import FrankaLiftEnvCfg from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import FrankaSoftEnvCfg +from isaaclab_tasks.core.lift.config.kuka_allegro.kuka_allegro_camera_env_cfg import ( + KukaAllegroLiftCameraEnvCfg, + KukaAllegroReorientCameraEnvCfg, +) +from isaaclab_tasks.core.lift.config.kuka_allegro.kuka_allegro_env_cfg import ( + KukaAllegroLiftEnvCfg, + KukaAllegroReorientEnvCfg, +) from isaaclab_tasks.core.lift.mdp.commands import pose_commands from isaaclab_tasks.core.lift.mdp.commands.pose_commands import ( CableUniformPoseCommand, DeformableUniformPoseCommand, ObjectUniformPoseCommand, ) +from isaaclab_tasks.core.lift.mdp.utils import collect_collision_meshes from isaaclab_tasks.utils.hydra import resolve_presets @@ -48,6 +59,66 @@ def __init__(self, environment_ids: torch.Tensor, **assets): self.env_origins = torch.zeros((len(environment_ids), 3)) +def test_kuka_allegro_lift_family_keeps_its_existing_reward_contract() -> None: + """Franka-only motion regularization must not enter Kuka state or camera tasks.""" + cfgs = ( + KukaAllegroLiftEnvCfg(), + KukaAllegroReorientEnvCfg(), + KukaAllegroLiftCameraEnvCfg().default, + KukaAllegroReorientCameraEnvCfg().default, + ) + for cfg in cfgs: + for name in ("action_rate", "joint_vel"): + assert not hasattr(cfg.rewards, name) + assert not hasattr(cfg.curriculum, name) + + +def test_rigid_lift_motion_regularization_follows_success_driven_adr() -> None: + """Franka motion penalties should grow with success, not elapsed steps.""" + cfg = FrankaLiftEnvCfg() + curriculum = cfg.curriculum + assert curriculum.adr.func is mdp.DifficultyScheduler + difficulty = SimpleNamespace(difficulty_frac=0.0) + env = SimpleNamespace( + common_step_counter=100_000, + curriculum_manager=SimpleNamespace(cfg=SimpleNamespace(adr=SimpleNamespace(func=difficulty))), + ) + + for term_name in ("action_rate", "joint_vel"): + assert getattr(cfg.rewards, term_name).weight == pytest.approx(-1e-4) + term = getattr(curriculum, term_name) + assert term.func is mdp.modify_term_cfg + assert term.params["address"] == f"rewards.{term_name}.weight" + assert term.params["modify_fn"] is mdp.difficulty_interpolate_float + params = term.params["modify_params"] + assert params == {"initial_value": -1e-4, "final_value": -1e-1, "difficulty_term_str": "adr"} + for fraction, expected in ( + (-1.0, -1e-4), + (0.0, -1e-4), + (0.02, -0.002098), + (0.5, -0.05005), + (1.0, -0.1), + (2.0, -0.1), + ): + difficulty.difficulty_frac = fraction + weight = term.params["modify_fn"](env, torch.arange(2), -1e-4, **params) + assert weight == pytest.approx(expected) + + +def test_abnormal_robot_state_ignores_nominal_velocity_excursions() -> None: + """The instability guard should leave policy exploration below twice the solver limit intact.""" + robot = SimpleNamespace( + data=SimpleNamespace( + joint_vel=SimpleNamespace(torch=torch.tensor([[15.0, 100.0], [20.0, 100.0], [20.1, 0.0]])), + joint_vel_limits=SimpleNamespace(torch=torch.full((3, 2), 10.0)), + ) + ) + env = SimpleNamespace(scene={"robot": robot}) + arm_only = SimpleNamespace(name="robot", joint_ids=[0]) + + assert mdp.abnormal_robot_state(env, arm_only).tolist() == [False, False, True] + + @pytest.mark.parametrize( ("selected_presets", "expected_physics"), [ @@ -63,7 +134,7 @@ def test_franka_soft_robot_physics_variant_matches_backend( """The Franka USD physics payload must match the selected simulation backend.""" cfg = resolve_presets(FrankaSoftEnvCfg(), selected=selected_presets) - assert cfg.scene.robot.spawn.variants == {"Physics": expected_physics} + assert cfg.scene.robot.spawn.variants == {"Physics": expected_physics, "Colliders": "gripper_only"} def test_franka_reorient_is_not_registered() -> None: @@ -71,15 +142,29 @@ def test_franka_reorient_is_not_registered() -> None: assert "Isaac-Reorient-Franka" not in gym.registry -def test_franka_rigid_task_selects_collision_meshes_for_reset_clearance() -> None: - """Reset validation keeps the original arm meshes when the asset defaults to capsules.""" - cfg = FrankaLiftEnvCfg() +def test_franka_lift_physx_runtimes_share_the_same_mdp() -> None: + """Isaac Sim PhysX and OvPhysX should differ only in runtime configuration.""" + isaacsim_cfg = resolve_presets(FrankaLiftEnvCfg(), selected=("isaacsim_physx", "cube")) + ovphysx_cfg = resolve_presets(FrankaLiftEnvCfg(), selected=("ovphysx", "cube")) + assert isaacsim_cfg.sim.physics.enable_external_forces_every_iteration + assert ovphysx_cfg.sim.physics.enable_external_forces_every_iteration + isaacsim = isaacsim_cfg.to_dict() + ovphysx = ovphysx_cfg.to_dict() + + for section in ("scene", "observations", "actions", "commands", "rewards", "terminations", "events", "curriculum"): + assert ovphysx[section] == isaacsim[section], section + + +def test_franka_lift_selects_gripper_only_colliders() -> None: + """Lift avoids the arm colliders that intersect the ground during reset sampling.""" + cfg = resolve_presets(FrankaLiftEnvCfg(), selected=()) stage = Usd.Stage.CreateInMemory() robot = stage.DefinePrim("/Robot", "Xform") colliders = robot.GetVariantSets().AddVariantSet("Colliders") for selection, prim_path, prim_type in ( ("convex_hulls", "/Robot/link1_c/link1_c", "Mesh"), ("primitives", "/Robot/link1_capsule", "Capsule"), + ("gripper_only", "/Robot/gripper_capsule", "Capsule"), ): colliders.AddVariant(selection) colliders.SetVariantSelection(selection) @@ -89,10 +174,120 @@ def test_franka_rigid_task_selects_collision_meshes_for_reset_clearance() -> Non select_usd_variants("/Robot", cfg.scene.robot.spawn.variants or {}, stage=stage) - assert stage.GetPrimAtPath("/Robot/link1_c/link1_c").IsValid() + assert stage.GetPrimAtPath("/Robot/gripper_capsule").IsValid() + assert not stage.GetPrimAtPath("/Robot/link1_c/link1_c").IsValid() assert not stage.GetPrimAtPath("/Robot/link1_capsule").IsValid() +def test_franka_lift_retains_aligned_pregrasp_resets() -> None: + """The aligned Lift reset must run after the generic finger-width reset.""" + lift = FrankaLiftEnvCfg() + + assert lift.actions.action.joint_names == [".*"] + assert lift.terminations.abnormal_robot.func is mdp.abnormal_robot_state + lift_reset = lift.events.conditional_reset.params + lift_reset_terms = list(lift_reset["terms"]) + assert lift_reset_terms.index("reset_object_to_target") > lift_reset_terms.index("reset_gripper_width") + lift_target_reset = lift_reset["terms"]["reset_object_to_target"] + assert lift_target_reset.func is mdp.reset_to_grasp + assert lift_target_reset.params["probability"] == pytest.approx(0.75) + assert lift_target_reset.params["pose_range"] == { + "x": (-0.002, 0.002), + "y": (-0.002, 0.002), + "z": (0.1, 0.1), + } + assert lift_target_reset.params["gripper_cfg"].joint_names == "panda_finger_joint.*" + assert len(lift_target_reset.params["gripper_joint_positions"]) == 8 + assert len(lift_target_reset.params["asset_orientations"]) == 8 + assert "object_robot_clearance" in lift_reset["valid_criteria"] + assert lift_reset["diversity_feature"] is None + assert lift_reset["success_monitor"].target_success_rate == pytest.approx(0.5) + + lift.play_mode() + assert lift.events.conditional_reset.params["terms"]["reset_object_to_target"].params[ + "probability" + ] == pytest.approx(0.75) + + +def test_franka_lift_pregrasp_resolves_object_variants_from_clone_topology() -> None: + """The pre-grasp term must map heterogeneous object prototypes onto environment IDs.""" + object_path = "/World/envs/env_.*/Object" + object_cfg = SimpleNamespace(prim_path=object_path) + robot_cfg = SimpleNamespace(prim_path="/World/envs/env_.*/Robot") + plan = make_clone_plan( + (object_cfg, object_cfg, robot_cfg), + ((0, 2), (1, 2)), + 4, + ) + robot = SimpleNamespace( + find_joints=lambda _: ([0, 1], []), + data=SimpleNamespace( + body_pos_w=SimpleNamespace(torch=torch.zeros((4, 1, 3))), + body_quat_w=SimpleNamespace(torch=torch.tensor([[[0.0, 0.0, 0.0, 1.0]]] * 4)), + ), + write_joint_position_to_sim_index=Mock(), + write_joint_velocity_to_sim_index=Mock(), + set_joint_position_target_index=Mock(), + ) + object_asset = SimpleNamespace( + device="cpu", write_root_pose_to_sim_index=Mock(), write_root_velocity_to_sim_index=Mock() + ) + scene = _FakeScene(torch.arange(4), robot=robot, object=object_asset) + scene.clone_plan = plan + env = SimpleNamespace( + cfg=SimpleNamespace(scene=SimpleNamespace(object=object_cfg)), + scene=scene, + device="cpu", + num_envs=4, + ) + cfg = SimpleNamespace( + params={ + "asset_cfg": SceneEntityCfg("object"), + "gripper_cfg": SceneEntityCfg("robot", joint_names="panda_finger_joint.*"), + "target_cfg": SimpleNamespace(name="robot", body_ids=[0]), + "pose_range": {axis: (0.0, 0.0) for axis in ("x", "y", "z")}, + "probability": 1.0, + "gripper_joint_positions": [0.026, 0.0135], + "asset_orientations": [(0.0, 0.0, 0.0, 1.0), (0.0, 0.7071068, 0.0, 0.7071068)], + } + ) + + term = mdp.reset_to_grasp(cfg, env) + term(env, torch.arange(4), **cfg.params) + + torch.testing.assert_close( + robot.write_joint_position_to_sim_index.call_args.kwargs["position"], + torch.tensor([[0.026, 0.026], [0.026, 0.026], [0.0135, 0.0135], [0.0135, 0.0135]]), + ) + root_pose = object_asset.write_root_pose_to_sim_index.call_args.kwargs["root_pose"] + torch.testing.assert_close(root_pose[:, :3], torch.zeros((4, 3))) + torch.testing.assert_close( + root_pose[:, 3:], + torch.tensor([[0.0, 0.0, 0.0, 1.0]] * 2 + [[0.0, 0.7071068, 0.0, 0.7071068]] * 2), + ) + + cfg.params["asset_orientations"] = [(0.0, 0.0, 0.0, 1.0)] + with pytest.raises(ValueError, match="one gripper pose per object variant"): + mdp.reset_to_grasp(cfg, env) + + +def test_reset_clearance_ignores_disabled_collision_geometry() -> None: + """Disabled colliders and visual-only geometry must not reject reset candidates.""" + stage = Usd.Stage.CreateInMemory() + root = stage.DefinePrim("/Object", "Xform") + for name, enabled in (("default_enabled", None), ("explicit_enabled", True), ("disabled", False)): + prim = UsdGeom.Cube.Define(stage, f"/Object/{name}").GetPrim() + collision = UsdPhysics.CollisionAPI.Apply(prim) + if enabled is not None: + collision.CreateCollisionEnabledAttr(enabled) + UsdGeom.Cube.Define(stage, "/Object/visual_only") + + with use_stage(stage): + meshes = collect_collision_meshes(root, lambda prim: (prim.GetName(), root)) + + assert set(meshes) == {"default_enabled", "explicit_enabled"} + + def _make_vision_camera(data_type: str, images: torch.Tensor) -> tuple[mdp.vision_camera, SimpleNamespace]: """Build a ``vision_camera`` term around a fake single-data-type camera sensor.""" sensor = SimpleNamespace( diff --git a/source/isaaclab_tasks/test/core/test_reach_franka_presets.py b/source/isaaclab_tasks/test/core/test_reach_franka_presets.py index 21794de95c49..cb9853d0c79b 100644 --- a/source/isaaclab_tasks/test/core/test_reach_franka_presets.py +++ b/source/isaaclab_tasks/test/core/test_reach_franka_presets.py @@ -32,6 +32,12 @@ _OSC_TASK, "Isaac-Open-Drawer-Franka", "Isaac-Open-Drawer-Franka-Direct", + "Isaac-Lift-Franka", +) +_SOFT_FRANKA_TASKS = ( + "Isaac-Lift-Soft-Franka", + "Isaac-Lift-Cloth-Franka", + "Isaac-Lift-Cable-Franka", ) @@ -54,7 +60,7 @@ def _without_controller_dependent_cfg(cfg): return cfg_dict -@pytest.mark.parametrize("task", _RIGID_FRANKA_TASKS) +@pytest.mark.parametrize("task", _RIGID_FRANKA_TASKS + _SOFT_FRANKA_TASKS) def test_franka_tasks_select_the_canonical_asset_and_backend_payload(task): default_cfg = _load_reach_env_cfg(task) physx_cfg = _load_reach_env_cfg(task, "isaacsim_physx")