Add factory_assembly_task and the relevant task examples

.gitignore

@@ -35,7 +35,9 @@
 *.app
 
 # IDE stuff
-*.vscode
+.vscode/*
+!.vscode/launch.json
+!.vscode/settings.json
 *.*~
 TAGS
 

isaaclab_arena/assets/object_library.py

@@ -14,6 +14,11 @@
 from isaaclab_arena.affordances.pressable import Pressable
 from isaaclab_arena.assets.object import Object
 from isaaclab_arena.assets.object_base import ObjectType
+from isaaclab_arena.assets.object_utils import (
+    ASSEMBLY_ARTICULATION_INIT_STATE,
+    RIGID_BODY_PROPS_HIGH_PRECISION,
+    RIGID_BODY_PROPS_STANDARD,
+)
 from isaaclab_arena.assets.register import register_asset
 from isaaclab_arena.utils.pose import Pose
 
@@ -335,3 +340,129 @@ class DexCube(LibraryObject):
 
     def __init__(self, prim_path: str | None = None, initial_pose: Pose | None = None):
         super().__init__(prim_path=prim_path, initial_pose=initial_pose)
+
+
+@register_asset
+class Peg(LibraryObject):
+    """
+    A peg.
+    """
+
+    name = "peg"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_peg_8mm.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (3.0, 3.0, 3.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_HIGH_PRECISION,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.019),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }
+
+
+@register_asset
+class Hole(LibraryObject):
+    """
+    A hole.
+    """
+
+    name = "hole"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_hole_8mm.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (3.0, 3.0, 3.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_HIGH_PRECISION,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.05),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }
+
+
+@register_asset
+class SmallGear(LibraryObject):
+    """
+    A small gear.
+    """
+
+    name = "small_gear"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_gear_small.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (2.0, 2.0, 2.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_STANDARD,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.019),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }
+
+
+@register_asset
+class LargeGear(LibraryObject):
+    """
+    A large gear.
+    """
+
+    name = "large_gear"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_gear_large.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (2.0, 2.0, 2.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_STANDARD,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.019),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }
+
+
+@register_asset
+class GearBase(LibraryObject):
+    """
+    Gear base.
+    """
+
+    name = "gear_base"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_gear_base.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (2.0, 2.0, 2.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_STANDARD,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.05),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }
+
+
+@register_asset
+class MediumGear(LibraryObject):
+    """
+    A medium gear.
+    """
+
+    name = "medium_gear"
+    tags = ["object"]
+    usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_gear_medium.usd"
+    object_type = ObjectType.ARTICULATION
+    scale = (2.0, 2.0, 2.0)
+    spawn_cfg_addon = {
+        "rigid_props": RIGID_BODY_PROPS_STANDARD,
+        "mass_props": sim_utils.MassPropertiesCfg(mass=0.019),
+        "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
+    }
+    asset_cfg_addon = {
+        "init_state": ASSEMBLY_ARTICULATION_INIT_STATE,
+    }

isaaclab_arena/assets/object_utils.py

@@ -3,6 +3,8 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
+import isaaclab.sim as sim_utils
+from isaaclab.assets import ArticulationCfg
 from pxr import Usd
 
 from isaaclab_arena.assets.object_base import ObjectType
@@ -59,3 +61,38 @@ def detect_object_type(usd_path: str | None = None, stage: Usd.Stage | None = No
         return ObjectType.ARTICULATION
     else:
         raise ValueError("This should not happen. There is an unknown USD type in the tree.")
+
+
+# Predefined rigid body property configurations for factory assembly tasks
+# High iteration count for precision tasks (peg/hole insertion)
+RIGID_BODY_PROPS_HIGH_PRECISION = sim_utils.RigidBodyPropertiesCfg(
+    disable_gravity=False,
+    max_depenetration_velocity=5.0,
+    linear_damping=0.0,
+    angular_damping=0.0,
+    max_linear_velocity=1000.0,
+    max_angular_velocity=3666.0,
+    enable_gyroscopic_forces=True,
+    solver_position_iteration_count=192,
+    solver_velocity_iteration_count=1,
+    max_contact_impulse=1e32,
+)
+
+# Standard iteration count for gear mesh tasks
+RIGID_BODY_PROPS_STANDARD = sim_utils.RigidBodyPropertiesCfg(
+    disable_gravity=False,
+    max_depenetration_velocity=5.0,
+    linear_damping=0.0,
+    angular_damping=0.0,
+    max_linear_velocity=1000.0,
+    max_angular_velocity=3666.0,
+    enable_gyroscopic_forces=True,
+    solver_position_iteration_count=32,
+    solver_velocity_iteration_count=32,
+    max_contact_impulse=1e32,
+)
+
+ASSEMBLY_ARTICULATION_INIT_STATE = ArticulationCfg.InitialStateCfg(
+    joint_pos={},
+    joint_vel={},
+)

isaaclab_arena/embodiments/franka/franka.py

@@ -23,6 +23,9 @@
 from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
 from isaaclab.utils import configclass
+
+# Create a custom Franka configuration for assembly tasks
+# This is defined at module level to avoid being treated as a config field
 from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG
 from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events
 from isaaclab_tasks.manager_based.manipulation.stack.mdp.observations import ee_frame_pos, ee_frame_quat
@@ -34,6 +37,17 @@
 from isaaclab_arena.embodiments.franka.observations import gripper_pos
 from isaaclab_arena.utils.pose import Pose
 
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG = FRANKA_PANDA_HIGH_PD_CFG.copy()
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.spawn.activate_contact_sensors = True
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_shoulder"].stiffness = 150.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_shoulder"].damping = 30.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_forearm"].stiffness = 150.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_forearm"].damping = 30.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_hand"].stiffness = 150.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.actuators["panda_hand"].damping = 30.0
+FRANKA_PANDA_ASSEMBLY_HIGH_PD_CFG.init_state.pos = (0.0, 0.0, 0.0)
+
 
 @register_asset
 class FrankaEmbodiment(EmbodimentBase):