Skip to content

Support Embodiment Placement for Droid/Franka #954

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
qianl-nv merged 6 commits into from
Jul 28, 2026
+491 −284
Merged

Support Embodiment Placement for Droid/Franka #954

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b517491
Add shared on-stand USD compose for Droid and Franka
qianl-nv Jul 27, 2026
c7609c3
Document Droid stand height and manual floor placement.
qianl-nv Jul 28, 2026
7aed247
Encapsulate on-stand prim layout in RobotPrimSpec and StandPrimSpec.
qianl-nv Jul 28, 2026
5db5acf
Assert on-stand USD Save() succeeds before promoting cache file.
qianl-nv Jul 28, 2026
c1d726d
Trim redundant on-stand runtime test coverage.
qianl-nv Jul 28, 2026
30e6077
Fix test_embodiment_collision_mesh
qianl-nv Jul 28, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
142 changes: 42 additions & 100 deletions isaaclab_arena/embodiments/droid/droid.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,13 @@
# SPDX-License-Identifier: Apache-2.0


import functools
import torch
import warnings
from abc import ABC
from typing import Any

import isaaclab.envs.mdp as mdp_isaac_lab
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg
from isaaclab.assets.asset_base_cfg import AssetBaseCfg
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import (
BinaryJointPositionActionCfg,
Expand All @@ -30,7 +26,6 @@
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.sensors.camera.camera_cfg import CameraCfg
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.configclass import configclass

from isaaclab_arena.assets.nucleus import ARENA_NUCLEUS_DIR
Expand All @@ -40,21 +35,37 @@
from isaaclab_arena.embodiments.droid.observations import arm_joint_pos, ee_pos, ee_quat, gripper_pos
from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase
from isaaclab_arena.embodiments.franka.franka import franka_stack_events
from isaaclab_arena.embodiments.robot_on_stand_utils import RobotPrimSpec, StandPrimSpec, compose_on_stand_usd
from isaaclab_arena.utils.cameras import ArenaCameraCfg
from isaaclab_arena.utils.pose import Pose, PosePerEnv, translate_by_xyz_offset
from isaaclab_arena.utils.pose import Pose

# The base stand's x/y footprint.
_STAND_FOOTPRINT_SCALE_XY: tuple[float, float] = (1.2, 1.2)
# The default stand height.
_DEFAULT_STAND_HEIGHT_M: float = 1.35
_FALLBACK_STAND_UNIT_HEIGHT_M: float = 0.795
_DROID_ROBOT_PRIM = RobotPrimSpec(
robot_usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/droid/franka_robotiq_2f_85_flattened.usd",
root_prim_path="/panda",
robot_base_prim_name="panda_link0",
stand_prim_name="stand_instanceable",
)
_DROID_STAND_PRIM = StandPrimSpec(
stand_usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/object_library/srl_robolab_assets/robots/franka_stand_grey.usda",
ref_prim_path="/World/franka_table",
payload_child_name="franka_table",
footprint_translate_xyz=(-0.05, 0.0, 0.0),
footprint_scale_xy=(1.2, 1.2),
stand_default_height=1.35,
)


class DroidEmbodimentBase(EmbodimentBase, ABC):
"""Abstract base class for DROID embodiments (https://droid-dataset.github.io/droid/docs/hardware-setup).

Includes Franka with robotiq gripper and specific set of cameras.
Subclasses must set ``self.action_config`` to a concrete action configuration.

``initial_pose`` / ``set_initial_pose`` set the base of the robot in world frame.
``stand_height_m`` sets the height of the stand mesh under the robot base link,
which changes how far the stand extends below the root link.
When manually placing the robot on floor, ``set_initial_pose`` z value and
``stand_height_m`` should be adjusted together to keep the bottom of stand fixed.
"""

name = "droid"
Expand All @@ -67,27 +78,17 @@ def __init__(
initial_joint_pose: list[float] | None = None,
concatenate_observation_terms: bool = False,
arm_mode: ArmMode | None = None,
stand_height_m: float = _DEFAULT_STAND_HEIGHT_M,
stand_height_m: float = _DROID_STAND_PRIM.stand_default_height,
):
super().__init__(enable_cameras, initial_pose, concatenate_observation_terms, arm_mode)
self.stand_height_m = stand_height_m
self.scene_config = DroidSceneCfg()
# ``stand_height_m`` is an absolute height in meters; convert it to the z-scale the USD needs.
stand_unit_height = _stand_unit_height_m(self.scene_config.stand.spawn.usd_path)
self.scene_config.stand.spawn.scale = (*_STAND_FOOTPRINT_SCALE_XY, stand_height_m / stand_unit_height)
# Lift the robot base (and stand) so a taller/shorter stand keeps its bottom on the floor.
self._robot_base_offset = (0.0, 0.0, stand_height_m - _DEFAULT_STAND_HEIGHT_M)
if self.initial_pose is None:
# No explicit base pose: lift the default robot and stand init states so the robot sits atop
# the lifted stand.
self.scene_config.robot.init_state.pos = translate_by_xyz_offset(
self.scene_config.robot.init_state.pos, self._robot_base_offset
)
self.scene_config.stand.init_state.pos = translate_by_xyz_offset(
self.scene_config.stand.init_state.pos, self._robot_base_offset
)
else:
# Explicit base pose: lift it via set_initial_pose; get_scene_cfg writes the scene config later.
self.set_initial_pose(self.initial_pose)
self.scene_config.robot.spawn.usd_path = compose_on_stand_usd(
Comment thread
qianl-nv marked this conversation as resolved.
_DROID_ROBOT_PRIM,
_DROID_STAND_PRIM,
stand_height_m=stand_height_m,
output_basename="droid_franka_robotiq_on_stand",
)
self.action_config = None
self.camera_config = DroidCameraCfg()
self.observation_config = DroidObservationsCfg()
Expand All @@ -98,32 +99,6 @@ def __init__(
self.mimic_env = None
self.add_camera_variations(self.camera_config)

def _translate_pose(self, pose: Pose | PosePerEnv) -> Pose | PosePerEnv:
"""Lift a base pose (or per-env poses) by the stand-height offset to match the spawned base."""
if isinstance(pose, PosePerEnv):
return PosePerEnv(poses=[p.translate(self._robot_base_offset) for p in pose.poses])
return pose.translate(self._robot_base_offset)

def set_initial_pose(self, pose: Pose | PosePerEnv, create_reset_event: bool = True) -> None:
"""Store the requested base pose(s), lifted by the stand-height offset to match the spawned base."""
super().set_initial_pose(self._translate_pose(pose), create_reset_event=create_reset_event)

def has_unplaced_auxiliary_prims(self) -> bool:
# Droid spawns a static stand prim that per-env reset does not yet reposition, so flag it and
# let the relation-placement guard reject a movable Droid rather than orphan the stand at env 0.
# TODO(zihaox): make the stand move with the base on reset, then drop this override. We can
# either override layout_pose_to_scene_writes to also write the stand's pose, or bake the stand
# into the robot USD so it moves with the base.
return True

def _update_scene_cfg_with_robot_initial_pose(self, scene_config: Any, pose: Pose) -> Any:
# ``pose`` is already lifted by the stand-height offset (see __init__ / set_initial_pose), so the
# base implementation sets the robot base as-is; we only add the stand placement here.
scene_config = super()._update_scene_cfg_with_robot_initial_pose(scene_config, pose)
scene_config.stand.init_state.pos = pose.position_xyz
scene_config.stand.init_state.rot = pose.rotation_xyzw
return scene_config

def set_initial_joint_pose(self, initial_joint_pose: list[float]) -> None:
self.event_config.init_franka_arm_pose.params["default_pose"] = initial_joint_pose

Expand All @@ -148,7 +123,7 @@ def __init__(
initial_joint_pose: list[float] | None = None,
concatenate_observation_terms: bool = False,
arm_mode: ArmMode | None = None,
stand_height_m: float = _DEFAULT_STAND_HEIGHT_M,
stand_height_m: float = _DROID_STAND_PRIM.stand_default_height,
):
super().__init__(
enable_cameras,
Expand All @@ -175,7 +150,7 @@ def __init__(
initial_joint_pose: list[float] | None = None,
concatenate_observation_terms: bool = False,
arm_mode: ArmMode | None = None,
stand_height_m: float = _DEFAULT_STAND_HEIGHT_M,
stand_height_m: float = _DROID_STAND_PRIM.stand_default_height,
):
super().__init__(
enable_cameras,
Expand Down Expand Up @@ -203,7 +178,7 @@ def __init__(
initial_joint_pose: list[float] | None = None,
concatenate_observation_terms: bool = False,
arm_mode: ArmMode | None = None,
stand_height_m: float = _DEFAULT_STAND_HEIGHT_M,
stand_height_m: float = _DROID_STAND_PRIM.stand_default_height,
):
super().__init__(
enable_cameras,
Expand All @@ -218,13 +193,17 @@ def __init__(

@configclass
class DroidSceneCfg:
"""Additions to the scene configuration coming from the Franka embodiment."""
"""Additions to the scene configuration coming from the Droid embodiment.

The robot USD path is overwritten at embodiment construction via
``compose_on_stand_usd`` (cached local robot+stand assembly).
"""

# The robot
# The robot (stand is baked into the local on-stand USD, not a separate prim).
robot: ArticulationCfg = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/droid/franka_robotiq_2f_85_flattened.usd",
usd_path=_DROID_ROBOT_PRIM.robot_usd_path,
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=True,
Expand Down Expand Up @@ -277,19 +256,6 @@ class DroidSceneCfg:
),
},
)
# The stand for the franka
# TODO(alexmillane, 2025-07-28): We probably want to make the stand an optional addition.
stand: AssetBaseCfg = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/Robot_Stand",
init_state=AssetBaseCfg.InitialStateCfg(pos=[-0.05, 0.0, 0.0], rot=[0.0, 0.0, 0.0, 1.0]),
spawn=UsdFileCfg(
usd_path=(
f"{ARENA_NUCLEUS_DIR}/Arena/assets/object_library/srl_robolab_assets/robots/franka_stand_grey.usda"
),
scale=(*_STAND_FOOTPRINT_SCALE_XY, _DEFAULT_STAND_HEIGHT_M / _FALLBACK_STAND_UNIT_HEIGHT_M),
activate_contact_sensors=False,
),
)

# The end-effector frame marker
ee_frame: FrameTransformerCfg = FrameTransformerCfg(
Expand Down Expand Up @@ -460,7 +426,7 @@ class DroidCameraCfg(ArenaCameraCfg):
"""Configuration for cameras. DROID cameras are mounted with pre-set poses."""

external_camera: CameraCfg = CameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/external_camera",
prim_path="{ENV_REGEX_NS}/Robot/panda_link0/external_camera",
height=720,
width=1280,
data_types=["rgb"],
Expand All @@ -473,7 +439,7 @@ class DroidCameraCfg(ArenaCameraCfg):
offset=CameraCfg.OffsetCfg(pos=(0.05, 0.57, 0.66), rot=(-0.195, 0.399, 0.805, -0.393), convention="opengl"),
)
external_camera_2: CameraCfg = CameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/external_camera_2",
prim_path="{ENV_REGEX_NS}/Robot/panda_link0/external_camera_2",
height=720,
width=1280,
data_types=["rgb"],
Expand All @@ -500,27 +466,3 @@ class DroidCameraCfg(ArenaCameraCfg):
pos=(0.011, -0.031, -0.074), rot=(0.570, 0.576, -0.409, -0.420), convention="opengl"
),
)


@functools.cache
def _stand_unit_height_m(usd_path: str) -> float:
"""Native (scale=1.0) z-height of the stand USD in meters, cached per asset path.

Falls back to ``_FALLBACK_STAND_UNIT_HEIGHT_M`` if the asset cannot be opened or measured.
"""
try:
from pxr import Usd, UsdGeom

stage = Usd.Stage.Open(usd_path)
assert stage is not None, f"could not open stand USD: {usd_path}"
root_prim = stage.GetDefaultPrim() or stage.GetPseudoRoot()
bound = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]).ComputeWorldBound(root_prim)
height = bound.ComputeAlignedRange().GetSize()[2]
assert height > 0.0, f"non-positive stand height {height} from {usd_path}"
return height
except Exception as exc: # noqa: BLE001 - any failure falls back to the measured constant
warnings.warn(
f"Falling back to {_FALLBACK_STAND_UNIT_HEIGHT_M} m for the stand height; "
f"could not measure {usd_path}: {exc!r}"
)
return _FALLBACK_STAND_UNIT_HEIGHT_M
45 changes: 31 additions & 14 deletions isaaclab_arena/embodiments/franka/franka.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,33 +26,38 @@
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.sensors import CameraCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
from isaaclab.utils.configclass import configclass
from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG, 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

from isaaclab_arena.assets.nucleus import ARENA_NUCLEUS_DIR
from isaaclab_arena.assets.register import register_asset
from isaaclab_arena.embodiments.common.arm_mode import ArmMode
from isaaclab_arena.embodiments.common.mimic_utils import get_rigid_and_articulated_object_poses
from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase
from isaaclab_arena.embodiments.franka.observations import gripper_pos
from isaaclab_arena.embodiments.robot_on_stand_utils import RobotPrimSpec, StandPrimSpec, compose_on_stand_usd
from isaaclab_arena.utils.cameras import ArenaCameraCfg
from isaaclab_arena.utils.pose import Pose

_DEFAULT_CAMERA_OFFSET = Pose(position_xyz=(0.11, -0.031, -0.074), rotation_xyzw=(0.0, 0.0, 0.70711, 0.70711))


# The reason to use our internal panda USD is to combine the panda and the stand within one USD.
# This is not ideal but currently required by the ObjectPlacementSolver to handle the robot placement correctly.
# TODO(cvolk): Move to the IsaacLab supported FRANKA_CFG and handle the handling of the stand internally.
_FRANKA_IK_REL_CFG = FRANKA_PANDA_HIGH_PD_CFG.copy()
_FRANKA_IK_REL_CFG.spawn.usd_path = f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/franka_panda_hand_on_stand.usd"

# Standard-PD Franka for joint-position control.
# Uses FRANKA_PANDA_CFG (gravity on, stiffness=80, damping=4) instead of HIGH_PD.
_FRANKA_JOINT_POS_CFG = FRANKA_PANDA_CFG.copy()
_FRANKA_JOINT_POS_CFG.spawn.usd_path = _FRANKA_IK_REL_CFG.spawn.usd_path
_FRANKA_ROBOT_PRIM = RobotPrimSpec(
# TODO(qianl): use FRANKA_PANDA_CFG spawn path once IsaacSim version updates to use Legacy path by default.
robot_usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/Legacy/panda_instanceable.usd",
root_prim_path="/panda",
robot_base_prim_name="panda_link0",
stand_prim_name="stand_instanceable",
)
_FRANKA_STAND_PRIM = StandPrimSpec(
stand_usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd",
ref_prim_path="/Stand",
payload_child_name="Stand",
footprint_translate_xyz=(-0.05, 0.0, 0.0),
footprint_scale_xy=(1.2, 1.2),
stand_default_height=0.8755,
)


class FrankaEmbodimentBase(EmbodimentBase):
Expand Down Expand Up @@ -113,7 +118,7 @@ def __init__(
concatenate_observation_terms=concatenate_observation_terms,
arm_mode=arm_mode,
)
self.scene_config.robot = _FRANKA_IK_REL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene_config.robot = _franka_robot_cfg_on_stand(FRANKA_PANDA_HIGH_PD_CFG.copy())
self.action_config = FrankaIKActionCfg()

def get_command_body_name(self) -> str:
Expand Down Expand Up @@ -167,7 +172,7 @@ def __init__(
arm_mode=arm_mode,
)
self.action_config = FrankaJointPosActionsCfg()
self.scene_config.robot = _FRANKA_JOINT_POS_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene_config.robot = _franka_robot_cfg_on_stand(FRANKA_PANDA_CFG.copy())

def get_command_body_name(self) -> str:
return "panda_hand"
Expand Down Expand Up @@ -449,3 +454,15 @@ def get_object_poses(self, env_ids: Sequence[int] | None = None):
object_pose_matrix = get_rigid_and_articulated_object_poses(state, env_ids)

return object_pose_matrix


def _franka_robot_cfg_on_stand(robot_cfg: ArticulationCfg) -> ArticulationCfg:
"""Copy ``robot_cfg`` onto ``{ENV_REGEX_NS}/Robot`` with the composed on-stand USD."""
cfg = robot_cfg.replace(prim_path="{ENV_REGEX_NS}/Robot")
Comment thread
qianl-nv marked this conversation as resolved.
cfg.spawn.usd_path = compose_on_stand_usd(
_FRANKA_ROBOT_PRIM,
_FRANKA_STAND_PRIM,
stand_height_m=_FRANKA_STAND_PRIM.stand_default_height,
output_basename="franka_panda_on_stand",
)
return cfg
Loading
Loading