diff --git a/examples/robosuite/custom_mjcf_table.scenic b/examples/robosuite/custom_mjcf_table.scenic
new file mode 100644
index 000000000..2717b8f54
--- /dev/null
+++ b/examples/robosuite/custom_mjcf_table.scenic
@@ -0,0 +1,87 @@
+# examples/robosuite/custom_mjcf_table.scenic
+"""Test with two XML tables and XML cubes on top, positioned in front of robot."""
+model scenic.simulators.robosuite.model
+
+# Get the directory of this scenic file for relative paths
+param scenic_file_dir = localPath(".")
+
+# Table 1 XML - a manipulable table object
+table_xml = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+# Red cube XML
+red_cube_xml = """
+
+
+
+
+
+
+
+
+
+"""
+
+# Green cube XML
+green_cube_xml = """
+
+
+
+
+
+
+
+
+
+"""
+
+
+# Create the first table (brown) - closer to robot
+table1 = new CustomObject on arena_floor,
+ with mjcfXml table_xml
+
+# Create the second table (blue) - farther from robot
+table2 = new CustomObject on arena_floor,
+ with mjcfXml table_xml
+
+
+red_cube = new CustomObject on table1,
+ with mjcfXml red_cube_xml
+
+# Place green cube on second table
+green_cube = new CustomObject on table2,
+ with mjcfXml green_cube_xml
+
+# Robot at origin
+ego = new Panda at (0, 0, 0)
\ No newline at end of file
diff --git a/examples/robosuite/custom_object/bread.stl b/examples/robosuite/custom_object/bread.stl
new file mode 100644
index 000000000..b69f87fc9
Binary files /dev/null and b/examples/robosuite/custom_object/bread.stl differ
diff --git a/examples/robosuite/custom_object/bread.xml b/examples/robosuite/custom_object/bread.xml
new file mode 100644
index 000000000..0aa4523ad
--- /dev/null
+++ b/examples/robosuite/custom_object/bread.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/examples/robosuite/custom_object/dragon.stl b/examples/robosuite/custom_object/dragon.stl
new file mode 100644
index 000000000..605f302a2
Binary files /dev/null and b/examples/robosuite/custom_object/dragon.stl differ
diff --git a/examples/robosuite/custom_object/textures/Dragon_ground_color.png b/examples/robosuite/custom_object/textures/Dragon_ground_color.png
new file mode 100644
index 000000000..37245b41d
Binary files /dev/null and b/examples/robosuite/custom_object/textures/Dragon_ground_color.png differ
diff --git a/examples/robosuite/custom_object/textures/bread.png b/examples/robosuite/custom_object/textures/bread.png
new file mode 100644
index 000000000..064ff13b3
Binary files /dev/null and b/examples/robosuite/custom_object/textures/bread.png differ
diff --git a/examples/robosuite/dual_table_workspace.scenic b/examples/robosuite/dual_table_workspace.scenic
new file mode 100644
index 000000000..c4824c746
--- /dev/null
+++ b/examples/robosuite/dual_table_workspace.scenic
@@ -0,0 +1,18 @@
+# examples/robosuite/dual_table_workspace.scenic
+model scenic.simulators.robosuite.model
+
+# Arena Setup
+back_table = new Table at (-0.6, 0, 0.8)
+front_table = new Table at (0.6, 0, 0.8)
+
+# OBJECTS
+red_cube = new Box at (Range(-0.7, -0.5), Range(-0.1, 0.1), 0.83),
+ with color (1, 0, 0, 1),
+
+
+green_cube = new Box at (Range(0.5, 0.7), Range(-0.1, 0.1), 0.83),
+ with color (0, 1, 0, 1),
+ with width 0.05, with length 0.05, with height 0.05
+
+# ROBOT
+ego = new Panda at (0, 0, 0)
\ No newline at end of file
diff --git a/examples/robosuite/file_xml.scenic b/examples/robosuite/file_xml.scenic
new file mode 100644
index 000000000..1a7897c12
--- /dev/null
+++ b/examples/robosuite/file_xml.scenic
@@ -0,0 +1,16 @@
+# examples/robosuite/file_xml.scenic
+"""Test loading CustomObjects from XML files with mesh and texture assets."""
+model scenic.simulators.robosuite.model
+
+param scenic_file_dir = localPath(".")
+
+# Robot at origin
+ego = new Panda on (0, 0, 0)
+
+# Use built-in Table object
+table1 = new Table on arena_floor
+
+# Load bread object from XML file
+bread = new CustomObject on table1,
+ with mjcfXml "custom_object/bread.xml",
+ with color (0, 1, 1, 1)
\ No newline at end of file
diff --git a/examples/robosuite/full_xml_test3.scenic b/examples/robosuite/full_xml_test3.scenic
new file mode 100644
index 000000000..72f83ffa8
--- /dev/null
+++ b/examples/robosuite/full_xml_test3.scenic
@@ -0,0 +1,77 @@
+# examples/robosuite/full_xml.scenic
+"""Currently No support for custom_arena_xml. fix: WIP"""
+"""Test custom arena and custom objects with complete MJCF XML."""
+model scenic.simulators.robosuite.model
+
+# Define the complete custom arena XML
+custom_arena_xml = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+# Define the ball object XML
+ball_object_xml = """
+
+
+
+
+
+
+
+
+
+"""
+
+# Create the custom arena
+arena = new CustomArena,
+ with arena_xml custom_arena_xml
+
+# Create a yellow ball using custom XML
+yellow_ball = new CustomObject at (0.5, 0.0, 1.0),
+ with mjcfXml ball_object_xml
+
+# Create a red box using built-in Box class for comparison
+red_box = new Box at (0.7, 0.2, 1.0),
+ with color (1, 0, 0, 1),
+ with width 0.04, with length 0.04, with height 0.04
+
+# Create the Panda robot
+ego = new Panda at (-0.5, 0, 0)
\ No newline at end of file
diff --git a/examples/robosuite/mesh_test.scenic b/examples/robosuite/mesh_test.scenic
new file mode 100644
index 000000000..5a6494e3a
--- /dev/null
+++ b/examples/robosuite/mesh_test.scenic
@@ -0,0 +1,54 @@
+# examples/robosuite/mesh_test.scenic
+"""Test custom mesh objects with relative paths in XML."""
+model scenic.simulators.robosuite.model
+
+param scenic_file_dir = localPath(".")
+
+# Dragon object XML - uses relative path to mesh file
+dragon_object_xml = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+# Use default table arena
+table = new Table on arena_floor
+
+# Create dragon - path in XML is relative to this .scenic file
+dragon = new CustomObject on table,
+ with mjcfXml dragon_object_xml,
+ with color (1, 0, 1, 1)
+
+# Add a simple ball for comparison
+ball = new Ball on table,
+ with color (0, 1, 0, 1),
+ with radius 0.03
+
+# Robot
+ego = new Panda on (0, 0, 0)
\ No newline at end of file
diff --git a/examples/robosuite/spatial_operators_demo.scenic b/examples/robosuite/spatial_operators_demo.scenic
new file mode 100644
index 000000000..cb26b62e2
--- /dev/null
+++ b/examples/robosuite/spatial_operators_demo.scenic
@@ -0,0 +1,78 @@
+# examples/robosuite/spatial_operators_demo.scenic
+""" WIP """
+
+"""
+Demonstration of spatial operators with mesh-aware RoboSuite objects.
+This example shows how Scenic can now use 'on' and other spatial operators
+with RoboSuite objects thanks to mesh extraction.
+"""
+model scenic.simulators.robosuite.model
+
+# Enable mesh extraction for spatial awareness
+param extract_meshes = True
+param scenic_file_dir = localPath(".")
+param render = True
+param camera_view = "frontview"
+
+# Robot at origin
+ego = new Panda at (0, 0, 0)
+
+# Create a table - Scenic knows its surface geometry
+main_table = new Table at (0.6, 0, 0.8),
+ with width 1.0,
+ with length 0.8
+
+# Red box at table surface (Table position is already at surface height)
+red_box = new Box at (0.6, -0.2, 0.85),
+ with color (1, 0, 0, 1),
+ with width 0.05, with length 0.05, with height 0.05
+
+# Green ball also at table surface
+green_ball = new Ball at (0.7, 0.1, 0.85),
+ with color (0, 1, 0, 1),
+ with radius 0.03
+
+# Blue cylinder on top of the red box - stacking works now
+blue_cylinder = new Cylinder on red_box, # Stack on top of box
+ with color (0, 0, 1, 1),
+ with width 0.04, with length 0.04, with height 0.08
+
+# Custom object with mesh
+custom_xml = """
+
+
+
+
+
+
+
+
+
+"""
+
+# Yellow block at table surface
+yellow_block = new CustomObject at (0.5, 0.2, 0.85),
+ with mjcfXml custom_xml
+
+# Complex object (Milk carton) at table surface
+milk = new Milk at (0.8, -0.1, 0.85)
+
+# Spatial constraint - require objects to be visible from robot
+require ego can see red_box
+require ego can see green_ball
+
+# Behavior using spatial awareness
+behavior PickupSequence():
+ """Pick up objects in sequence based on their spatial positions."""
+ # Check which object is closer
+ if distance to red_box < distance to green_ball:
+ do PickAndLift(red_box, height=1.1)
+ else:
+ do PickAndLift(green_ball, height=1.1)
+
+ego.behavior = PickupSequence()
\ No newline at end of file
diff --git a/examples/robosuite/stack_lift.scenic b/examples/robosuite/stack_lift.scenic
new file mode 100644
index 000000000..c3ac17491
--- /dev/null
+++ b/examples/robosuite/stack_lift.scenic
@@ -0,0 +1,30 @@
+# examples/robosuite/stack_lift.scenic
+model scenic.simulators.robosuite.model
+
+work_table = new Table on (0.6, 0, 0.8),
+ with width 0.6,
+ with length 1.2,
+ with height 0.05
+
+bottom_cube = new Box on work_table,
+ with color (0.2, 0.3, 0.8, 1),
+ with width 0.06, with length 0.06, with height 0.06
+
+top_cube = new Box on bottom_cube,
+ with color (0.8, 0.2, 0.2, 1)
+
+
+pickup_object = top_cube
+
+behavior StackLift():
+ """Pick up the top cube from the stack and lift it."""
+ do PickObject(pickup_object)
+ do LiftToHeight(1.05)
+ for _ in range(10):
+ if pickup_object.position.z > 1.0:
+ terminate simulation
+ wait
+ terminate simulation
+
+ego = new UR5e on (0, 0, 0),
+ with behavior StackLift()
\ No newline at end of file
diff --git a/examples/robosuite/test_dynamic_properties.scenic b/examples/robosuite/test_dynamic_properties.scenic
new file mode 100644
index 000000000..05f88200a
--- /dev/null
+++ b/examples/robosuite/test_dynamic_properties.scenic
@@ -0,0 +1,27 @@
+# test_simple_dynamics.scenic
+model scenic.simulators.robosuite.model
+
+param render = True
+param real_time = False
+
+# Single robot
+behavior MonitorProperties():
+ """Monitor dynamic properties."""
+ for step in range(30):
+ print(f"\nStep {step}:")
+ print(f" Joint positions: {self.jointPositions[:3]}")
+ print(f" EEF position: {self.eefPos}")
+ print(f" Gripper: {self.gripperState}")
+
+ # Simple movement
+ if step < 10:
+ take OSCPositionAction(position_delta=[0, 0, 0.01]) # Up
+ elif step < 20:
+ take OSCPositionAction(position_delta=[0.01, 0, 0]) # Forward
+ else:
+ take OSCPositionAction(gripper=1) # Close gripper
+
+ terminate simulation
+
+ego = new Panda on (0, 0, 0),
+ with behavior MonitorProperties()
\ No newline at end of file
diff --git a/examples/robosuite/test_free_joint_tracking.scenic b/examples/robosuite/test_free_joint_tracking.scenic
new file mode 100644
index 000000000..e3c33072b
--- /dev/null
+++ b/examples/robosuite/test_free_joint_tracking.scenic
@@ -0,0 +1,37 @@
+# test_free_joint_tracking.scenic
+model scenic.simulators.robosuite.model
+
+# Table at standard height
+table = new Table at (0.6, 0, 0.425)
+
+# Box that will fall and tumble
+falling_box = new Box at (0.6, 0, 1.2),
+ with color (1, 0, 0, 1)
+
+# Box on table (should stay still)
+stable_box = new Box on table,
+ with color (0, 1, 0, 1)
+
+behavior TrackFreeBodies():
+ """Track positions and velocities of free-joint objects."""
+ print("Initial state:")
+ print(f" Falling box: pos={falling_box.position}, vel={falling_box.velocity}")
+ print(f" Stable box: pos={stable_box.position}, vel={stable_box.velocity}")
+
+ for step in range(20):
+ if step % 5 == 0:
+ print(f"\nStep {step}:")
+ print(f" Falling: z={falling_box.position.z:.3f}, vel_z={falling_box.velocity.z:.3f}")
+ print(f" Stable: z={stable_box.position.z:.3f}, vel_z={stable_box.velocity.z:.3f}")
+ wait
+
+ # Verify tracking worked
+ if falling_box.position.z < 1.0:
+ print("\n✓ Free joint tracking working - falling box moved")
+ else:
+ print("\n✗ Free joint tracking failed - box didn't fall")
+
+ terminate simulation
+
+ego = new Panda on (0, 0, 0),
+ with behavior TrackFreeBodies()
\ No newline at end of file
diff --git a/examples/robosuite/test_object_rotation.scenic b/examples/robosuite/test_object_rotation.scenic
new file mode 100644
index 000000000..4b3b8050f
--- /dev/null
+++ b/examples/robosuite/test_object_rotation.scenic
@@ -0,0 +1,38 @@
+# test_simple_rotation.scenic
+model scenic.simulators.robosuite.model
+
+# Create table
+table = new Table at (0.6, 0, 0.425)
+
+# Test different rotations with boxes at different positions
+box1 = new Box at (0.4, 0, 0.88),
+ with color (1, 0, 0, 1),
+ facing (0, 0, 0) # No rotation
+
+box2 = new Box at (0.6, 0, 0.88),
+ with color (0, 1, 0, 1),
+ facing (45 deg, 0, 0) # 45 degree yaw
+
+box3 = new Box at (0.8, 0, 0.88),
+ with color (0, 0, 1, 1),
+ facing (0, 30 deg, 0) # 30 degree pitch
+
+behavior CheckRotations():
+ """Verify rotations are applied correctly."""
+ print("Initial rotations (should match what was specified):")
+ print(f" Box1 (red): yaw={box1.yaw:.2f}, pitch={box1.pitch:.2f}, roll={box1.roll:.2f}")
+ print(f" Box2 (green): yaw={box2.yaw:.2f}, pitch={box2.pitch:.2f}, roll={box2.roll:.2f}")
+ print(f" Box3 (blue): yaw={box3.yaw:.2f}, pitch={box3.pitch:.2f}, roll={box3.roll:.2f}")
+
+ for _ in range(5):
+ wait
+
+ print("\nAfter 5 steps (should remain the same):")
+ print(f" Box1 (red): yaw={box1.yaw:.2f}, pitch={box1.pitch:.2f}, roll={box1.roll:.2f}")
+ print(f" Box2 (green): yaw={box2.yaw:.2f}, pitch={box2.pitch:.2f}, roll={box2.roll:.2f}")
+ print(f" Box3 (blue): yaw={box3.yaw:.2f}, pitch={box3.pitch:.2f}, roll={box3.roll:.2f}")
+
+ terminate simulation
+
+ego = new Panda on (0, 0, 0),
+ with behavior CheckRotations()
\ No newline at end of file
diff --git a/examples/robosuite/test_velocity_debug.scenic b/examples/robosuite/test_velocity_debug.scenic
new file mode 100644
index 000000000..de1c3a09e
--- /dev/null
+++ b/examples/robosuite/test_velocity_debug.scenic
@@ -0,0 +1,19 @@
+# test_velocity_debug.scenic
+model scenic.simulators.robosuite.model
+
+falling_box = new Box at (0.6, 0, 1.2),
+ with color (1, 0, 0, 1)
+
+behavior DebugVelocity():
+ prev_pos = falling_box.position
+ for step in range(10):
+ curr_pos = falling_box.position
+ # Manual velocity calculation
+ manual_vel = (curr_pos.z - prev_pos.z) / 0.01 # assuming 0.01s timestep
+ print(f"Step {step}: z={curr_pos.z:.3f}, vel_from_sim={falling_box.velocity.z:.3f}, manual_vel={manual_vel:.3f}")
+ prev_pos = curr_pos
+ wait
+ terminate simulation
+
+ego = new Panda on (0, 0, 0),
+ with behavior DebugVelocity()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/STATUS.md b/src/scenic/simulators/robosuite/STATUS.md
new file mode 100644
index 000000000..89df0c6f1
--- /dev/null
+++ b/src/scenic/simulators/robosuite/STATUS.md
@@ -0,0 +1,276 @@
+# RoboSuite-Scenic Interface Documentation
+
+## Status: Prototype
+
+### Overview
+This interface enables Scenic scenarios to run in the RoboSuite robotics simulator, allowing declarative specification of robotic manipulation tasks with physics simulation.
+
+---
+
+## Working Features
+
+### Scene Creation
+- **Robot Support**: Panda, UR5e, Jaco, IIWA robots
+- **Basic Objects**: Box, Ball, Cylinder, Capsule primitives
+- **Custom Objects**: MJCF XML with automatic collision generation
+- **Tables**: Standard table support with MultiTableArena
+- **Custom Arenas**: Full MJCF arena definitions
+
+### Dynamic Simulation
+- **Position Tracking**: Object positions update correctly from physics
+- **Orientation Tracking**: Yaw/pitch/roll from MuJoCo quaternions
+- **Robot Properties**: Joint positions, end-effector position, gripper state
+- **Behaviors**: Pick-and-place behaviors with OSC control
+- **Collision Detection**: Automatic collision geometry for all objects
+
+### Asset Management
+- **Mesh Loading**: STL, OBJ with automatic repair
+- **Texture Support**: PNG, JPG (auto-converted)
+- **Path Resolution**:
+ - Embedded XML: Paths relative to Scenic file location
+ - External XML: Paths relative to XML file location (see [xml-file-path.md](src/scenic/simulators/robosuite/docs/xml-file-path.md))
+
+---
+
+## Partially Working
+
+### Velocity Tracking
+- **Status**: Returns zero for free-joint objects
+- **Root Cause**: Incorrect access to MuJoCo's `data.qvel` array
+- **Attempted Fixes**: Tried both `data.cvel` and `data.qvel` approaches
+- **Known Bug**: Currently no working implementation for velocity tracking
+- **Details**: See [velocity_issue.md](src/scenic/simulators/robosuite/docs/velocity_issue.md) for full investigation
+
+### Initial Orientations
+- **Status**: `facing` specifier ignored, objects start at zero rotation
+- **Workaround**: Use behaviors to set orientation after creation
+
+### Table Dimensions
+- **Issue**: Height hardcoded at 0.85m, changing causes visual/physics mismatch
+- **Root Cause**: MultiTableArena design deadlock between Scenic and RoboSuite
+- **Details**: See [table-rendering-issue.md](src/scenic/simulators/robosuite/docs/table-rendering-issue.md)
+
+---
+
+## Known Issues
+
+### Critical Bugs
+1. **Free-joint velocity always zero**
+ - MuJoCo `qvel` access not working for manipulable objects
+ - No current workaround available
+ - Velocity property returns incorrect zero values
+
+2. **Multi-robot action handling**
+ - Single `pending_robot_action` variable overwrites with multiple robots
+ - Causes action dimension mismatch errors
+ - **Solution designed**: See [multi-robot_handling.md](src/scenic/simulators/robosuite/docs/multi-robot_handling.md)
+
+3. **Segmentation fault on exit**
+ - Viewer cleanup issue
+ - Occurs after simulation completes
+
+### Memory & Performance
+- **Memory leak**: Temporary directories not always cleaned
+- **Import overhead**: Fixed - RoboSuite now only imports when needed (see [import_fix.md](src/scenic/simulators/robosuite/docs/import_fix.md))
+
+### Limitations
+- No sensor support (cameras, force sensors)
+- Default arena components conflict with custom arenas
+- Table dimensions cannot be dynamically modified
+
+---
+
+## Example Files Guide
+
+### `examples/robosuite/custom_mjcf_table.scenic`
+**Tests**: Custom MJCF XML object creation
+- Creates tables as manipulable objects (incorrect approach)
+- Demonstrates embedded XML with primitive geometries
+- **Issues Found**:
+ - Tables created with free joints (fall over)
+ - Missing collision geometry
+
+### `examples/robosuite/dual_table_workspace.scenic`
+**Tests**: Multiple table placement
+- Two standard tables with objects
+- Random object placement using Range
+- **Issues Found**:
+ - Table height positioning error (z=0.8 instead of 0.425)
+ - Object placement height mismatch
+ - Use of `at` causes collision issues
+
+### `examples/robosuite/file_xml.scenic`
+**Tests**: External XML file loading
+- Loads MJCF from `custom_object/bread.xml`
+- Path resolution from Scenic file location
+- Standard Table usage (correct approach)
+
+### `examples/robosuite/full_xml.scenic`
+**Tests**: Custom arena with complete MJCF
+- Full arena definition with floor, lights, camera
+- Custom arena property (`arenaXml`)
+- **Issues Found**:
+ - Property name typo (`arena_xml` should be `arenaXml`)
+ - Conflicts with default arena components
+ - Missing collision geometry in ball object
+
+### `examples/robosuite/mesh_test.scenic`
+**Tests**: Mesh file loading (STL)
+- Loads dragon mesh from `custom_object/dragon.stl`
+- Tests relative path resolution in MJCF
+- Mesh scaling and material application
+
+### `examples/robosuite/spatial_operators_demo.scenic`
+**Tests**: Spatial operators and object stacking
+- Object stacking with `on` operator
+- Visibility requirements (`can see`)
+- Distance-based behavior decisions
+- **Issues Found**:
+ - Table positioning error
+ - Undefined `Milk` class
+ - Unused `extract_meshes` parameter
+
+### `examples/robosuite/stack_lift.scenic`
+**Tests**: Pick and lift behavior
+- Object stacking (box on box)
+- PickObject and LiftToHeight behaviors
+- Conditional simulation termination
+- **Issues Found**:
+ - Table height override problem
+
+---
+
+## File Structure & Utilities
+
+### Static Assets (`src/scenic/simulators/robosuite/utils/`)
+Pre-extracted 3D meshes for Scenic visualization. These allow Scenic to render objects without needing RoboSuite running.
+
+#### `arena_meshes/`
+- `floor.glb`, `walls.glb` - Default arena components
+- `arena_config.json` - Dimensions and metadata
+- Generated by: `extract_arena.py`
+
+#### `robot_meshes/`
+- `Panda.glb`, `UR5e.glb`, `Jaco.glb`, `IIWA.glb` - Robot models
+- `dimensions.txt` - Robot bounding box dimensions
+- Generated by: `extract_robots.py`
+
+#### `table_meshes/`
+- `standard_table.glb` - Default table mesh (fixed dimensions)
+- Note: Cannot be dynamically adjusted due to MultiTableArena constraints
+- Generated by: `extract_table.py`
+
+#### `table_components/`
+- Individual table parts for future composite mesh support
+- Currently unused - waiting for Scenic composite mesh feature
+- Generated by: `extract_table_components.py`
+
+### Extraction Scripts (`src/scenic/simulators/robosuite/utils/`)
+These scripts extract geometry from RoboSuite's runtime buffers to create static meshes. Unlike the main interface (which parses MJCF XML), these access RoboSuite's compiled models directly.
+
+#### Key Scripts
+- **`mjcf_to_mesh.py`**: Standalone converter using trimesh primitives
+- **`extract_*.py`**: Runtime extraction from RoboSuite buffers
+- **`default.scenic`**: Test file to verify extracted mesh rendering
+
+---
+
+## Basic Usage
+
+### Minimal Example
+```scenic
+model scenic.simulators.robosuite.model
+
+table = new Table at (0.6, 0, 0.425)
+box = new Box on table, with color (1, 0, 0, 1)
+ego = new Panda on (0, 0, 0)
+
+behavior PickBox():
+ do PickAndLift(box, height=1.1)
+
+ego = new Panda on (0, 0, 0),
+ with behavior PickBox()
+```
+
+### Common Pitfalls to Avoid
+1. **Positioning specifiers**:
+ - Use `on` for robots and ground-level objects to avoid collision issues
+ - `at` positions by center, causing robots to appear inside ground in Scenic's visualization
+ - Tables can use `at` since they're elevated, but ensure correct z-coordinate (0.425)
+2. **Table position**: Always use z=0.425 for Table center
+3. **Custom tables**: Use built-in Table class, not CustomObject
+4. **Robot creation**: Use `on` for positioning to avoid ground collision issues
+
+### Running Simulations
+```bash
+# Compile only (no RoboSuite import)
+scenic my_scenario.scenic
+
+# Run simulation
+scenic my_scenario.scenic --simulate --count 1
+```
+
+### Regenerating Static Assets
+```bash
+cd src/scenic/simulators/robosuite/utils
+python extract_robots.py
+python extract_arena.py
+python extract_table.py
+```
+
+---
+
+## Required Fixes
+
+### High Priority
+- Fix velocity fetching from `data.qvel` for free joints
+- Implement multi-robot action handling (design in src/scenic/simulators/robosuite/docs/)
+- Resolve segmentation fault on cleanup
+
+### Medium Priority
+- Apply initial orientations from `facing` specifier
+- Remove hardcoded table dimensions (needs Scenic composite mesh support)
+- Improve error messages and exception handling
+- Fix example files (incorrect syntax usage)
+
+### Future Work
+- Sensor integration (RGB, depth, force)
+- Support for joint constraints
+- erformance optimization for mesh loading
+- Comprehensive test suite
+
+---
+
+## Technical Details
+
+### Why Static Mesh Extraction?
+RoboSuite builds complex objects at runtime from multiple components. To enable Scenic's collision checking and visualization without running RoboSuite, we pre-extract these as single meshes. This avoids the circular dependency where Scenic needs geometry for collision checking before RoboSuite runs.
+
+### Known Issues
+
+1. **Velocity Data Access Pattern**
+ - RoboSuite stores velocities in `qvel` array indexed by joint DOF
+ - Our joint name lookup fails to find the correct mapping
+ - Needs investigation of RoboSuite's joint naming conventions
+
+2. **Table Rendering Deadlock**
+ - MultiTableArena expects only thickness parameter
+ - Scenic needs full geometry before simulation
+ - Cannot synchronize dynamically without composite mesh support
+
+3. **Multi-Robot Control**
+ - Current single action buffer design
+ - Needs per-robot action tracking
+ - Must handle different action dimensions per robot type
+
+---
+
+
+## Documentation Files
+- `src/scenic/simulators/robosuite/docs/velocity_issue.md` - Detailed velocity tracking investigation
+- `src/scenic/simulators/robosuite/docs/table-rendering-issue.md` - Table dimension synchronization problem
+- `src/scenic/simulators/robosuite/docs/multi-robot_handling.md` - Proposed multi-robot solution
+- `src/scenic/simulators/robosuite/docs/xml-file-path.md` - Path resolution behavior
+- `src/scenic/simulators/robosuite/docs/import_fix.md` - RoboSuite import optimization
+
+---
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/__init__.py b/src/scenic/simulators/robosuite/__init__.py
new file mode 100644
index 000000000..287e36abe
--- /dev/null
+++ b/src/scenic/simulators/robosuite/__init__.py
@@ -0,0 +1,5 @@
+"""RoboSuite simulator interface for Scenic."""
+
+from .simulator import RobosuiteSimulator, RobosuiteSimulation
+
+__all__ = ['RobosuiteSimulator', 'RobosuiteSimulation']
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/docs/import_fix.md b/src/scenic/simulators/robosuite/docs/import_fix.md
new file mode 100644
index 000000000..e877c569e
--- /dev/null
+++ b/src/scenic/simulators/robosuite/docs/import_fix.md
@@ -0,0 +1,32 @@
+## Fix Documentation: Preventing RoboSuite Import During Compilation
+
+**Problem:** RoboSuite was importing when running `scenic` without `-S` flag, causing warnings even when only compiling scenarios.
+
+**Root Cause:** `simulator.py` had RoboSuite imports at module level, executing when `model.scenic` imported from it.
+
+**Solution:** Moved all RoboSuite imports inside `RobosuiteSimulator.__init__()` to defer loading until simulator creation.
+
+### Changes Made:
+
+1. **Removed module-level imports** in `simulator.py`:
+ - Deleted the try/except block importing RoboSuite
+ - Removed global `_ROBOSUITE_AVAILABLE` check
+
+2. **Deferred imports to runtime** in `RobosuiteSimulator.__init__()`:
+ ```python
+ def __init__(self, ...):
+ # Import RoboSuite only when creating simulator
+ try:
+ global mujoco, suite
+ import mujoco
+ import robosuite as suite
+ except ImportError as e:
+ raise RuntimeError(f"Unable to import RoboSuite: {e}")
+ ```
+
+3. **Kept deferred class definitions** (`_get_arena_class()`, `_get_object_class()`, `_get_env_class()`) which are called in `RobosuiteSimulation.__init__()`
+
+4. **Added fallback in model.scenic** (already done):
+ - Try/except handles case when RoboSuite isn't installed at all
+
+**Result:** RoboSuite only imports when running with `-S` flag, eliminating warnings during scenario compilation.
diff --git a/src/scenic/simulators/robosuite/docs/multi-robot_handelling.md b/src/scenic/simulators/robosuite/docs/multi-robot_handelling.md
new file mode 100644
index 000000000..f9301d5e0
--- /dev/null
+++ b/src/scenic/simulators/robosuite/docs/multi-robot_handelling.md
@@ -0,0 +1,77 @@
+## Multi-Robot Action Handling Limitation
+
+### Current Limitation
+The current implementation only supports single-robot action execution despite allowing multiple robots in the scene. The `pending_robot_action` is a single variable that gets overwritten if multiple robots take actions.
+
+```python
+# Current: Single action buffer
+self.pending_robot_action = None # Only stores one robot's action
+```
+
+### Why This Exists
+- Originally developed and tested with single-robot environments
+- Multi-robot scene support was added later for positioning/visualization
+- Action handling wasn't updated to support concurrent control
+
+### Proposed Solution for Future Implementation
+
+```python
+class RobosuiteSimulation(Simulation):
+ def __init__(self, ...):
+ # Change from single action to per-robot actions
+ self.pending_robot_actions = {} # Dict[robot_idx: action_array]
+ self.robot_action_dims = {} # Dict[robot_idx: action_dim]
+
+ def setup(self):
+ """Initialize per-robot action dimensions."""
+ # ... existing setup ...
+
+ # Track action dimension per robot
+ for i, robot in enumerate(self.robosuite_env.robots):
+ if hasattr(robot, 'controller') and hasattr(robot.controller, 'control_dim'):
+ self.robot_action_dims[i] = robot.controller.control_dim
+ else:
+ self.robot_action_dims[i] = DEFAULT_ACTION_DIM
+
+ def executeActions(self, allActions):
+ """Clear all pending actions before parent executes."""
+ self.pending_robot_actions.clear()
+ super().executeActions(allActions)
+
+ def step(self):
+ """Step with multi-robot action support."""
+ # Build concatenated action vector for all robots
+ full_action = []
+ for i in range(len(self.robots)):
+ if i in self.pending_robot_actions:
+ action = self.pending_robot_actions[i]
+ else:
+ action = np.zeros(self.robot_action_dims.get(i, DEFAULT_ACTION_DIM))
+ full_action.extend(action)
+
+ action_array = np.array(full_action) if full_action else np.zeros(sum(self.robot_action_dims.values()))
+
+ # Pass concatenated actions to RoboSuite
+ obs, reward, done, info = self.robosuite_env.step(action_array)
+```
+
+### Action Classes Update
+```python
+class SetJointPositions(Action):
+ def applyTo(self, agent, sim):
+ if agent in sim.robots:
+ robot_idx = sim.robots.index(agent)
+ # Store action for specific robot
+ sim.pending_robot_actions[robot_idx] = np.array(self.positions)
+```
+
+### Testing Requirements
+- Single robot scenarios must remain compatible
+- Test with 2+ robots taking simultaneous actions
+- Verify action dimensions match controller expectations
+- Test mixed robot types (Panda + UR5e) with different action dims
+
+### Known Complexities
+- Different robots may have different action dimensions
+- RoboSuite expects concatenated action vector in robot creation order
+- Controller types affect action dimension (OSC=7, Joint Position varies)
diff --git a/src/scenic/simulators/robosuite/docs/table-rendering-issue.md b/src/scenic/simulators/robosuite/docs/table-rendering-issue.md
new file mode 100644
index 000000000..4ec7f769f
--- /dev/null
+++ b/src/scenic/simulators/robosuite/docs/table-rendering-issue.md
@@ -0,0 +1,53 @@
+# Table Rendering Issue Documentation
+
+## Issue Summary
+Tables appear correctly in robosuite but have dimension mismatches in Scenic's internal viewer when height is adjusted. Setting `height: 0.85` (full table height) causes robosuite to render a white block, while `height: 0.05` (tabletop thickness) causes Scenic to show a squished table.
+
+## Technical Root Cause
+
+### MultiTableArena Design
+Robosuite's `MultiTableArena` provides full control over table elements as parameters without making tables free-joint objects. This design:
+- Prevents tables from being physics-enabled manipulable objects
+- Allows efficient arena-level table management
+- Takes only tabletop thickness as the "size" parameter, not full height
+
+### The Deadlock Problem
+Synchronizing table rendering between simulators would require:
+1. Running robosuite first to generate table geometry with Scenic configuration
+2. Exporting that geometry back to Scenic for matching visualization
+3. But Scenic needs the geometry *before* robosuite runs for collision checking
+
+This circular dependency creates a deadlock that prevents dynamic synchronization.
+
+## Current Solution
+Static GLB files pre-built for standard table configurations:
+- `utils/table_meshes/standard_table.glb` - Fixed geometry for Scenic
+- Hardcoded thickness (0.05m) passed to robosuite's MultiTableArena
+- Height property fixed at 0.85m for Scenic collision detection
+
+## Limitations
+
+### Dimension Mismatches
+- **Height**: Primary breaking feature - changing causes visual inconsistencies
+- **Width/Length**: Extreme values cause leg stretching in Scenic (not replicated in robosuite)
+- Static nature means robosuite renders correctly but changes don't reflect in Scenic
+
+### Configuration Restrictions
+Users cannot customize table dimensions without breaking visual consistency between simulators.
+
+## Potential Future Solution
+Composite mesh assembly in Scenic:
+- Separate meshes for tabletop and legs
+- Unified into single object while retaining component individuality
+- Would allow dynamic dimension adjustments
+
+**Current blocker**: Scenic lacks this composite mesh feature where multiple meshes can be unified into one object with components maintaining their individual properties.
+
+## Implementation Notes
+```python
+# Fixed configuration in model.scenic
+height: 0.85 # DO NOT MODIFY - Full table for Scenic collision
+
+# Hardcoded in simulator.py
+'size': (obj.width, obj.length, 0.05) # FIXED - MultiTableArena thickness only
+```
diff --git a/src/scenic/simulators/robosuite/docs/velocity_issue.md b/src/scenic/simulators/robosuite/docs/velocity_issue.md
new file mode 100644
index 000000000..6547ba7c4
--- /dev/null
+++ b/src/scenic/simulators/robosuite/docs/velocity_issue.md
@@ -0,0 +1,111 @@
+# RoboSuite Dynamic Property Fetching - Implementation Status and Issues
+
+## Overview
+This document details the implementation of dynamic property fetching from RoboSuite/MuJoCo for Scenic objects, addressing the PR comment: "This doesn't properly update the other dynamic properties like yaw, pitch, roll, velocity, etc. -- those need to have their current values looked up in robosuite."
+
+## Successfully Implemented Properties
+
+### Position Tracking ✅
+- **Method**: `_get_object_position()`
+- **Implementation**: Fetches from `data.xpos[body_id]`
+- **Status**: Working correctly for all object types
+
+### Orientation Properties ✅
+- **Methods**: `_get_object_orientation()` for yaw, pitch, roll
+- **Implementation**:
+ - Fetches quaternion from `data.xquat[body_id]`
+ - Converts quaternion [w,x,y,z] to Euler angles (yaw, pitch, roll)
+ - Uses proper ZXY rotation order for Scenic compatibility
+- **Status**: Working correctly
+
+### Robot-Specific Properties ✅
+- **Properties**: jointPositions, eefPos, gripperState
+- **Implementation**: Fetches from RoboSuite's robot observations
+- **Status**: Working correctly
+
+## Unresolved Issue: Velocity Tracking for Free-Joint Objects
+
+### Problem Description
+Velocity and angular velocity properties return zero for manipulable objects with free joints, even when objects are clearly moving (e.g., falling boxes).
+
+### Test Results
+```
+Manual calculation: -30.866 m/s (actual falling velocity)
+vel_from_sim: 0.000 (incorrect)
+```
+
+### Attempted Solutions
+
+#### Attempt 1: Using `data.cvel` (Original Implementation)
+```python
+def _get_object_velocity(self, obj) -> Vector:
+ body_id = self.body_id_map[obj_name]
+ vel = self.data.cvel[body_id][:3]
+ return Vector(vel[0], vel[1], vel[2])
+```
+**Result**: Always returns zero. `data.cvel` is for composite rigid bodies in contact, not free-floating joints.
+
+#### Attempt 2: Using `data.qvel` with Joint Lookup
+```python
+def _get_object_velocity(self, obj) -> Vector:
+ # Find joint for this object
+ for mobj in self.robosuite_env.mujoco_objects:
+ if mobj.name == obj_name:
+ joint_name = mobj.joint_name
+ break
+
+ # Get velocity from qvel
+ joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
+ qvel_addr = self.model.jnt_dofadr[joint_id]
+ vel = self.data.qvel[qvel_addr:qvel_addr+3]
+ return Vector(vel[0], vel[1], vel[2])
+```
+**Result**: Still returns zero. Possible issues:
+- Joint name lookup may not be finding the correct joint
+- `mujoco_objects` list might not be accessible at the right time
+- Indexing into `qvel` might be incorrect
+
+### Root Cause Analysis
+The issue appears to be in how RoboSuite sets up free joints for manipulable objects and how we're accessing their velocity data. Possible causes:
+
+1. **Joint Name Mismatch**: The joint names we're looking for might not match what's actually in the model
+2. **Timing Issue**: The `mujoco_objects` list might not be populated when we need it
+3. **Index Calculation**: The `qvel` indices for free joints might need different calculation
+4. **RoboSuite Integration**: There might be a mismatch between how RoboSuite manages these joints internally
+
+### Workaround
+For scenarios requiring velocity information, manual calculation from position differences works:
+```python
+manual_velocity = (current_pos - previous_pos) / timestep
+```
+
+## Initial Orientation Setting Issue
+
+### Secondary Issue Discovered
+Objects don't respect initial orientations set via `facing` specifier.
+
+### Attempted Fix
+```python
+# In _reset_internal()
+if hasattr(obj, 'orientation'):
+ scenic_orientation = obj.orientation
+ if hasattr(scenic_orientation, 'getQuaternion'):
+ quat = list(scenic_orientation.getQuaternion())
+ # ... fallback to Euler conversion
+```
+**Status**: Partially implemented but not fully tested. Not part of original PR scope.
+
+## Recommendations
+
+1. **For PR Approval**: The core request has been addressed for most properties. Position, orientation, and robot-specific properties now correctly fetch from RoboSuite.
+
+2. **Future Work**:
+ - Debug joint name resolution for free-joint objects
+ - Investigate RoboSuite's internal joint management
+ - Consider alternative velocity sources or calculation methods
+ - Complete initial orientation setting implementation
+
+3. **Known Limitations**: Document that velocity/angular velocity for manipulable objects currently requires manual calculation from position differences.
+
+## Testing
+Use `test_velocity_debug.scenic` to verify the issue and test any future fixes. The manual velocity calculation confirms objects are moving correctly in physics, just the velocity fetch is broken.
diff --git a/src/scenic/simulators/robosuite/docs/xml-file-path.md b/src/scenic/simulators/robosuite/docs/xml-file-path.md
new file mode 100644
index 000000000..f46ee5297
--- /dev/null
+++ b/src/scenic/simulators/robosuite/docs/xml-file-path.md
@@ -0,0 +1,39 @@
+Here's the documentation for the path resolution behavior:
+
+## XML Path Resolution in CustomObject
+
+### Embedded XML (string in Scenic file)
+```python
+# Base directory: Scenic file's location
+object_xml = """
+
+
+ # Relative to .scenic file
+ # Relative to .scenic file
+
+ ...
+
+"""
+obj = new CustomObject with mjcf_xml object_xml
+```
+
+### External XML (file path)
+```python
+# Base directory: XML file's location
+obj = new CustomObject with mjcf_xml "subfolder/object.xml"
+```
+
+In `subfolder/object.xml`:
+```xml
+
+
+
+
+
+ ...
+
+```
+
+**Key difference:** When loading from file, paths are relative to the XML file's directory, not the Scenic file's directory. This follows standard XML/file system conventions where relative paths are resolved from the document's location.
+
+**Recommendation:** Use embedded XML when you want paths relative to your Scenic file, or adjust paths in external XML files to be relative to their own location.
diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic
new file mode 100644
index 000000000..a60db55d2
--- /dev/null
+++ b/src/scenic/simulators/robosuite/model.scenic
@@ -0,0 +1,448 @@
+"""Scenic world model for RoboSuite robotic manipulation.
+
+The model supports various robots (Panda, UR5e, Jaco, IIWA), manipulable objects
+(primitives and custom MJCF), and tables. It provides behaviors for pick-and-place
+tasks and integrates with RoboSuite's physics simulation.
+
+Global Parameters:
+ env_config (dict): Additional RoboSuite environment configuration options.
+ Default is empty dict.
+ controller_config (dict or None): Robot controller configuration. If None,
+ uses RoboSuite's default controller for the robot type. Default is None.
+ camera_view (str or None): Camera viewpoint for rendering. Options include
+ 'frontview', 'birdview', 'agentview', 'sideview', 'robot0_robotview',
+ 'robot0_eye_in_hand'. Default is None (uses RoboSuite default).
+ render (bool): Whether to render the simulation visually. Default is True.
+ real_time (bool): Whether to run simulation in real-time. Default is True.
+ speed (float): Simulation speed multiplier when real_time is True. Default is 1.0.
+ lite_physics (bool or None): Whether to use simplified physics for faster
+ simulation. Default is None (uses RoboSuite default).
+ scenic_file_dir (Path): Directory containing the Scenic file, used for resolving
+ relative paths in MJCF XML files. Automatically set to localPath(".").
+"""
+
+
+from scenic.core.utils import repairMesh
+import trimesh
+import json
+import tempfile
+import os
+from pathlib import Path
+import numpy as np
+
+# At top of model.scenic
+try:
+ from .simulator import RobosuiteSimulator, OSCPositionAction
+except ImportError:
+ # Dummy classes for compilation without RoboSuite
+ class RobosuiteSimulator:
+ def __init__(self, **kwargs):
+ raise RuntimeError('RoboSuite is required to run simulations from this scenario')
+
+ class OSCPositionAction: pass
+
+# Global parameters
+param env_config = {}
+param controller_config = None
+param camera_view = None
+param render = True
+param real_time = True
+param speed = 1.0
+param lite_physics = None
+param scenic_file_dir = localPath(".")
+
+# Load arena configuration
+_arena_config_path = localPath("utils/arena_meshes/arena_config.json")
+with open(_arena_config_path) as f:
+ _arena_config = json.load(f)
+
+# Simulator
+simulator RobosuiteSimulator(
+ render=globalParameters.render,
+ real_time=globalParameters.real_time,
+ speed=globalParameters.speed,
+ env_config=globalParameters.env_config,
+ controller_config=globalParameters.controller_config,
+ camera_view=globalParameters.camera_view,
+ lite_physics=globalParameters.lite_physics
+)
+
+# Load robot dimensions
+_robot_dims_path = localPath("utils/robot_meshes/dimensions.txt")
+_robot_dimensions = {}
+with open(_robot_dims_path) as f:
+ for line in f:
+ if ':' in line:
+ robot, dims_str = line.strip().split(':')
+ dims = eval(dims_str.strip())
+ _robot_dimensions[robot] = dims
+
+
+def _mjcf_to_shape(mjcfXml: str, scenic_file_path: str = None) -> Shape:
+ """Convert MJCF XML to MeshShape via temp GLB file."""
+ if not mjcfXml:
+ return BoxShape()
+
+ import xml.etree.ElementTree as ET
+ import uuid
+ import shutil
+
+ base_dir = scenic_file_path if scenic_file_path else os.getcwd()
+
+ # Handle file path vs XML string
+ xml_content = mjcfXml
+ if not mjcfXml.strip().startswith('<'):
+ if not os.path.isabs(mjcfXml):
+ xml_path = os.path.join(base_dir, mjcfXml)
+ else:
+ xml_path = mjcfXml
+
+ if os.path.exists(xml_path):
+ with open(xml_path, 'r') as f:
+ xml_content = f.read()
+ base_dir = os.path.dirname(xml_path)
+ else:
+ print(f"Warning: MJCF file not found: {xml_path}")
+ return BoxShape()
+
+ # Create temp directory
+ temp_id = str(uuid.uuid4())[:8]
+ temp_dir = Path(tempfile.gettempdir()) / f"scenic_mjcf_{temp_id}"
+ temp_dir.mkdir(exist_ok=True)
+ temp_glb = temp_dir / "converted.glb"
+
+ try:
+ root = ET.fromstring(xml_content)
+ mesh_geom = root.find('.//geom[@mesh]')
+
+ if mesh_geom is not None:
+ # Handle mesh file
+ mesh_name = mesh_geom.get('mesh')
+ mesh_elem = root.find(f'.//asset/mesh[@name="{mesh_name}"]')
+ mesh_file = mesh_elem.get('file')
+ scale = np.array([float(s) for s in mesh_elem.get('scale', '1 1 1').split()])
+
+ if not os.path.isabs(mesh_file):
+ mesh_path = os.path.join(base_dir, mesh_file)
+ else:
+ mesh_path = mesh_file
+
+ mesh = trimesh.load(mesh_path)
+ if isinstance(mesh, trimesh.Scene):
+ mesh = trimesh.util.concatenate(list(mesh.geometry.values()))
+
+ mesh.apply_scale(scale)
+
+ else:
+ # Handle primitives
+ geoms = root.findall('.//geom[@type]')
+ if not geoms:
+ print("Warning: No mesh or primitive geom found")
+ return BoxShape()
+
+ meshes = []
+ for geom in geoms:
+ geom_type = geom.get('type')
+ size = np.array([float(s) for s in geom.get('size', '0.1 0.1 0.1').split()])
+
+ if geom_type == 'box':
+ geom_mesh = trimesh.creation.box(extents=size * 2)
+ elif geom_type == 'sphere':
+ geom_mesh = trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ elif geom_type == 'cylinder':
+ geom_mesh = trimesh.creation.cylinder(radius=size[0], height=size[1] * 2)
+ elif geom_type == 'capsule':
+ geom_mesh = trimesh.creation.capsule(radius=size[0], height=size[1] * 2)
+ else:
+ continue
+
+ pos = np.array([float(p) for p in geom.get('pos', '0 0 0').split()])
+ if np.any(pos != 0):
+ geom_mesh.apply_translation(pos)
+
+ meshes.append(geom_mesh)
+
+ if meshes:
+ mesh = trimesh.util.concatenate(meshes)
+ else:
+ print("Warning: No valid primitives found")
+ return BoxShape()
+
+ # Export, load, repair sequence
+ mesh.export(str(temp_glb))
+
+ loaded_mesh = trimesh.load(str(temp_glb))
+ if isinstance(loaded_mesh, trimesh.Scene):
+ loaded_mesh = trimesh.util.concatenate(list(loaded_mesh.geometry.values()))
+
+ repaired_mesh = repairMesh(loaded_mesh, pitch=0.02, verbose=False)
+
+ repaired_glb = temp_dir / "repaired.glb"
+ repaired_mesh.export(str(repaired_glb))
+
+ shape = MeshShape.fromFile(str(repaired_glb))
+
+ # Schedule cleanup
+ def cleanup():
+ if temp_dir.exists():
+ shutil.rmtree(temp_dir, ignore_errors=True)
+
+ import atexit
+ atexit.register(cleanup)
+
+ return shape
+
+ except Exception as e:
+ print(f"Error in _mjcf_to_shape: {e}")
+ if temp_dir.exists():
+ shutil.rmtree(temp_dir, ignore_errors=True)
+ return BoxShape()
+
+
+def _load_robot_mesh_shape(path):
+ """Load mesh and create MeshShape with repair."""
+ mesh = trimesh.load(path, force='mesh')
+ if isinstance(mesh, trimesh.Scene):
+ mesh = mesh.dump(concatenate=True)
+ repaired_mesh = repairMesh(mesh, pitch=0.02, verbose=False)
+ return MeshShape(repaired_mesh)
+
+# Base classes
+class RoboSuiteObject(Object):
+ """Base class for all RoboSuite objects."""
+ density: 1000
+ friction: (1.0, 0.005, 0.0001)
+ solref: (0.02, 1.0)
+ solimp: (0.9, 0.95, 0.001, 0.5, 2.0)
+ allowCollisions: False
+ requireVisible: False
+ regionContainedIn: None
+
+class ArenaFloor(RoboSuiteObject):
+ """Floor component for Scenic's collision system."""
+ shape: MeshShape.fromFile(localPath("utils/arena_meshes/floor.glb"))
+ position: (0.0, 0.0, 0.0)
+ width: _arena_config['floor']['dimensions'][0]
+ length: _arena_config['floor']['dimensions'][1]
+ height: _arena_config['floor']['dimensions'][2]
+ allowCollisions: True
+ requireVisible: False
+ _isArenaComponent: True
+
+class ArenaWalls(RoboSuiteObject):
+ """Walls component for Scenic's collision system."""
+ shape: MeshShape.fromFile(localPath("utils/arena_meshes/walls.glb"))
+ width: _arena_config['walls']['dimensions'][0]
+ length: _arena_config['walls']['dimensions'][1]
+ height: _arena_config['walls']['dimensions'][2]
+ allowCollisions: False
+ requireVisible: False
+ _isArenaComponent: True
+
+# Create default arena components
+arena_floor = new ArenaFloor
+arena_walls = new ArenaWalls on (0.5, 0.0, 0.0)
+
+class CustomArena(RoboSuiteObject):
+ """Custom arena defined by complete MJCF XML."""
+ isCustomArena: True
+ arenaXml: ""
+
+class Table(RoboSuiteObject):
+ """Table in environment - creates MultiTableArena.
+
+ Note: Table dimensions have limitations due to RoboSuite's MultiTableArena:
+ - Height is fixed at 0.85m (full table) for Scenic collision detection
+ - RoboSuite only uses tabletop thickness (0.05m) internally
+ - Changing dimensions may cause visual inconsistencies between simulators
+ See table-rendering-issue.md for technical details.
+ """
+ isTable: True
+ shape: MeshShape.fromFile(localPath("utils/table_meshes/standard_table.glb"))
+ width: 0.8
+ length: 0.8
+ height: 0.85 # Full table height for Scenic collision - DO NOT MODIFY
+ position: (0, 0, 0.425) # Center of table volume
+
+class ManipulationObject(RoboSuiteObject):
+ """Base class for manipulable objects."""
+ color: (0.5, 0.5, 0.5, 1.0)
+
+class CustomObject(ManipulationObject):
+ """Custom object with automatic dimension extraction."""
+ objectType: "MJCF"
+ mjcfXml: ""
+ shape: _mjcf_to_shape(self.mjcfXml, globalParameters.scenic_file_dir) if self.mjcfXml else BoxShape()
+
+# Primitive objects with makeRobosuiteObject methods
+class Box(ManipulationObject):
+ """Box object."""
+ width: 0.03
+ length: 0.03
+ height: 0.03
+
+ def makeRobosuiteObject(self, name):
+ from robosuite.models.objects import BoxObject
+ return BoxObject(
+ name=name,
+ size=[self.width, self.length, self.height],
+ rgba=self.color
+ )
+
+class Ball(ManipulationObject):
+ """Ball/sphere object."""
+ radius: 0.03
+ width: 0.06
+ length: 0.06
+ height: 0.06
+
+ def makeRobosuiteObject(self, name):
+ from robosuite.models.objects import BallObject
+ return BallObject(
+ name=name,
+ size=[self.radius],
+ rgba=self.color
+ )
+
+class Cylinder(ManipulationObject):
+ """Cylinder object."""
+ width: 0.06
+ length: 0.06
+ height: 0.12
+
+ def makeRobosuiteObject(self, name):
+ from robosuite.models.objects import CylinderObject
+ radius = (self.width + self.length) / 4
+ return CylinderObject(
+ name=name,
+ size=[radius, self.height],
+ rgba=self.color
+ )
+
+class Capsule(ManipulationObject):
+ """Capsule object."""
+ width: 0.045
+ length: 0.045
+ height: 0.09
+
+ def makeRobosuiteObject(self, name):
+ from robosuite.models.objects import CapsuleObject
+ radius = (self.width + self.length) / 4
+ return CapsuleObject(
+ name=name,
+ size=[radius, self.height],
+ rgba=self.color
+ )
+
+# Robots
+class Robot(RoboSuiteObject):
+ """Base robot class."""
+ robotType: "Panda"
+ gripperType: "default"
+ controllerConfig: None
+ initialQpos: None
+ baseType: "default"
+ width: 0.4
+ length: 0.4
+ height: 1.0
+ position: (0, 0, 0)
+
+ jointPositions[dynamic]: []
+ eefPos[dynamic]: [0, 0, 0]
+ gripperState[dynamic]: [0, 0]
+
+class Panda(Robot):
+ robotType: "Panda"
+ shape: _load_robot_mesh_shape(localPath("utils/robot_meshes/Panda.glb"))
+ width: _robot_dimensions.get("Panda", [0.4, 0.4, 1.0])[0]
+ length: _robot_dimensions.get("Panda", [0.4, 0.4, 1.0])[1]
+ height: _robot_dimensions.get("Panda", [0.4, 0.4, 1.0])[2]
+ gripperType: "PandaGripper"
+
+class UR5e(Robot):
+ robotType: "UR5e"
+ shape: _load_robot_mesh_shape(localPath("utils/robot_meshes/UR5e.glb"))
+ width: _robot_dimensions.get("UR5e", [0.4, 0.4, 1.0])[0]
+ length: _robot_dimensions.get("UR5e", [0.4, 0.4, 1.0])[1]
+ height: _robot_dimensions.get("UR5e", [0.4, 0.4, 1.0])[2]
+ gripperType: "Robotiq85Gripper"
+
+class Jaco(Robot):
+ robotType: "Jaco"
+ shape: _load_robot_mesh_shape(localPath("utils/robot_meshes/Jaco.glb"))
+ width: _robot_dimensions.get("Jaco", [0.4, 0.4, 1.0])[0]
+ length: _robot_dimensions.get("Jaco", [0.4, 0.4, 1.0])[1]
+ height: _robot_dimensions.get("Jaco", [0.4, 0.4, 1.0])[2]
+ gripperType: "JacoThreeFingerGripper"
+
+class IIWA(Robot):
+ robotType: "IIWA"
+ shape: _load_robot_mesh_shape(localPath("utils/robot_meshes/IIWA.glb"))
+ width: _robot_dimensions.get("IIWA", [0.4, 0.4, 1.0])[0]
+ length: _robot_dimensions.get("IIWA", [0.4, 0.4, 1.0])[1]
+ height: _robot_dimensions.get("IIWA", [0.4, 0.4, 1.0])[2]
+ gripperType: "Robotiq140Gripper"
+
+# Behaviors
+behavior OpenGripper(steps=20):
+ """Open gripper over multiple steps."""
+ for _ in range(steps):
+ take OSCPositionAction(gripper=-1)
+
+behavior CloseGripper(steps=30):
+ """Close gripper over multiple steps."""
+ for _ in range(steps):
+ take OSCPositionAction(gripper=1)
+
+behavior MoveToPosition(target_pos, tolerance=0.02, max_steps=100, gain=3.0):
+ """Move end-effector to target position."""
+ for _ in range(max_steps):
+ eef_pos = self.eefPos
+ error = [target_pos[i] - eef_pos[i] for i in range(3)]
+
+ if sum(e**2 for e in error)**0.5 < tolerance:
+ return
+
+ limit = 0.3
+ delta = [max(-limit, min(limit, e * gain)) for e in error]
+ take OSCPositionAction(position_delta=delta, gripper=-1)
+
+behavior MoveAboveObject(target_object, height_offset=0.1):
+ """Move above a tracked object."""
+ target_pos = [target_object.position.x,
+ target_object.position.y,
+ target_object.position.z + height_offset]
+ do MoveToPosition(target_pos)
+
+behavior MoveToGrasp(target_object, grasp_offset=0.02):
+ """Move to grasp position for object."""
+ target_pos = [target_object.position.x,
+ target_object.position.y,
+ target_object.position.z - grasp_offset]
+ do MoveToPosition(target_pos, tolerance=0.01)
+
+behavior LiftToHeight(target_height=1.0, max_steps=200):
+ """Lift to absolute height."""
+ for _ in range(max_steps):
+ eef_pos = self.eefPos
+ error = [0, 0, target_height - eef_pos[2]]
+
+ if abs(error[2]) < 0.02:
+ return
+
+ limit = 0.3
+ gain = 3.0
+ delta = [max(-limit, min(limit, e * gain)) for e in error]
+ take OSCPositionAction(position_delta=delta, gripper=1)
+
+behavior PickObject(target_object):
+ """Pick up a specific object."""
+ do OpenGripper()
+ do MoveAboveObject(target_object)
+ do MoveToGrasp(target_object)
+ do CloseGripper()
+
+behavior PickAndLift(target_object, height=1.05):
+ """Complete pick and lift for specific object."""
+ do PickObject(target_object)
+ do LiftToHeight(height)
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py
new file mode 100644
index 000000000..d5d55af68
--- /dev/null
+++ b/src/scenic/simulators/robosuite/simulator.py
@@ -0,0 +1,1105 @@
+"""RoboSuite Simulator interface for Scenic."""
+
+from typing import Dict, List, Any, Optional, Union
+import numpy as np
+import os
+import tempfile
+import shutil
+import xml.etree.ElementTree as ET
+
+from scenic.core.simulators import Simulation, Simulator
+from scenic.core.vectors import Vector
+from scenic.core.dynamics import Action
+from scenic.core.object_types import Orientation
+
+# Constants
+DEFAULT_PHYSICS_TIMESTEP = 0.002
+DEFAULT_TIMESTEP = 0.01
+DEFAULT_ACTION_DIM = 7
+PHYSICS_SETTLE_STEPS = 10
+
+# Camera view mapping
+CAMERA_VIEWS = {
+ "frontview": 0,
+ "birdview": 1,
+ "agentview": 2,
+ "sideview": 3,
+ "robot0_robotview": 4,
+ "robot0_eye_in_hand": 5
+}
+
+
+# Deferred class definitions to avoid importing RoboSuite at module load
+def _get_arena_class():
+ """Get CustomXMLArena class (requires RoboSuite imports)."""
+ from robosuite.models.arenas import Arena
+ from robosuite.utils.mjcf_utils import xml_path_completion
+
+ class CustomXMLArena(Arena):
+ """Arena created from custom XML string or file."""
+
+ def __init__(self, xml_string, scenic_file_path=None):
+ super().__init__(xml_path_completion("arenas/empty_arena.xml"))
+
+ # Handle file path vs XML string
+ if not xml_string.strip().startswith('<'):
+ base_dir = os.path.dirname(scenic_file_path) if scenic_file_path else os.getcwd()
+ if not os.path.isabs(xml_string):
+ xml_path = os.path.join(base_dir, xml_string)
+ else:
+ xml_path = xml_string
+
+ if os.path.exists(xml_path):
+ with open(xml_path, 'r') as f:
+ xml_string = f.read()
+ else:
+ raise FileNotFoundError(f"Arena XML file not found: {xml_path}")
+
+ # Parse custom XML
+ custom_tree = ET.fromstring(xml_string)
+
+ custom_worldbody = custom_tree.find(".//worldbody")
+ if custom_worldbody is None:
+ raise ValueError("Custom XML must have a worldbody element")
+
+ # Replace worldbody content
+ for child in list(self.worldbody):
+ self.worldbody.remove(child)
+
+ for element in custom_worldbody:
+ self.worldbody.append(element)
+
+ # Merge assets
+ custom_asset = custom_tree.find(".//asset")
+ if custom_asset is not None and self.asset is not None:
+ for child in custom_asset:
+ self.asset.append(child)
+
+ # Set floor reference
+ self.floor = self.worldbody.find(".//geom[@name='floor']")
+ if self.floor is None:
+ for geom in self.worldbody.findall(".//geom"):
+ if geom.get("type") == "plane":
+ self.floor = geom
+ break
+
+ # Set table references if they exist
+ self.table_body = self.worldbody.find(".//body[@name='custom_table']")
+ self.table_top = self.worldbody.find(".//geom[@name='table_top']")
+
+ return CustomXMLArena
+
+
+def _get_object_class():
+ """Get CustomMeshObject class (requires RoboSuite imports)."""
+ from robosuite.models.objects import MujocoXMLObject
+
+ class CustomMeshObject(MujocoXMLObject):
+ """Custom manipulable object from XML string with automatic joint and collision handling."""
+
+ _existing_joint_names = set()
+
+ def __init__(self, name, xml_string, scenic_file_path=None):
+ self.object_name = name
+ self.temp_dir = tempfile.mkdtemp()
+
+ base_dir = scenic_file_path if scenic_file_path else os.getcwd()
+
+ # Handle file path vs XML string
+ if not xml_string.strip().startswith('<'):
+ if not os.path.isabs(xml_string):
+ xml_path = os.path.join(base_dir, xml_string)
+ else:
+ xml_path = xml_string
+
+ if os.path.exists(xml_path):
+ with open(xml_path, 'r') as f:
+ xml_string = f.read()
+ base_dir = os.path.dirname(xml_path)
+ else:
+ raise FileNotFoundError(f"Object XML file not found: {xml_path}")
+
+ tree = ET.fromstring(xml_string)
+
+ self._process_asset_files(tree, base_dir)
+ tree = self._process_xml_tree(tree, name)
+
+ fixed_xml = ET.tostring(tree, encoding='unicode')
+
+ temp_xml_path = os.path.join(self.temp_dir, f"{name}.xml")
+ with open(temp_xml_path, 'w') as f:
+ f.write(fixed_xml)
+
+ try:
+ super().__init__(
+ temp_xml_path,
+ name=name,
+ joints=None,
+ obj_type="all",
+ duplicate_collision_geoms=False
+ )
+ finally:
+ pass # Keep temp_dir for MuJoCo to access files
+
+ def _process_asset_files(self, tree, base_dir):
+ """Copy mesh and texture files to temp directory."""
+ # Process mesh files
+ for mesh in tree.findall(".//mesh"):
+ file_attr = mesh.get("file")
+ if file_attr:
+ if not os.path.isabs(file_attr):
+ full_path = os.path.join(base_dir, file_attr)
+ else:
+ full_path = file_attr
+
+ if os.path.exists(full_path):
+ filename = os.path.basename(full_path)
+ temp_path = os.path.join(self.temp_dir, filename)
+ shutil.copy2(full_path, temp_path)
+ mesh.set("file", filename)
+
+ # Auto-copy MTL file for OBJ meshes
+ if full_path.endswith('.obj'):
+ mtl_path = full_path.replace('.obj', '.mtl')
+ if os.path.exists(mtl_path):
+ mtl_filename = os.path.basename(mtl_path)
+ temp_mtl_path = os.path.join(self.temp_dir, mtl_filename)
+ shutil.copy2(mtl_path, temp_mtl_path)
+
+ # Process texture files
+ for texture in tree.findall(".//texture"):
+ file_attr = texture.get("file")
+ if file_attr and not texture.get("builtin"):
+ if not os.path.isabs(file_attr):
+ full_path = os.path.join(base_dir, file_attr)
+ else:
+ full_path = file_attr
+
+ if os.path.exists(full_path):
+ filename = os.path.basename(full_path)
+
+ # Convert JPG to PNG if needed
+ if filename.lower().endswith(('.jpg', '.jpeg')):
+ try:
+ from PIL import Image
+ img = Image.open(full_path)
+ filename = filename.rsplit('.', 1)[0] + '.png'
+ temp_path = os.path.join(self.temp_dir, filename)
+ img.save(temp_path)
+ except ImportError:
+ temp_path = os.path.join(self.temp_dir, filename)
+ shutil.copy2(full_path, temp_path)
+ else:
+ temp_path = os.path.join(self.temp_dir, filename)
+ shutil.copy2(full_path, temp_path)
+
+ texture.set("file", filename)
+
+ def _process_xml_tree(self, tree, name):
+ """Ensure joint and collision geometry."""
+ # Find object body
+ object_body = tree.find(".//body[@name='object']")
+ if object_body is None:
+ worldbody = tree.find(".//worldbody")
+ if worldbody is not None:
+ for body in worldbody.findall("body"):
+ inner_body = body.find("body[@name='object']")
+ if inner_body is not None:
+ object_body = inner_body
+ break
+
+ if object_body is None:
+ return tree
+
+ self._ensure_free_joint(object_body, name)
+ self._ensure_collision_geometry(object_body, name)
+
+ return tree
+
+ def _ensure_free_joint(self, object_body, name):
+ """Ensure object has a free joint for physics."""
+ has_free_joint = False
+ existing_joint_name = None
+
+ for joint in object_body.findall("joint"):
+ if joint.get("type") == "free":
+ has_free_joint = True
+ existing_joint_name = joint.get("name")
+ if existing_joint_name:
+ self._existing_joint_names.add(existing_joint_name)
+ self.joint_name = existing_joint_name
+ break
+
+ if not has_free_joint:
+ # Generate unique joint name
+ joint_counter = 0
+ while True:
+ joint_name = f"{name}_joint_{joint_counter}"
+ if joint_name not in self._existing_joint_names:
+ break
+ joint_counter += 1
+
+ joint = ET.Element("joint")
+ joint.set("name", joint_name)
+ joint.set("type", "free")
+ joint.set("damping", "0.0005")
+
+ object_body.insert(0, joint)
+ self._existing_joint_names.add(joint_name)
+ self.joint_name = joint_name
+
+ def _ensure_collision_geometry(self, object_body, name):
+ """Ensure proper collision geometry exists."""
+ has_proper_collision = False
+ mesh_collision_geom = None
+ visual_geom = None
+
+ for geom in object_body.findall("geom"):
+ geom_group = geom.get("group", "0")
+ geom_type = geom.get("type", "box")
+
+ if geom_group == "0":
+ if geom_type == "mesh":
+ mesh_collision_geom = geom
+ else:
+ has_proper_collision = True
+ break
+ elif geom_group == "1":
+ visual_geom = geom
+
+ # Convert mesh collision to visual + box collision
+ if mesh_collision_geom is not None and not has_proper_collision:
+ mesh_collision_geom.set("group", "1")
+ mesh_collision_geom.set("contype", "0")
+ mesh_collision_geom.set("conaffinity", "0")
+
+ collision_geom = ET.Element("geom")
+ collision_geom.set("name", "collision_auto")
+ collision_geom.set("type", "box")
+ collision_geom.set("size", "0.04 0.04 0.04")
+
+ pos = mesh_collision_geom.get("pos", "0 0 0")
+ collision_geom.set("pos", pos)
+
+ collision_geom.set("group", "0")
+ collision_geom.set("rgba", "0 0 0 0")
+
+ for prop in ["solimp", "solref", "density", "friction"]:
+ value = mesh_collision_geom.get(prop)
+ if value:
+ collision_geom.set(prop, value)
+
+ if not collision_geom.get("solimp"):
+ collision_geom.set("solimp", "0.998 0.998 0.001")
+ if not collision_geom.get("solref"):
+ collision_geom.set("solref", "0.001 1")
+ if not collision_geom.get("density"):
+ collision_geom.set("density", "100")
+ if not collision_geom.get("friction"):
+ collision_geom.set("friction", "0.95 0.3 0.1")
+
+ index = list(object_body).index(mesh_collision_geom) + 1
+ object_body.insert(index, collision_geom)
+ return
+
+ # Create collision if none exists
+ if not has_proper_collision:
+ if visual_geom is not None:
+ collision_geom = ET.Element("geom")
+ collision_geom.set("name", "collision_auto")
+
+ geom_type = visual_geom.get("type", "box")
+ pos = visual_geom.get("pos", "0 0 0")
+
+ if geom_type == "mesh":
+ collision_geom.set("type", "box")
+ collision_geom.set("size", "0.04 0.04 0.04")
+ else:
+ collision_geom.set("type", geom_type)
+ size = visual_geom.get("size")
+ if size:
+ collision_geom.set("size", size)
+
+ collision_geom.set("pos", pos)
+
+ else:
+ collision_geom = ET.Element("geom")
+ collision_geom.set("name", "collision_default")
+ collision_geom.set("type", "box")
+ collision_geom.set("size", "0.04 0.04 0.04")
+ collision_geom.set("pos", "0 0 0")
+
+ collision_geom.set("group", "0")
+ collision_geom.set("rgba", "0 0 0 0")
+ collision_geom.set("solimp", "0.998 0.998 0.001")
+ collision_geom.set("solref", "0.001 1")
+ collision_geom.set("density", "100")
+ collision_geom.set("friction", "0.95 0.3 0.1")
+
+ if visual_geom is not None:
+ index = list(object_body).index(visual_geom) + 1
+ object_body.insert(index, collision_geom)
+ else:
+ object_body.append(collision_geom)
+
+ def __del__(self):
+ """Clean up temp directory."""
+ if hasattr(self, 'temp_dir') and os.path.exists(self.temp_dir):
+ shutil.rmtree(self.temp_dir)
+
+ return CustomMeshObject
+
+
+def _get_env_class():
+ """Get ScenicManipulationEnv class (requires RoboSuite imports)."""
+ from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+ from robosuite.models.arenas import EmptyArena, MultiTableArena
+ from robosuite.models.tasks import ManipulationTask
+ from robosuite.utils.observables import Observable, sensor
+
+ class ScenicManipulationEnv(ManipulationEnv):
+ """Scenic-driven manipulation environment."""
+
+ def __init__(self, scenic_sim, **kwargs):
+ self.scenic_sim = scenic_sim
+ super().__init__(**kwargs)
+
+ def _load_model(self):
+ """Load models and create arena."""
+ super()._load_model()
+
+ # Position robots directly from scenic_sim
+ for i, robot in enumerate(self.scenic_sim.robots):
+ if i < len(self.robots):
+ pos = [robot.position.x, robot.position.y, 0]
+ self.robots[i].robot_model.set_base_xpos(pos)
+
+ mujoco_arena = self._create_arena()
+ mujoco_arena.set_origin([0, 0, 0])
+
+ self.mujoco_objects = self._create_objects()
+
+ self.model = ManipulationTask(
+ mujoco_arena=mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=self.mujoco_objects
+ )
+
+ def _create_arena(self):
+ """Create arena based on scenic objects."""
+ # Check for custom arena
+ for obj in self.scenic_sim.objects:
+ if hasattr(obj, 'isCustomArena') and obj.isCustomArena:
+ xml_string = getattr(obj, 'arenaXml', '')
+ scenic_file_path = str(self.scenic_sim.scene.params.get('scenic_file_dir', ''))
+ return CustomXMLArena(xml_string, scenic_file_path)
+
+ # Check for tables
+ tables = [obj for obj in self.scenic_sim.objects
+ if hasattr(obj, 'isTable') and obj.isTable]
+
+ if tables:
+ table_offsets = []
+ for t in tables:
+ table_offsets.append([t.position.x, t.position.y, 0.8])
+
+ return MultiTableArena(
+ table_offsets=table_offsets,
+ table_full_sizes=[(t.width, t.length, 0.05) for t in tables],
+ has_legs=[True] * len(tables)
+ )
+
+ return EmptyArena()
+
+ def _create_objects(self) -> List:
+ """Create Robosuite objects from Scenic objects."""
+ mujoco_objects = []
+ scenic_file_path = str(self.scenic_sim.scene.params.get('scenic_file_dir', ''))
+
+ for obj in self.scenic_sim.objects:
+ # Skip non-manipulable objects
+ if (hasattr(obj, '_isArenaComponent') and obj._isArenaComponent) or \
+ hasattr(obj, 'robotType') or \
+ hasattr(obj, 'isCustomArena') or \
+ hasattr(obj, 'isTable'):
+ continue
+
+ # Generate unique name
+ scenic_name = getattr(obj, 'name', None)
+ if hasattr(obj, 'objectType') and obj.objectType == 'MJCF':
+ obj_name = f"{scenic_name}_{len(mujoco_objects)}" if scenic_name else f"mjcf_object_{len(mujoco_objects)}"
+ mj_obj = CustomMeshObject(
+ name=obj_name,
+ xml_string=getattr(obj, 'mjcfXml', ''),
+ scenic_file_path=scenic_file_path
+ )
+ elif hasattr(obj, 'makeRobosuiteObject'):
+ obj_name = f"{scenic_name}_{len(mujoco_objects)}" if scenic_name else f"{obj.__class__.__name__}_{len(mujoco_objects)}"
+ mj_obj = obj.makeRobosuiteObject(obj_name)
+ else:
+ continue
+
+ # Store mapping
+ self.scenic_sim.object_name_map[obj] = obj_name
+ mujoco_objects.append(mj_obj)
+
+ return mujoco_objects
+
+ def _reset_internal(self):
+ """Reset environment internals."""
+ super()._reset_internal()
+
+ # Set initial positions and orientations
+ for obj in self.scenic_sim.objects:
+ obj_name = self.scenic_sim.object_name_map.get(obj)
+ if not obj_name:
+ continue
+
+ # Find corresponding mujoco object
+ mj_obj = None
+ for mobj in self.mujoco_objects:
+ if mobj.name == obj_name:
+ mj_obj = mobj
+ break
+
+ if mj_obj:
+ pos = [obj.position.x, obj.position.y, obj.position.z]
+
+ # Get orientation from Scenic object
+ if hasattr(obj, 'orientation'):
+ scenic_orientation = obj.orientation
+ if scenic_orientation is not None:
+ # Get quaternion from Orientation object
+ # Try different methods to access quaternion
+ if hasattr(scenic_orientation, 'getQuaternion'):
+ quat = list(scenic_orientation.getQuaternion())
+ elif hasattr(scenic_orientation, 'toQuaternion'):
+ quat = list(scenic_orientation.toQuaternion())
+ elif hasattr(scenic_orientation, '_quaternion'):
+ quat = list(scenic_orientation._quaternion)
+ else:
+ # Fallback: compute from Euler angles
+ import math
+ yaw = obj.yaw if hasattr(obj, 'yaw') else 0
+ pitch = obj.pitch if hasattr(obj, 'pitch') else 0
+ roll = obj.roll if hasattr(obj, 'roll') else 0
+
+ # Convert Euler to quaternion (ZXY order)
+ cy = math.cos(yaw * 0.5)
+ sy = math.sin(yaw * 0.5)
+ cp = math.cos(pitch * 0.5)
+ sp = math.sin(pitch * 0.5)
+ cr = math.cos(roll * 0.5)
+ sr = math.sin(roll * 0.5)
+
+ w = cr * cp * cy + sr * sp * sy
+ x = sr * cp * cy - cr * sp * sy
+ y = cr * sp * cy + sr * cp * sy
+ z = cr * cp * sy - sr * sp * cy
+
+ quat = [w, x, y, z]
+ else:
+ quat = [1, 0, 0, 0] # Default quaternion
+ else:
+ quat = [1, 0, 0, 0] # Default quaternion
+
+ # Find joint name
+ joint_name = None
+ possible_names = []
+
+ if hasattr(mj_obj, 'joint_name'):
+ base_joint = mj_obj.joint_name
+ possible_names.append(f"{mj_obj.name}_{base_joint}")
+ possible_names.append(base_joint)
+
+ if hasattr(mj_obj, 'joints') and mj_obj.joints:
+ possible_names.extend(mj_obj.joints)
+
+ possible_names.append(f"{mj_obj.name}_joint0")
+ possible_names.append(f"{mj_obj.name}_object_joint")
+
+ for name in possible_names:
+ if name in self.sim.model.joint_names:
+ joint_name = name
+ break
+
+ if joint_name:
+ self.sim.data.set_joint_qpos(
+ joint_name,
+ np.concatenate([np.array(pos), np.array(quat)])
+ )
+
+ self.sim.forward()
+ self.sim.step()
+
+ def _setup_references(self):
+ """Setup references to simulation objects."""
+ super()._setup_references()
+ # Let scenic_sim handle body mapping
+ for obj in self.mujoco_objects:
+ try:
+ if hasattr(obj, 'root_body'):
+ body_name = obj.root_body
+ else:
+ body_name = obj.name
+
+ body_id = self.sim.model.body_name2id(body_name)
+ self.scenic_sim.body_id_map[obj.name] = body_id
+ except:
+ for suffix in ['_main', '_body', '']:
+ try:
+ body_name = f"{obj.name}{suffix}"
+ body_id = self.sim.model.body_name2id(body_name)
+ self.scenic_sim.body_id_map[obj.name] = body_id
+ break
+ except:
+ continue
+
+ def reward(self, action=None) -> float:
+ """Compute reward."""
+ return 0.0
+
+ def _check_success(self) -> bool:
+ """Check task success."""
+ return False
+
+ return ScenicManipulationEnv
+
+
+class RobosuiteSimulator(Simulator):
+ """Simulator for RoboSuite robotic manipulation environments.
+
+ Args:
+ render: Whether to render the simulation visually. If True, opens a
+ viewer window showing the simulation. Default True.
+ real_time: Whether to run the simulation in real-time. If False, runs
+ as fast as possible. Default True.
+ speed: Speed multiplier for real-time simulation. Values > 1.0 speed up
+ the simulation, < 1.0 slow it down. Only used when real_time=True.
+ Default 1.0.
+ env_config: Additional configuration passed to RoboSuite environment.
+ Can include settings like 'control_freq', 'horizon', etc. Default
+ empty dict.
+ controller_config: Robot controller configuration. If None, uses
+ RoboSuite's default controller for the robot type. Can specify
+ controller type and parameters. Default None.
+ camera_view: Name of camera to use for rendering. Options include
+ 'frontview', 'birdview', 'agentview', 'sideview', 'robot0_robotview',
+ 'robot0_eye_in_hand'. Default None (uses RoboSuite default).
+ lite_physics: Whether to use simplified physics for faster simulation.
+ Reduces physics accuracy but improves performance. Default None
+ (uses RoboSuite default).
+ """
+
+ def __init__(self, render: bool = True, real_time: bool = True, speed: float = 1.0,
+ env_config: Optional[Dict] = None, controller_config: Optional[Dict] = None,
+ camera_view: Optional[str] = None, lite_physics: Optional[bool] = None):
+ super().__init__()
+
+ # Import RoboSuite only when creating simulator
+ try:
+ global mujoco, suite
+ import mujoco
+ import robosuite as suite
+ except ImportError as e:
+ raise RuntimeError(f"Unable to import RoboSuite: {e}")
+
+ self.render = render
+ self.real_time = real_time
+ self.speed = speed
+ self.env_config = env_config or {}
+ self.controller_config = controller_config
+ self.camera_view = camera_view
+ self.lite_physics = lite_physics
+
+ def createSimulation(self, scene, **kwargs):
+ """Create a simulation instance."""
+ return RobosuiteSimulation(
+ scene, self.render, self.real_time, self.speed,
+ self.env_config, self.controller_config,
+ self.camera_view, self.lite_physics, **kwargs
+ )
+
+
+class RobosuiteSimulation(Simulation):
+ """Simulation for RoboSuite custom environments."""
+
+ def __init__(self, scene, render: bool, real_time: bool, speed: float,
+ env_config: Optional[Dict], controller_config: Optional[Dict],
+ camera_view: Optional[str], lite_physics: Optional[bool], **kwargs):
+
+ # Load deferred classes
+ global CustomXMLArena, CustomMeshObject, ScenicManipulationEnv
+ CustomXMLArena = _get_arena_class()
+ CustomMeshObject = _get_object_class()
+ ScenicManipulationEnv = _get_env_class()
+
+ self.render = render
+ self.real_time = real_time
+ self.speed = speed
+ self.env_config = env_config or {}
+ self.controller_config = controller_config
+ self.camera_view = camera_view
+ self.lite_physics = lite_physics
+
+ self.robosuite_env = None
+ self.model = None
+ self.data = None
+
+ self.body_id_map = {}
+ self.object_name_map = {}
+ self.robots = []
+ self.prev_positions = {}
+ self._current_obs = None
+
+ self.pending_robot_action = None
+ self.action_dim = DEFAULT_ACTION_DIM
+ self.controller_type = None
+
+ self.timestep = kwargs.get('timestep') or DEFAULT_TIMESTEP
+ self.physics_timestep = kwargs.get('physics_timestep') or DEFAULT_PHYSICS_TIMESTEP
+ self.physics_steps = int(self.timestep / self.physics_timestep)
+ self.agents = []
+
+ super().__init__(scene, **kwargs)
+
+ def setup(self):
+ """Initialize the RoboSuite environment."""
+ super().setup()
+
+ # Extract robots for environment
+ robot_types = []
+ for obj in self.objects:
+ if hasattr(obj, 'robotType'):
+ robot_types.append(obj.robotType)
+ self.robots.append(obj)
+
+ if not robot_types:
+ raise ValueError("At least one robot is required in the scene")
+
+ robot_arg = robot_types[0] if len(robot_types) == 1 else robot_types
+
+ env_kwargs = {
+ 'scenic_sim': self, # Pass self directly
+ 'robots': robot_arg,
+ 'has_renderer': self.render,
+ 'render_camera': self.camera_view,
+ 'camera_names': ["frontview", "robot0_eye_in_hand"],
+ 'controller_configs': self.controller_config,
+ **self.env_config
+ }
+
+ if self.lite_physics is not None:
+ env_kwargs['lite_physics'] = self.lite_physics
+
+ self.robosuite_env = ScenicManipulationEnv(**env_kwargs)
+ self._current_obs = self.robosuite_env.reset()
+ self._detect_controller_type()
+
+ self._finalize_setup()
+
+ def _finalize_setup(self):
+ """Common setup after environment creation."""
+ self.model = self.robosuite_env.sim.model._model
+ self.data = self.robosuite_env.sim.data._data
+
+ if self.render and self.camera_view is not None:
+ self._set_camera_view()
+
+ self._setup_body_mapping()
+
+ self.agents = [obj for obj in self.objects
+ if hasattr(obj, 'behavior') and obj.behavior]
+
+ def _set_camera_view(self):
+ """Set the camera view if specified."""
+ camera_id = (CAMERA_VIEWS.get(self.camera_view.lower(), 0)
+ if isinstance(self.camera_view, str)
+ else self.camera_view)
+ if self.robosuite_env.viewer:
+ self.robosuite_env.viewer.set_camera(camera_id=camera_id)
+
+ def _extract_scenic_config(self) -> Dict:
+ """Extract configuration from Scenic scene."""
+ config = {'robots': [], 'tables': [], 'objects': [], 'custom_arena': None}
+
+ scenic_file_path = None
+
+ if 'scenic_file_dir' in self.scene.params:
+ path_obj = self.scene.params.get('scenic_file_dir')
+ if path_obj:
+ scenic_file_path = str(path_obj)
+
+ if not scenic_file_path:
+ import sys
+ for arg in sys.argv:
+ if arg.endswith('.scenic'):
+ scenic_file_abspath = os.path.abspath(arg)
+ if os.path.exists(scenic_file_abspath):
+ scenic_file_path = os.path.dirname(scenic_file_abspath)
+ break
+
+ if not scenic_file_path:
+ scenic_file_path = os.getcwd()
+
+ for obj in self.objects:
+ if hasattr(obj, '_isArenaComponent') and obj._isArenaComponent:
+ continue
+
+ if hasattr(obj, 'robotType'):
+ self._add_robot_config(config['robots'], obj)
+ elif hasattr(obj, 'isCustomArena') and obj.isCustomArena:
+ config['custom_arena'] = {
+ 'xml': getattr(obj, 'arenaXml', ''),
+ 'scenic_file_path': scenic_file_path
+ }
+ elif hasattr(obj, 'isTable') and obj.isTable:
+ self._add_table_config(config['tables'], obj)
+ elif hasattr(obj, 'makeRobosuiteObject') or (hasattr(obj, 'objectType') and obj.objectType == 'MJCF'):
+ self._add_object_config(config['objects'], obj, scenic_file_path)
+
+ return config
+
+ def _add_robot_config(self, robots: List, obj):
+ """Add robot configuration."""
+ robots.append({
+ 'type': obj.robotType,
+ 'position': [obj.position.x, obj.position.y, 0]
+ })
+ self.robots.append(obj)
+
+ def _add_table_config(self, tables: List, obj):
+ """Add table configuration."""
+ tables.append({
+ 'position': [obj.position.x, obj.position.y, obj.position.z],
+ 'size': (obj.width, obj.length, 0.05) # MultiTableArena tabletop thickness
+ })
+
+ def _add_object_config(self, objects: List, obj, scenic_file_path):
+ """Add object configuration."""
+ scenic_name = getattr(obj, 'name', None)
+
+ if hasattr(obj, 'objectType') and obj.objectType == 'MJCF':
+ obj_name = f"{scenic_name}_{len(objects)}" if scenic_name else f"mjcf_object_{len(objects)}"
+ obj_type = 'MJCF'
+ else:
+ base_name = scenic_name if scenic_name else obj.__class__.__name__
+ obj_name = f"{base_name}_{len(objects)}"
+ obj_type = obj.__class__.__name__
+
+ config = {
+ 'type': obj_type,
+ 'name': obj_name,
+ 'position': [obj.position.x, obj.position.y, obj.position.z],
+ 'color': getattr(obj, 'color', [1, 0, 0, 1]),
+ 'scenic_obj': obj # Store reference to the Scenic object
+ }
+
+ self.object_name_map[obj] = obj_name
+
+ if obj_type == 'MJCF':
+ config['mjcfXml'] = getattr(obj, 'mjcfXml', '')
+ config['scenic_file_path'] = scenic_file_path
+
+ objects.append(config)
+
+ def _get_robot_arg(self, robots: List) -> Union[str, List[str]]:
+ """Get robot argument for environment creation."""
+ if not robots:
+ raise ValueError("At least one robot is required")
+ return ([r['type'] for r in robots] if len(robots) > 1
+ else robots[0]['type'])
+
+ def _detect_controller_type(self):
+ """Detect controller type from first robot."""
+ if self.robots and self.robosuite_env.robots:
+ first_robot = self.robosuite_env.robots[0]
+ if hasattr(first_robot, 'controller'):
+ self.controller_type = type(first_robot.controller).__name__
+ if hasattr(first_robot.controller, 'control_dim'):
+ self.action_dim = first_robot.controller.control_dim
+
+ def _setup_body_mapping(self):
+ """Map environment objects to MuJoCo body IDs."""
+ for obj in self.objects:
+ if obj in self.object_name_map:
+ self._map_object_body(obj)
+
+ def _map_object_body(self, obj):
+ """Map single object to body ID."""
+ obj_name = self.object_name_map.get(obj)
+ if not obj_name:
+ return
+
+ for suffix in ['', '_main', '_body']:
+ body_name = f"{obj_name}{suffix}"
+ try:
+ body_id = mujoco.mj_name2id(
+ self.model, mujoco.mjtObj.mjOBJ_BODY, body_name
+ )
+ if body_id != -1:
+ self.body_id_map[obj_name] = body_id
+ self.prev_positions[obj_name] = self.data.xpos[body_id].copy()
+ break
+ except:
+ continue
+
+ def createObjectInSimulator(self, obj):
+ """Required by Scenic's Simulator interface."""
+ pass
+
+ def executeActions(self, allActions: Dict[Any, List]) -> None:
+ """Execute actions by calling their applyTo methods."""
+ self.pending_robot_action = None
+ super().executeActions(allActions)
+
+ def step(self):
+ """Step the simulation forward one timestep."""
+ if hasattr(self, '_done') and self._done:
+ return
+
+ for name, body_id in self.body_id_map.items():
+ self.prev_positions[name] = self.data.xpos[body_id].copy()
+
+ action = (self.pending_robot_action if self.pending_robot_action is not None
+ else np.zeros(self.action_dim))
+
+ for _ in range(self.physics_steps):
+ obs, reward, done, info = self.robosuite_env.step(action)
+ self._current_obs = obs
+ self._done = done
+ if self.render:
+ self.robosuite_env.render()
+ if done:
+ break
+
+ self.pending_robot_action = None
+
+ def getProperties(self, obj, properties: List[str]) -> Dict[str, Any]:
+ """Get current property values for an object."""
+ values = {}
+ robot_idx = self.robots.index(obj) if obj in self.robots else None
+
+ for prop in properties:
+ if prop == 'position':
+ values[prop] = self._get_object_position(obj)
+ elif prop == 'jointPositions' and robot_idx is not None:
+ values[prop] = self._get_robot_joints(robot_idx)
+ elif prop == 'eefPos' and robot_idx is not None:
+ values[prop] = self._get_eef_position(robot_idx)
+ elif prop == 'gripperState' and robot_idx is not None:
+ values[prop] = self._get_gripper_state(robot_idx)
+ elif prop in ['yaw', 'pitch', 'roll', 'orientation']:
+ values[prop] = self._get_object_orientation(obj, prop)
+ elif prop == 'velocity':
+ values[prop] = self._get_object_velocity(obj)
+ elif prop in ['angularVelocity', 'angularSpeed']:
+ values[prop] = self._get_object_angular_velocity(obj, prop)
+ elif prop == 'speed':
+ values[prop] = self._get_object_speed(obj)
+ else:
+ values[prop] = getattr(obj, prop, None)
+
+ return values
+
+ def _get_object_position(self, obj) -> Vector:
+ """Get object position from MuJoCo."""
+ obj_name = self.object_name_map.get(obj)
+ if obj_name and obj_name in self.body_id_map:
+ body_id = self.body_id_map[obj_name]
+ pos = self.data.xpos[body_id]
+ return Vector(pos[0], pos[1], pos[2])
+ return obj.position
+
+ def _get_object_orientation(self, obj, prop: str):
+ """Get object orientation from MuJoCo."""
+ obj_name = self.object_name_map.get(obj)
+ if obj_name and obj_name in self.body_id_map:
+ body_id = self.body_id_map[obj_name]
+ quat = self.data.xquat[body_id] # [w, x, y, z]
+
+ # Convert quaternion to Euler angles (ZXY order for Scenic)
+ # This matches Scenic's orientation convention
+ w, x, y, z = quat
+
+ # Convert to Euler angles
+ # Roll (x-axis rotation)
+ sinr_cosp = 2 * (w * x + y * z)
+ cosr_cosp = 1 - 2 * (x * x + y * y)
+ roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+ # Pitch (y-axis rotation)
+ sinp = 2 * (w * y - z * x)
+ pitch = np.arcsin(np.clip(sinp, -1, 1))
+
+ # Yaw (z-axis rotation)
+ siny_cosp = 2 * (w * z + x * y)
+ cosy_cosp = 1 - 2 * (y * y + z * z)
+ yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+ if prop == 'yaw':
+ return yaw
+ elif prop == 'pitch':
+ return pitch
+ elif prop == 'roll':
+ return roll
+ elif prop == 'orientation':
+ # Return a Scenic Orientation object
+ return Orientation.fromEuler(yaw, pitch, roll)
+
+ # Fallback to stored values
+ return getattr(obj, prop, 0 if prop in ['yaw', 'pitch', 'roll'] else None)
+
+ def _get_object_velocity(self, obj) -> Vector:
+ """Get object linear velocity from MuJoCo."""
+ obj_name = self.object_name_map.get(obj)
+ if obj_name:
+ # For objects with free joints, velocity is in qvel
+ # Try to find the joint for this object
+ joint_name = None
+
+ # Look for the joint name in our mujoco objects
+ for mobj in self.robosuite_env.mujoco_objects if hasattr(self, 'robosuite_env') else []:
+ if mobj.name == obj_name:
+ if hasattr(mobj, 'joint_name'):
+ joint_name = mobj.joint_name
+ break
+
+ if joint_name:
+ try:
+ # Get joint ID
+ joint_id = mujoco.mj_name2id(
+ self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name
+ )
+ if joint_id != -1:
+ # Get qvel address for this joint
+ # Free joint has 6 DOF for velocity: 3 linear + 3 angular
+ qvel_addr = self.model.jnt_dofadr[joint_id]
+ # Linear velocity is in the first 3 components
+ vel = self.data.qvel[qvel_addr:qvel_addr+3]
+ return Vector(vel[0], vel[1], vel[2])
+ except Exception as e:
+ pass
+
+ # Fallback to body cvel if no joint found
+ if obj_name in self.body_id_map:
+ body_id = self.body_id_map[obj_name]
+ vel = self.data.cvel[body_id][:3]
+ return Vector(vel[0], vel[1], vel[2])
+
+ # Final fallback
+ return getattr(obj, 'velocity', Vector(0, 0, 0))
+
+ def _get_object_angular_velocity(self, obj, prop: str):
+ """Get object angular velocity from MuJoCo."""
+ obj_name = self.object_name_map.get(obj)
+ if obj_name:
+ # For objects with free joints, angular velocity is in qvel
+ joint_name = None
+
+ # Look for the joint name in our mujoco objects
+ for mobj in self.robosuite_env.mujoco_objects if hasattr(self, 'robosuite_env') else []:
+ if mobj.name == obj_name:
+ if hasattr(mobj, 'joint_name'):
+ joint_name = mobj.joint_name
+ break
+
+ if joint_name:
+ try:
+ # Get joint ID
+ joint_id = mujoco.mj_name2id(
+ self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name
+ )
+ if joint_id != -1:
+ # Get qvel address for this joint
+ # Free joint has 6 DOF for velocity: 3 linear + 3 angular
+ qvel_addr = self.model.jnt_dofadr[joint_id]
+ # Angular velocity is in components 3-5
+ ang_vel = self.data.qvel[qvel_addr+3:qvel_addr+6]
+
+ if prop == 'angularVelocity':
+ return Vector(ang_vel[0], ang_vel[1], ang_vel[2])
+ elif prop == 'angularSpeed':
+ return np.linalg.norm(ang_vel)
+ except Exception as e:
+ pass
+
+ # Fallback to body cvel if no joint found
+ if obj_name in self.body_id_map:
+ body_id = self.body_id_map[obj_name]
+ ang_vel = self.data.cvel[body_id][3:]
+
+ if prop == 'angularVelocity':
+ return Vector(ang_vel[0], ang_vel[1], ang_vel[2])
+ elif prop == 'angularSpeed':
+ return np.linalg.norm(ang_vel)
+
+ # Final fallback
+ if prop == 'angularVelocity':
+ return getattr(obj, 'angularVelocity', Vector(0, 0, 0))
+ else:
+ return getattr(obj, 'angularSpeed', 0)
+
+ def _get_object_speed(self, obj) -> float:
+ """Get object speed (magnitude of velocity)."""
+ velocity = self._get_object_velocity(obj)
+ return np.linalg.norm([velocity.x, velocity.y, velocity.z])
+
+ def _get_robot_joints(self, robot_idx: int) -> List:
+ """Get robot joint positions."""
+ if robot_idx < len(self.robosuite_env.robots):
+ return list(self.robosuite_env.robots[robot_idx]._joint_positions)
+ return []
+
+ def _get_eef_position(self, robot_idx: int) -> List:
+ """Get end-effector position."""
+ if self._current_obs:
+ return list(self._current_obs.get(f'robot{robot_idx}_eef_pos', [0, 0, 0]))
+ return [0, 0, 0]
+
+ def _get_gripper_state(self, robot_idx: int) -> List:
+ """Get gripper state."""
+ if self._current_obs:
+ return list(self._current_obs.get(f'robot{robot_idx}_gripper_qpos', [0, 0]))
+ return [0, 0]
+
+ def getCurrentObservation(self) -> Optional[Dict]:
+ """Get current observation dictionary."""
+ return self._current_obs
+
+ def destroy(self):
+ """Clean up simulation resources."""
+ if self.robosuite_env:
+ self.robosuite_env.close()
+ self.robosuite_env = None
+
+
+# Controller Interface
+class OSCPositionAction(Action):
+ """Operational Space Control for end-effector."""
+
+ def __init__(self, position_delta: Optional[List[float]] = None,
+ orientation_delta: Optional[List[float]] = None,
+ gripper: Optional[float] = None):
+ self.position_delta = position_delta or [0, 0, 0]
+ self.orientation_delta = orientation_delta or [0, 0, 0]
+ self.gripper = gripper if gripper is not None else 0
+
+ def canBeTakenBy(self, agent) -> bool:
+ """Only robots can take this action."""
+ return hasattr(agent, 'robotType')
+
+ def applyTo(self, agent, sim: RobosuiteSimulation):
+ """Apply action to agent."""
+ robot_idx = sim.robots.index(agent)
+
+ if sim.controller_type == 'JOINT_POSITION':
+ action = np.zeros(sim.action_dim)
+ action[:3] = self.position_delta
+ else:
+ action = np.zeros(7)
+ action[:3] = self.position_delta
+ action[3:6] = self.orientation_delta
+ action[6] = self.gripper
+
+ sim.pending_robot_action = action
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/arena_meshes/arena_config.json b/src/scenic/simulators/robosuite/utils/arena_meshes/arena_config.json
new file mode 100644
index 000000000..720869083
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/arena_meshes/arena_config.json
@@ -0,0 +1,52 @@
+{
+ "floor": {
+ "position": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "dimensions": [
+ 10.0,
+ 10.0,
+ 0.01
+ ],
+ "bounds": [
+ [
+ -5.0,
+ -5.0,
+ -0.005
+ ],
+ [
+ 5.0,
+ 5.0,
+ 0.005
+ ]
+ ],
+ "num_meshes": 1
+ },
+ "walls": {
+ "position": [
+ 0.5,
+ 0.0,
+ 1.5
+ ],
+ "dimensions": [
+ 5.02,
+ 6.02,
+ 3.000000000000001
+ ],
+ "bounds": [
+ [
+ -2.01,
+ -3.01,
+ -4.440892098500626e-16
+ ],
+ [
+ 3.01,
+ 3.01,
+ 3.0000000000000004
+ ]
+ ],
+ "num_meshes": 6
+ }
+}
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/arena_meshes/floor.glb b/src/scenic/simulators/robosuite/utils/arena_meshes/floor.glb
new file mode 100644
index 000000000..72745b430
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/arena_meshes/floor.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/arena_meshes/walls.glb b/src/scenic/simulators/robosuite/utils/arena_meshes/walls.glb
new file mode 100644
index 000000000..496c6b36d
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/arena_meshes/walls.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/default.scenic b/src/scenic/simulators/robosuite/utils/default.scenic
new file mode 100644
index 000000000..1878bafee
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/default.scenic
@@ -0,0 +1,47 @@
+# src/scenic/simulators/robosuite/utils/default.scenic
+import trimesh
+from scenic.core.utils import repairMesh
+
+# Helper function to load and repair
+def load_and_repair(path, name):
+ loaded = trimesh.load(path)
+ if isinstance(loaded, trimesh.Scene):
+ mesh = loaded.dump(concatenate=True)
+ else:
+ mesh = loaded
+
+ repaired = repairMesh(mesh, pitch=0.02, verbose=False)
+ output_path = f"/home/lol/Documents/Scenic/{name}_repaired.glb"
+ repaired.export(output_path)
+ print(f"{name}: {repaired.bounds[1] - repaired.bounds[0]}")
+ return MeshShape(repaired)
+
+# Load and repair meshes
+red_cube_shape = load_and_repair("/home/lol/Documents/Scenic/red_cube.glb", "red_cube")
+dragon_shape = load_and_repair("/home/lol/Documents/Scenic/dragon.glb", "dragon")
+bread_shape = load_and_repair("/home/lol/Documents/Scenic/bread.glb", "bread")
+table_shape = load_and_repair("/home/lol/Documents/Scenic/table.glb", "table")
+
+# Load existing table mesh (no repair)
+table_mesh = MeshShape.fromFile("/home/lol/Documents/Scenic/src/scenic/simulators/robosuite/utils/table_meshes/standard_table.glb")
+
+# Create objects
+standard_table = new Object at (0, 0, 0),
+ with shape table_mesh,
+ with color (0, 1, 0, 1)
+
+custom_table = new Object at (2, 0, 0),
+ with shape table_shape,
+ with color (0.6, 0.3, 0.1, 1)
+
+red_cube = new Object at (4, 0, 0),
+ with shape red_cube_shape,
+ with color (1, 0, 0, 1)
+
+dragon = new Object at (6, 0, 0),
+ with shape dragon_shape,
+ with color (0.8, 0.2, 0.2, 1)
+
+bread = new Object at (8, 0, 0),
+ with shape bread_shape,
+ with color (0.9, 0.7, 0.4, 1)
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/extract_arena.py b/src/scenic/simulators/robosuite/utils/extract_arena.py
new file mode 100644
index 000000000..0515c828b
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/extract_arena.py
@@ -0,0 +1,227 @@
+"""Extract arena components (walls and floor) from RoboSuite EmptyArena."""
+
+import numpy as np
+import trimesh
+import mujoco
+import json
+import os
+from collections import defaultdict
+from typing import Dict, Set, Optional
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.models.arenas import EmptyArena
+from robosuite.models.tasks import ManipulationTask
+
+
+class ArenaExtractor:
+ """Extract arena components from MuJoCo environment."""
+
+ def __init__(self, env):
+ self.env = env
+ self.model = env.sim.model._model
+ self.data = env.sim.data._data
+ self.mesh_cache = self._cache_meshes()
+
+ def _cache_meshes(self) -> Dict[int, trimesh.Trimesh]:
+ """Cache all meshes from MuJoCo model."""
+ cache = {}
+ for mesh_id in range(self.model.nmesh):
+ mesh = self._extract_mesh_data(mesh_id)
+ if mesh is not None:
+ cache[mesh_id] = mesh
+ return cache
+
+ def _extract_mesh_data(self, mesh_id: int) -> Optional[trimesh.Trimesh]:
+ """Extract single mesh from MuJoCo."""
+ try:
+ vert_start = self.model.mesh_vertadr[mesh_id]
+ vert_count = self.model.mesh_vertnum[mesh_id]
+ face_start = self.model.mesh_faceadr[mesh_id]
+ face_count = self.model.mesh_facenum[mesh_id]
+
+ if vert_count == 0 or face_count == 0:
+ return None
+
+ vertices = self.model.mesh_vert[vert_start:vert_start + vert_count].reshape(-1, 3)
+ faces = self.model.mesh_face[face_start:face_start + face_count].reshape(-1, 3)
+
+ scale = self.model.mesh_scale[mesh_id]
+ vertices = vertices * scale
+
+ mesh = trimesh.Trimesh(vertices=vertices, faces=faces)
+ mesh.fix_normals()
+ return mesh
+ except Exception:
+ return None
+
+ def _create_primitive(self, geom_type: int, size: np.ndarray) -> Optional[trimesh.Trimesh]:
+ """Create primitive geometry mesh."""
+ try:
+ if geom_type == mujoco.mjtGeom.mjGEOM_BOX:
+ return trimesh.creation.box(extents=size * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_SPHERE:
+ return trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER:
+ return trimesh.creation.cylinder(radius=size[0], height=size[1] * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE:
+ if size[1] < 0.001:
+ return trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ return trimesh.creation.capsule(radius=size[0], height=size[1] * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
+ return trimesh.creation.box(extents=[10, 10, 0.01])
+ else:
+ return None
+ except:
+ return None
+
+ def extract_arena(self) -> Dict[str, Set[int]]:
+ """Extract arena components."""
+ components = defaultdict(set)
+ processed_geoms = set()
+
+ # Handle world body geoms (walls, floor)
+ for geom_id in range(self.model.ngeom):
+ geom_group = self.model.geom_group[geom_id]
+ if geom_group != 1:
+ continue
+
+ body_id = self.model.geom_bodyid[geom_id]
+ if body_id == 0: # World body
+ geom_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_id) or ""
+ geom_type = self.model.geom_type[geom_id]
+
+ # Check for walls first
+ if any(kw in geom_name.lower() for kw in ['wall', 'boundary', 'barrier', 'corner']):
+ components['walls'].add(geom_id)
+ processed_geoms.add(geom_id)
+ elif 'floor' in geom_name.lower():
+ components['floor'].add(geom_id)
+ processed_geoms.add(geom_id)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
+ components['floor'].add(geom_id)
+ processed_geoms.add(geom_id)
+
+ print(f" Found arena components: {list(components.keys())}")
+ print(f" Walls: {len(components.get('walls', set()))} geoms")
+ print(f" Floor: {len(components.get('floor', set()))} geoms")
+
+ return components
+
+ def _extract_geom_ids(self, geom_ids: Set[int]) -> Optional[trimesh.Scene]:
+ """Extract specific geometry IDs into a scene."""
+ scene = trimesh.Scene()
+
+ for geom_id in geom_ids:
+ geom_group = self.model.geom_group[geom_id]
+ if geom_group != 1:
+ continue
+
+ geom_type = self.model.geom_type[geom_id]
+ geom_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_id)
+
+ mesh = None
+ if geom_type == mujoco.mjtGeom.mjGEOM_MESH:
+ mesh_id = self.model.geom_dataid[geom_id]
+ mesh = self.mesh_cache.get(mesh_id)
+ if mesh:
+ mesh = mesh.copy()
+ elif geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
+ mesh = trimesh.creation.box(extents=[10, 10, 0.01])
+ else:
+ geom_size = self.model.geom_size[geom_id].copy()
+ mesh = self._create_primitive(geom_type, geom_size)
+
+ if mesh:
+ transform = np.eye(4)
+ transform[:3, :3] = self.data.geom_xmat[geom_id].reshape(3, 3)
+ transform[:3, 3] = self.data.geom_xpos[geom_id]
+
+ node_name = geom_name if geom_name else f"geom_{geom_id}"
+ scene.add_geometry(mesh, node_name=node_name, transform=transform)
+
+ return scene if len(scene.geometry) > 0 else None
+
+ def export_arena_components(self, output_dir: str = "arena_meshes"):
+ """Export arena components with configuration."""
+ os.makedirs(output_dir, exist_ok=True)
+
+ components = self.extract_arena()
+ metadata = {}
+
+ for component_name, geom_ids in components.items():
+ if not geom_ids:
+ continue
+
+ scene = self._extract_geom_ids(geom_ids)
+ if scene:
+ output_path = os.path.join(output_dir, f"{component_name}.glb")
+ scene.export(output_path)
+
+ bounds = scene.bounds
+ center = (bounds[0] + bounds[1]) / 2
+ dimensions = bounds[1] - bounds[0]
+
+ metadata[component_name] = {
+ 'position': center.tolist(),
+ 'dimensions': dimensions.tolist(),
+ 'bounds': bounds.tolist(),
+ 'num_meshes': len(scene.geometry)
+ }
+
+ print(f" ✓ Exported {component_name}.glb ({len(scene.geometry)} meshes)")
+ print(f" Position: {center}")
+ print(f" Dimensions: {dimensions}")
+
+ config_path = os.path.join(output_dir, "arena_config.json")
+ with open(config_path, 'w') as f:
+ json.dump(metadata, f, indent=2)
+
+ print(f"\n✓ Saved configuration to {config_path}")
+ return metadata
+
+
+class ArenaOnlyEnv(ManipulationEnv):
+ """Minimal environment with just arena."""
+
+ def __init__(self, **kwargs):
+ super().__init__(
+ robots="Panda",
+ has_renderer=False,
+ has_offscreen_renderer=False,
+ use_camera_obs=False,
+ **kwargs
+ )
+
+ def _load_model(self):
+ super()._load_model()
+ self.mujoco_arena = EmptyArena()
+ self.mujoco_arena.set_origin([0, 0, 0])
+
+ self.model = ManipulationTask(
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=[]
+ )
+
+ def reward(self, action):
+ return 0
+
+
+def extract_arena():
+ """Extract arena components."""
+ print("Extracting arena components...")
+
+ env = ArenaOnlyEnv()
+ env.reset()
+
+ extractor = ArenaExtractor(env)
+ metadata = extractor.export_arena_components()
+
+ env.close()
+
+ print("\n✓ Arena extraction complete")
+ return metadata
+
+
+if __name__ == "__main__":
+ extract_arena()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/extract_robots.py b/src/scenic/simulators/robosuite/utils/extract_robots.py
new file mode 100644
index 000000000..9a75a36c5
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/extract_robots.py
@@ -0,0 +1,282 @@
+"""Extract robot meshes from RoboSuite."""
+
+import numpy as np
+import trimesh
+import mujoco
+import os
+import re
+from collections import defaultdict
+from typing import Dict, Set, Optional
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.models.arenas import EmptyArena
+from robosuite.models.tasks import ManipulationTask
+
+
+class RobotExtractor:
+ """Extract robots from MuJoCo environment."""
+
+ def __init__(self, env):
+ self.env = env
+ self.model = env.sim.model._model
+ self.data = env.sim.data._data
+ self.mesh_cache = self._cache_meshes()
+
+ def _cache_meshes(self) -> Dict[int, trimesh.Trimesh]:
+ """Cache all meshes from MuJoCo model."""
+ cache = {}
+ for mesh_id in range(self.model.nmesh):
+ mesh = self._extract_mesh_data(mesh_id)
+ if mesh is not None:
+ cache[mesh_id] = mesh
+ return cache
+
+ def _extract_mesh_data(self, mesh_id: int) -> Optional[trimesh.Trimesh]:
+ """Extract single mesh from MuJoCo."""
+ try:
+ vert_start = self.model.mesh_vertadr[mesh_id]
+ vert_count = self.model.mesh_vertnum[mesh_id]
+ face_start = self.model.mesh_faceadr[mesh_id]
+ face_count = self.model.mesh_facenum[mesh_id]
+
+ if vert_count == 0 or face_count == 0:
+ return None
+
+ vertices = self.model.mesh_vert[vert_start:vert_start + vert_count].reshape(-1, 3)
+ faces = self.model.mesh_face[face_start:face_start + face_count].reshape(-1, 3)
+
+ scale = self.model.mesh_scale[mesh_id]
+ vertices = vertices * scale
+
+ mesh = trimesh.Trimesh(vertices=vertices, faces=faces)
+ mesh.fix_normals()
+ return mesh
+ except Exception:
+ return None
+
+ def _create_primitive(self, geom_type: int, size: np.ndarray) -> Optional[trimesh.Trimesh]:
+ """Create primitive geometry mesh."""
+ try:
+ if geom_type == mujoco.mjtGeom.mjGEOM_BOX:
+ return trimesh.creation.box(extents=size * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_SPHERE:
+ return trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER:
+ return trimesh.creation.cylinder(radius=size[0], height=size[1] * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE:
+ if size[1] < 0.001:
+ return trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ return trimesh.creation.capsule(radius=size[0], height=size[1] * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
+ return trimesh.creation.box(extents=[10, 10, 0.01])
+ else:
+ return None
+ except:
+ return None
+
+ def extract_robot(self) -> Dict[str, Set[int]]:
+ """Extract robot components."""
+ components = defaultdict(set)
+ robot_indices = set()
+ processed_geoms = set()
+
+ # Skip world body geoms
+ for geom_id in range(self.model.ngeom):
+ geom_group = self.model.geom_group[geom_id]
+ if geom_group != 1:
+ continue
+ body_id = self.model.geom_bodyid[geom_id]
+ if body_id == 0:
+ processed_geoms.add(geom_id)
+
+ # Build body hierarchy
+ body_to_geoms = defaultdict(set)
+ body_to_parent = {}
+ body_names = {}
+
+ for body_id in range(1, self.model.nbody):
+ body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, body_id) or ""
+ body_names[body_id] = body_name
+ parent_id = self.model.body_parentid[body_id]
+ body_to_parent[body_id] = parent_id
+
+ # Map visual geoms to bodies
+ for geom_id in range(self.model.ngeom):
+ if geom_id in processed_geoms:
+ continue
+ geom_group = self.model.geom_group[geom_id]
+ if geom_group != 1:
+ continue
+ body_id = self.model.geom_bodyid[geom_id]
+ if body_id > 0:
+ body_to_geoms[body_id].add(geom_id)
+
+ # Process robot bodies
+ processed_bodies = set()
+
+ for body_id, geom_ids in body_to_geoms.items():
+ if not geom_ids or body_id in processed_bodies:
+ continue
+
+ body_name = body_names[body_id]
+
+ robot_patterns = ['robot', 'link', 'gripper', 'finger', 'hand', 'mount', 'base', 'eef', 'arm', 'robotiq']
+ if any(pattern in body_name.lower() for pattern in robot_patterns):
+ match = re.search(r'robot(\d+)', body_name.lower())
+ if match:
+ idx = int(match.group(1))
+ robot_indices.add(idx)
+ components[f'robot_{idx}'].update(geom_ids)
+ else:
+ robot_indices.add(0)
+ components['robot_0'].update(geom_ids)
+ processed_bodies.add(body_id)
+ continue
+
+ # Skip infrastructure
+ geom_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, list(geom_ids)[0]) or ""
+ combined = f"{body_name} {geom_name}".lower()
+ if any(kw in combined for kw in ['eef_target', 'controller', 'pedestal', 'torso', 'fixed_mount']):
+ processed_bodies.add(body_id)
+ continue
+
+ # Collect mount/base for robots
+ for idx in robot_indices:
+ robot_key = f'robot_{idx}'
+ for body_id, geom_ids in body_to_geoms.items():
+ if body_id in processed_bodies:
+ continue
+ body_name = body_names[body_id]
+ if any(kw in body_name.lower() for kw in ['mount', 'base', 'pedestal', 'torso']):
+ current = body_id
+ for _ in range(10):
+ if current in body_to_parent:
+ current = body_to_parent[current]
+ if 'robot' in body_names.get(current, '').lower():
+ components[robot_key].update(geom_ids)
+ break
+ else:
+ components[robot_key].update(geom_ids)
+ break
+
+ print(f" Robot indices found: {sorted(robot_indices)}")
+ print(f" Total geoms collected: {len(components.get('robot_0', set()))}")
+
+ return components
+
+ def _extract_geom_ids(self, geom_ids: Set[int]) -> Optional[trimesh.Scene]:
+ """Extract specific geometry IDs into a scene."""
+ scene = trimesh.Scene()
+
+ for geom_id in geom_ids:
+ geom_group = self.model.geom_group[geom_id]
+ if geom_group != 1:
+ continue
+
+ geom_type = self.model.geom_type[geom_id]
+ geom_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_id)
+
+ mesh = None
+ if geom_type == mujoco.mjtGeom.mjGEOM_MESH:
+ mesh_id = self.model.geom_dataid[geom_id]
+ mesh = self.mesh_cache.get(mesh_id)
+ if mesh:
+ mesh = mesh.copy()
+ elif geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
+ mesh = trimesh.creation.box(extents=[10, 10, 0.01])
+ else:
+ geom_size = self.model.geom_size[geom_id].copy()
+ mesh = self._create_primitive(geom_type, geom_size)
+
+ if mesh:
+ transform = np.eye(4)
+ transform[:3, :3] = self.data.geom_xmat[geom_id].reshape(3, 3)
+ transform[:3, 3] = self.data.geom_xpos[geom_id]
+
+ node_name = geom_name if geom_name else f"geom_{geom_id}"
+ scene.add_geometry(mesh, node_name=node_name, transform=transform)
+
+ return scene if len(scene.geometry) > 0 else None
+
+ def export_robot(self, output_path: str):
+ """Export the robot mesh."""
+ components = self.extract_robot()
+
+ if 'robot_0' in components:
+ scene = self._extract_geom_ids(components['robot_0'])
+ if scene:
+ scene.export(output_path)
+ bounds = scene.bounds
+ dimensions = bounds[1] - bounds[0]
+ print(f" Meshes in scene: {len(scene.geometry)}")
+ return dimensions
+ else:
+ print(f" Warning: No robot_0 component found")
+ return None
+
+
+class MinimalEnv(ManipulationEnv):
+ """Minimal environment for robot extraction."""
+
+ def __init__(self, robots="Panda", **kwargs):
+ super().__init__(
+ robots=robots,
+ has_renderer=False,
+ has_offscreen_renderer=False,
+ use_camera_obs=False,
+ **kwargs
+ )
+
+ def _load_model(self):
+ super()._load_model()
+ self.mujoco_arena = EmptyArena()
+ self.mujoco_arena.set_origin([0, 0, 0])
+
+ self.model = ManipulationTask(
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=[]
+ )
+
+ def reward(self, action):
+ return 0
+
+
+def extract_all_robots():
+ """Extract all robot types."""
+ robot_types = ["Panda", "UR5e", "Sawyer", "Jaco", "IIWA"]
+ output_dir = "robot_meshes"
+ os.makedirs(output_dir, exist_ok=True)
+
+ dimensions_info = {}
+
+ for robot_type in robot_types:
+ print(f"\nExtracting {robot_type}...")
+ try:
+ env = MinimalEnv(robots=robot_type)
+ env.reset()
+
+ extractor = RobotExtractor(env)
+ output_path = os.path.join(output_dir, f"{robot_type}.glb")
+ dimensions = extractor.export_robot(output_path)
+
+ if dimensions is not None:
+ dimensions_info[robot_type] = dimensions.tolist()
+ print(f" ✓ Saved: {output_path}")
+ print(f" Dimensions: {dimensions}")
+
+ env.close()
+
+ except Exception as e:
+ print(f" ✗ Failed: {e}")
+
+ # Write dimensions file
+ with open(os.path.join(output_dir, "dimensions.txt"), "w") as f:
+ for robot, dims in dimensions_info.items():
+ f.write(f"{robot}: {dims}\n")
+
+ print(f"\n✓ Extracted {len(dimensions_info)} robots to {output_dir}/")
+
+
+if __name__ == "__main__":
+ extract_all_robots()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/extract_table.py b/src/scenic/simulators/robosuite/utils/extract_table.py
new file mode 100644
index 000000000..781da932c
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/extract_table.py
@@ -0,0 +1,126 @@
+"""Extract table mesh from RoboSuite's MultiTableArena."""
+
+import numpy as np
+import trimesh
+import mujoco
+import json
+import os
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.models.arenas import MultiTableArena
+from robosuite.models.tasks import ManipulationTask
+
+
+class TableEnv(ManipulationEnv):
+ """Environment with just a table for extraction."""
+
+ def __init__(self, **kwargs):
+ super().__init__(
+ robots="Panda",
+ has_renderer=False,
+ has_offscreen_renderer=False,
+ use_camera_obs=False,
+ **kwargs
+ )
+
+ def _load_model(self):
+ super()._load_model()
+ # Create arena with one standard table
+ self.mujoco_arena = MultiTableArena(
+ table_offsets=[(0, 0, 0.8)],
+ table_full_sizes=[(0.8, 0.8, 0.05)],
+ has_legs=[True]
+ )
+ self.mujoco_arena.set_origin([0, 0, 0])
+
+ self.model = ManipulationTask(
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=[]
+ )
+
+ def reward(self, action):
+ return 0
+
+
+def extract_table():
+ """Extract table mesh with legs."""
+ print("Extracting table mesh...")
+
+ env = TableEnv()
+ env.reset()
+
+ model = env.sim.model._model
+ data = env.sim.data._data
+
+ # Collect table geometry
+ scene = trimesh.Scene()
+
+ for geom_id in range(model.ngeom):
+ if model.geom_group[geom_id] != 1: # Only visual
+ continue
+
+ geom_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, geom_id) or ""
+ body_id = model.geom_bodyid[geom_id]
+ body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, body_id) or ""
+
+ # Skip robot and arena components
+ if any(kw in body_name.lower() for kw in ['robot', 'link', 'gripper', 'wall', 'floor']):
+ continue
+ if body_id == 0: # Skip world body
+ continue
+
+ # Table components
+ if 'table' in body_name.lower() or 'table' in geom_name.lower():
+ geom_type = model.geom_type[geom_id]
+ geom_size = model.geom_size[geom_id].copy()
+
+ # Create primitive mesh
+ if geom_type == mujoco.mjtGeom.mjGEOM_BOX:
+ mesh = trimesh.creation.box(extents=geom_size * 2)
+ elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER:
+ mesh = trimesh.creation.cylinder(radius=geom_size[0], height=geom_size[1] * 2)
+ else:
+ continue
+
+ # Apply transform
+ transform = np.eye(4)
+ transform[:3, :3] = data.geom_xmat[geom_id].reshape(3, 3)
+ transform[:3, 3] = data.geom_xpos[geom_id]
+
+ scene.add_geometry(mesh, node_name=f"{body_name}_{geom_name}", transform=transform)
+ print(f" Added: {body_name} - {geom_name}")
+
+ # Export
+ output_dir = "utils/table_meshes"
+ os.makedirs(output_dir, exist_ok=True)
+
+ output_path = os.path.join(output_dir, "standard_table.glb")
+ scene.export(output_path)
+
+ # Calculate dimensions and metadata
+ bounds = scene.bounds
+ dimensions = bounds[1] - bounds[0]
+ center = (bounds[0] + bounds[1]) / 2
+
+ metadata = {
+ "dimensions": dimensions.tolist(),
+ "center": center.tolist(),
+ "bounds": bounds.tolist(),
+ "table_height": 0.8, # Standard height
+ "table_surface_height": 0.825 # Top surface (0.8 + 0.025)
+ }
+
+ config_path = os.path.join(output_dir, "table_config.json")
+ with open(config_path, 'w') as f:
+ json.dump(metadata, f, indent=2)
+
+ print(f"✓ Exported table to {output_path}")
+ print(f" Dimensions: {dimensions}")
+ print(f" Center: {center}")
+
+ env.close()
+
+
+if __name__ == "__main__":
+ extract_table()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/extract_table_components.py b/src/scenic/simulators/robosuite/utils/extract_table_components.py
new file mode 100644
index 000000000..161dff3a5
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/extract_table_components.py
@@ -0,0 +1,170 @@
+"""Extract table components separately for modular composition."""
+
+import numpy as np
+import trimesh
+import mujoco
+import json
+import os
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.models.arenas import MultiTableArena
+from robosuite.models.tasks import ManipulationTask
+
+
+class TableEnv(ManipulationEnv):
+ """Environment with table for extraction."""
+
+ def __init__(self, **kwargs):
+ super().__init__(
+ robots="Panda",
+ has_renderer=False,
+ has_offscreen_renderer=False,
+ use_camera_obs=False,
+ **kwargs
+ )
+
+ def _load_model(self):
+ super()._load_model()
+ self.mujoco_arena = MultiTableArena(
+ table_offsets=[(0, 0, 0.8)],
+ table_full_sizes=[(0.8, 0.8, 0.05)],
+ has_legs=[True]
+ )
+ self.mujoco_arena.set_origin([0, 0, 0])
+
+ self.model = ManipulationTask(
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=[]
+ )
+
+ def reward(self, action):
+ return 0
+
+
+def extract_table_components():
+ """Extract table top and single leg as separate meshes."""
+ print("Extracting table components...")
+
+ env = TableEnv()
+ env.reset()
+
+ model = env.sim.model._model
+ data = env.sim.data._data
+
+ table_top_scene = trimesh.Scene()
+ table_leg_mesh = None
+ leg_positions = []
+ leg_radius = 0.025
+ leg_height = 0.775
+ table_top_pos = None
+ table_top_size = None
+
+ for geom_id in range(model.ngeom):
+ if model.geom_group[geom_id] != 1:
+ continue
+
+ geom_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, geom_id) or ""
+ body_id = model.geom_bodyid[geom_id]
+ body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, body_id) or ""
+
+ if body_id == 0 or any(kw in body_name.lower() for kw in ['robot', 'wall', 'floor', 'link', 'gripper']):
+ continue
+
+ if 'table' in body_name.lower() or 'table' in geom_name.lower():
+ geom_type = model.geom_type[geom_id]
+ geom_size = model.geom_size[geom_id].copy()
+ geom_pos = data.geom_xpos[geom_id].copy()
+
+ is_top = 'top' in geom_name.lower() or (
+ geom_type == mujoco.mjtGeom.mjGEOM_BOX and
+ geom_size[2] < 0.03
+ )
+ is_leg = 'leg' in geom_name.lower() or (
+ geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER
+ )
+
+ if is_top:
+ mesh = trimesh.creation.box(extents=geom_size * 2)
+ transform = np.eye(4)
+ transform[:3, :3] = data.geom_xmat[geom_id].reshape(3, 3)
+ transform[:3, 3] = geom_pos
+ table_top_scene.add_geometry(mesh, node_name="table_top", transform=transform)
+ table_top_pos = geom_pos.tolist()
+ table_top_size = (geom_size * 2).tolist()
+ print(f" Found table top at {geom_pos}")
+
+ elif is_leg:
+ leg_positions.append(geom_pos.tolist())
+ print(f" Found leg at {geom_pos}")
+
+ if table_leg_mesh is None:
+ leg_radius = geom_size[0]
+ leg_height = geom_size[1] * 2
+ mesh = trimesh.creation.cylinder(radius=leg_radius, height=leg_height)
+ table_leg_mesh = mesh
+
+ # Calculate offset ratio
+ offset_ratio = 0.45
+ if leg_positions and len(leg_positions) >= 4 and table_top_size:
+ avg_x_offset = np.mean([abs(pos[0]) for pos in leg_positions])
+ avg_y_offset = np.mean([abs(pos[1]) for pos in leg_positions])
+
+ table_width = table_top_size[0] if table_top_size else 0.8
+ table_length = table_top_size[1] if table_top_size else 0.8
+
+ if table_width > 0 and table_length > 0:
+ x_ratio = avg_x_offset / (table_width / 2)
+ y_ratio = avg_y_offset / (table_length / 2)
+ offset_ratio = round((x_ratio + y_ratio) / 2, 3)
+
+ # Export components
+ output_dir = "utils/table_components"
+ os.makedirs(output_dir, exist_ok=True)
+
+ if len(table_top_scene.geometry) > 0:
+ top_path = os.path.join(output_dir, "table_top.glb")
+ table_top_scene.export(top_path)
+ print(f"✓ Exported table top to {top_path}")
+
+ if table_leg_mesh:
+ leg_scene = trimesh.Scene()
+ leg_scene.add_geometry(table_leg_mesh, node_name="table_leg")
+ leg_path = os.path.join(output_dir, "table_leg.glb")
+ leg_scene.export(leg_path)
+ print(f"✓ Exported table leg to {leg_path}")
+
+ # Save metadata
+ metadata = {
+ "table_top": {
+ "default_size": table_top_size if table_top_size else [0.8, 0.8, 0.05],
+ "position": table_top_pos if table_top_pos else [0, 0, 0.8],
+ "extracted": table_top_pos is not None
+ },
+ "table_leg": {
+ "radius": leg_radius,
+ "height": leg_height,
+ "offset_from_center": offset_ratio,
+ "actual_positions": leg_positions,
+ "num_legs_found": len(leg_positions)
+ },
+ "extraction_info": {
+ "table_width": table_top_size[0] if table_top_size else 0.8,
+ "table_length": table_top_size[1] if table_top_size else 0.8,
+ "table_thickness": table_top_size[2] if table_top_size else 0.05,
+ "total_height_with_legs": table_top_pos[2] + table_top_size[2]/2 if table_top_pos and table_top_size else 0.825
+ }
+ }
+
+ config_path = os.path.join(output_dir, "table_config.json")
+ with open(config_path, 'w') as f:
+ json.dump(metadata, f, indent=2)
+
+ print(f"✓ Saved configuration to {config_path}")
+
+ env.close()
+ return metadata
+
+
+if __name__ == "__main__":
+ extract_table_components()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/mjcf_to_mesh.py b/src/scenic/simulators/robosuite/utils/mjcf_to_mesh.py
new file mode 100644
index 000000000..ea141ab2a
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/mjcf_to_mesh.py
@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+"""
+Standalone MJCF to GLB converter utility.
+Useful for testing MJCF XML conversion without launching RoboSuite/MuJoCo.
+"""
+
+import xml.etree.ElementTree as ET
+import trimesh
+import numpy as np
+import argparse
+import sys
+from pathlib import Path
+
+
+def convert_mjcf(xml_content, output_file=None, base_path="."):
+ """
+ Convert MJCF XML string to GLB file.
+
+ Args:
+ xml_content: MJCF XML string
+ output_file: Output filename (auto-generated if None)
+ base_path: Base directory for resolving relative mesh paths
+
+ Returns:
+ Trimesh object
+ """
+ root = ET.fromstring(xml_content)
+
+ if output_file is None:
+ model_name = root.get('model', 'output')
+ output_file = f"{model_name}.glb"
+
+ mesh_geom = root.find('.//geom[@mesh]')
+
+ if mesh_geom is not None:
+ # Handle mesh file
+ mesh_name = mesh_geom.get('mesh')
+ mesh_elem = root.find(f'.//asset/mesh[@name="{mesh_name}"]')
+ if not mesh_elem:
+ raise ValueError(f"Mesh asset '{mesh_name}' not found")
+
+ mesh_file = mesh_elem.get('file')
+ scale = np.array([float(s) for s in mesh_elem.get('scale', '1 1 1').split()])
+
+ # Resolve path
+ mesh_path = Path(base_path) / mesh_file if not Path(mesh_file).is_absolute() else Path(mesh_file)
+
+ if not mesh_path.exists():
+ raise FileNotFoundError(f"Mesh file not found: {mesh_path}")
+
+ mesh = trimesh.load(str(mesh_path))
+ if isinstance(mesh, trimesh.Scene):
+ mesh = trimesh.util.concatenate(list(mesh.geometry.values()))
+
+ mesh.apply_scale(scale)
+
+ else:
+ # Handle primitives
+ geoms = root.findall('.//geom[@type]')
+ if not geoms:
+ raise ValueError("No mesh or primitive geom found")
+
+ meshes = []
+ for geom in geoms:
+ geom_type = geom.get('type')
+ size = np.array([float(s) for s in geom.get('size', '0.1 0.1 0.1').split()])
+
+ if geom_type == 'box':
+ geom_mesh = trimesh.creation.box(extents=size * 2)
+ elif geom_type == 'sphere':
+ geom_mesh = trimesh.creation.icosphere(radius=size[0], subdivisions=2)
+ elif geom_type == 'cylinder':
+ geom_mesh = trimesh.creation.cylinder(radius=size[0], height=size[1] * 2)
+ elif geom_type == 'capsule':
+ geom_mesh = trimesh.creation.capsule(radius=size[0], height=size[1] * 2)
+ else:
+ print(f"Warning: Unsupported geom type: {geom_type}")
+ continue
+
+ pos = np.array([float(p) for p in geom.get('pos', '0 0 0').split()])
+ if np.any(pos != 0):
+ geom_mesh.apply_translation(pos)
+
+ meshes.append(geom_mesh)
+
+ if not meshes:
+ raise ValueError("No valid geometry found")
+
+ mesh = trimesh.util.concatenate(meshes)
+
+ mesh.export(output_file)
+
+ # Print info
+ volume = mesh.volume if mesh.is_volume else 0
+ print(f"Exported: {output_file}")
+ print(f" Vertices: {len(mesh.vertices)}")
+ print(f" Faces: {len(mesh.faces)}")
+ print(f" Volume: {volume:.6f}")
+ print(f" Bounds: {mesh.bounds[1] - mesh.bounds[0]}")
+
+ return mesh
+
+
+def main():
+ parser = argparse.ArgumentParser(description='Convert MJCF XML to GLB mesh')
+ parser.add_argument('xml_file', nargs='?', help='MJCF XML file path')
+ parser.add_argument('-o', '--output', help='Output GLB file')
+ parser.add_argument('-b', '--base-path', default='.', help='Base path for mesh files')
+ parser.add_argument('--test', action='store_true', help='Run test examples')
+
+ args = parser.parse_args()
+
+ if args.test:
+ run_tests()
+ elif args.xml_file:
+ with open(args.xml_file, 'r') as f:
+ xml_content = f.read()
+
+ base_path = Path(args.xml_file).parent if not args.base_path else args.base_path
+ convert_mjcf(xml_content, args.output, base_path)
+ else:
+ parser.print_help()
+
+
+def run_tests():
+ """Run test conversions with example XML."""
+
+ # Test 1: Box primitive
+ box_xml = """
+
+
+
+
+
+
+
+
+
+ """
+
+ # Test 2: Multi-primitive (table)
+ table_xml = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+ """
+
+ # Test 3: Sphere
+ sphere_xml = """
+
+
+
+
+
+
+
+
+
+ """
+
+ print("Running tests...")
+ print("\n1. Box Test:")
+ convert_mjcf(box_xml, "test_box.glb")
+
+ print("\n2. Table Test:")
+ convert_mjcf(table_xml, "test_table.glb")
+
+ print("\n3. Sphere Test:")
+ convert_mjcf(sphere_xml, "test_sphere.glb")
+
+ print("\n✓ Tests complete!")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/IIWA.glb b/src/scenic/simulators/robosuite/utils/robot_meshes/IIWA.glb
new file mode 100644
index 000000000..1415088aa
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/robot_meshes/IIWA.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/Jaco.glb b/src/scenic/simulators/robosuite/utils/robot_meshes/Jaco.glb
new file mode 100644
index 000000000..70bb9a0af
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/robot_meshes/Jaco.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/Panda.glb b/src/scenic/simulators/robosuite/utils/robot_meshes/Panda.glb
new file mode 100644
index 000000000..bfa899bd4
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/robot_meshes/Panda.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/Sawyer.glb b/src/scenic/simulators/robosuite/utils/robot_meshes/Sawyer.glb
new file mode 100644
index 000000000..92970778d
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/robot_meshes/Sawyer.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/UR5e.glb b/src/scenic/simulators/robosuite/utils/robot_meshes/UR5e.glb
new file mode 100644
index 000000000..7b176487e
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/robot_meshes/UR5e.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/robot_meshes/dimensions.txt b/src/scenic/simulators/robosuite/utils/robot_meshes/dimensions.txt
new file mode 100644
index 000000000..15bce6ebb
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/robot_meshes/dimensions.txt
@@ -0,0 +1,5 @@
+Panda: [0.9938944579988174, 0.6858599684110671, 1.5993617270869454]
+UR5e: [0.8907478644453279, 0.6858599684110671, 1.5593836204265223]
+Sawyer: [1.0236498897739559, 0.6858599684110671, 1.6606441595802548]
+Jaco: [0.9489285662370466, 0.6858599684110671, 1.5862445649401822]
+IIWA: [1.0412771240030825, 0.6858599684110671, 1.6445948581652305]
diff --git a/src/scenic/simulators/robosuite/utils/table_components/table_config.json b/src/scenic/simulators/robosuite/utils/table_components/table_config.json
new file mode 100644
index 000000000..61863b352
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/table_components/table_config.json
@@ -0,0 +1,49 @@
+{
+ "table_top": {
+ "default_size": [
+ 0.8,
+ 0.8,
+ 0.05
+ ],
+ "position": [
+ 0.0,
+ 0.0,
+ 0.775
+ ],
+ "extracted": true
+ },
+ "table_leg": {
+ "radius": 0.025,
+ "height": 0.775,
+ "offset_from_center": 0.75,
+ "actual_positions": [
+ [
+ 0.30000000000000004,
+ 0.30000000000000004,
+ 0.3875
+ ],
+ [
+ -0.30000000000000004,
+ 0.30000000000000004,
+ 0.3875
+ ],
+ [
+ -0.30000000000000004,
+ -0.30000000000000004,
+ 0.3875
+ ],
+ [
+ 0.30000000000000004,
+ -0.30000000000000004,
+ 0.3875
+ ]
+ ],
+ "num_legs_found": 4
+ },
+ "extraction_info": {
+ "table_width": 0.8,
+ "table_length": 0.8,
+ "table_thickness": 0.05,
+ "total_height_with_legs": 0.8
+ }
+}
\ No newline at end of file
diff --git a/src/scenic/simulators/robosuite/utils/table_components/table_leg.glb b/src/scenic/simulators/robosuite/utils/table_components/table_leg.glb
new file mode 100644
index 000000000..6edbfd448
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/table_components/table_leg.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/table_components/table_top.glb b/src/scenic/simulators/robosuite/utils/table_components/table_top.glb
new file mode 100644
index 000000000..a59ca1ded
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/table_components/table_top.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/table_meshes/standard_table.glb b/src/scenic/simulators/robosuite/utils/table_meshes/standard_table.glb
new file mode 100644
index 000000000..38846d2b0
Binary files /dev/null and b/src/scenic/simulators/robosuite/utils/table_meshes/standard_table.glb differ
diff --git a/src/scenic/simulators/robosuite/utils/table_meshes/table_config.json b/src/scenic/simulators/robosuite/utils/table_meshes/table_config.json
new file mode 100644
index 000000000..a2a6080d0
--- /dev/null
+++ b/src/scenic/simulators/robosuite/utils/table_meshes/table_config.json
@@ -0,0 +1,26 @@
+{
+ "dimensions": [
+ 0.8,
+ 0.8,
+ 0.8
+ ],
+ "center": [
+ 0.0,
+ 0.0,
+ 0.4
+ ],
+ "bounds": [
+ [
+ -0.4,
+ -0.4,
+ 0.0
+ ],
+ [
+ 0.4,
+ 0.4,
+ 0.8
+ ]
+ ],
+ "table_height": 0.8,
+ "table_surface_height": 0.825
+}
\ No newline at end of file