From 8cfbfe4eef37d86a9d53c50c6e26d984caa2331e Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Mon, 28 Jul 2025 20:21:53 +0530 Subject: [PATCH 1/8] code_optimized --- .../basic_shapes.scenic | 29 ++ .../circular_formation.scenic | 26 ++ .../deifferent_shape_objects.scenic | 60 +++ .../grid_formation.scenic | 24 ++ .../height_levels.scenic | 23 ++ .../basic_object_placement/test_static.scenic | 7 + .../robosuite/lift_custom_position.scenic | 155 ++++++++ examples/robosuite/lift_task.scenic | 77 ++++ examples/robosuite/make_osc_test.scenic | 189 +++++++++ examples/robosuite/tower_collapse.scenic | 83 ++++ examples/robosuite/ur5e_lift_test.scenic | 159 ++++++++ examples/robosuite/xml_test.scenic | 20 + src/scenic/simulators/robosuite/__init__.py | 11 + src/scenic/simulators/robosuite/model.scenic | 208 ++++++++++ src/scenic/simulators/robosuite/simulator.py | 371 ++++++++++++++++++ src/scenic/simulators/robosuite/utils.py | 11 + .../simulators/robosuite/xml_builder.py | 115 ++++++ 17 files changed, 1568 insertions(+) create mode 100644 examples/robosuite/basic_object_placement/basic_shapes.scenic create mode 100644 examples/robosuite/basic_object_placement/circular_formation.scenic create mode 100644 examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic create mode 100644 examples/robosuite/basic_object_placement/grid_formation.scenic create mode 100644 examples/robosuite/basic_object_placement/height_levels.scenic create mode 100644 examples/robosuite/basic_object_placement/test_static.scenic create mode 100644 examples/robosuite/lift_custom_position.scenic create mode 100644 examples/robosuite/lift_task.scenic create mode 100644 examples/robosuite/make_osc_test.scenic create mode 100644 examples/robosuite/tower_collapse.scenic create mode 100644 examples/robosuite/ur5e_lift_test.scenic create mode 100644 examples/robosuite/xml_test.scenic create mode 100644 src/scenic/simulators/robosuite/__init__.py create mode 100644 src/scenic/simulators/robosuite/model.scenic create mode 100644 src/scenic/simulators/robosuite/simulator.py create mode 100644 src/scenic/simulators/robosuite/utils.py create mode 100644 src/scenic/simulators/robosuite/xml_builder.py diff --git a/examples/robosuite/basic_object_placement/basic_shapes.scenic b/examples/robosuite/basic_object_placement/basic_shapes.scenic new file mode 100644 index 000000000..833a36f62 --- /dev/null +++ b/examples/robosuite/basic_object_placement/basic_shapes.scenic @@ -0,0 +1,29 @@ +# test_suite_1_shapes.scenic +"""Test Suite 1: Different Shapes and Colors - FIXED.""" + +model scenic.simulators.robosuite.model + +class TestCube(Object): + width: 0.15 + length: 0.15 + height: 0.15 + # Remove shape property - use class name instead + +class TestBall(Object): + width: 0.15 + length: 0.15 + height: 0.15 + # Remove shape property - use class name instead + +class TestCylinder(Object): + width: 0.15 + length: 0.15 + height: 0.2 + # Remove shape property - use class name instead + +# Different shapes in a line +red_cube = new TestCube at (-0.4, 0.0, 0.1), with color (1, 0, 0) +green_ball = new TestBall at (0.0, 0.0, 0.1), with color (0, 1, 0) +blue_cylinder = new TestCylinder at (0.4, 0.0, 0.1), with color (0, 0, 1) + +ego = green_ball diff --git a/examples/robosuite/basic_object_placement/circular_formation.scenic b/examples/robosuite/basic_object_placement/circular_formation.scenic new file mode 100644 index 000000000..72271b295 --- /dev/null +++ b/examples/robosuite/basic_object_placement/circular_formation.scenic @@ -0,0 +1,26 @@ + +# test_suite_4_circle.scenic +"""Test Suite 4: Circular Formation.""" + +model scenic.simulators.robosuite.model + +class CircleCube(Object): + width: 0.08 + length: 0.08 + height: 0.08 + +# Arrange cubes in a circle +import math + +radius = 0.3 +cube_1 = new CircleCube at (radius * cos(0), radius * sin(0), 0.04), with color (1, 0, 0) +cube_2 = new CircleCube at (radius * cos(60 deg), radius * sin(60 deg), 0.04), with color (1, 0.5, 0) +cube_3 = new CircleCube at (radius * cos(120 deg), radius * sin(120 deg), 0.04), with color (1, 1, 0) +cube_4 = new CircleCube at (radius * cos(180 deg), radius * sin(180 deg), 0.04), with color (0, 1, 0) +cube_5 = new CircleCube at (radius * cos(240 deg), radius * sin(240 deg), 0.04), with color (0, 0, 1) +cube_6 = new CircleCube at (radius * cos(300 deg), radius * sin(300 deg), 0.04), with color (0.5, 0, 1) + +# Center cube +center_cube = new CircleCube at (0.0, 0.0, 0.04), with color (1, 1, 1) + +ego = center_cube diff --git a/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic b/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic new file mode 100644 index 000000000..2d3f3c723 --- /dev/null +++ b/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic @@ -0,0 +1,60 @@ +"""Advanced RoboSuite Test: Probabilistic Tower Collapse Scenario +Tests bidirectional data flow with physics-based constraints and temporal logic. +""" + +model scenic.simulators.robosuite.model + +# Probabilistic object types +class Tower(Object): + width: Range(0.08, 0.15) + length: Range(0.08, 0.15) + height: Range(0.2, 0.5) + color: Uniform((1, 0, 0), (0, 1, 0), (0, 0, 1)) + +class Ball(Object): + width: Range(0.06, 0.12) + length: self.width + height: self.width + color: (1, 1, 0) + +# Fixed towers with probabilistic properties +tower1 = new Tower at (Range(-0.3, -0.1), Range(-0.2, 0.2), Range(0.3, 0.8)) +tower2 = new Tower at (Range(-0.1, 0.1), Range(-0.2, 0.2), Range(0.3, 0.8)) +tower3 = new Tower at (Range(0.1, 0.3), Range(-0.2, 0.2), Range(0.3, 0.8)) + +# Ensure separation +require (distance from tower1 to tower2) > 0.15 +require (distance from tower2 to tower3) > 0.15 +require (distance from tower1 to tower3) > 0.15 + +# Projectile ball +projectile = new Ball at (Range(-0.4, -0.3), 0, Range(0.5, 1.0)) + +# Target selection - probabilistic +target_tower = Uniform(tower1, tower2, tower3) +ego = tower1 # Fixed ego object + +# Physics-based requirements using bidirectional data flow +require always projectile.position.z > -0.1 +require eventually (distance from projectile to target_tower) < 0.15 + +# Temporal monitoring +require always (target_tower.position.z > 0.05) until ( + (distance from projectile to target_tower) < 0.2 +) + +# Record bidirectional data +record projectile.position as "ball_trajectory" +record target_tower.position as "tower_position" +record projectile.velocity as "ball_velocity" +record tower1.position.z as "tower1_height" +record tower2.position.z as "tower2_height" +record tower3.position.z as "tower3_height" + +# Stability requirements +require always tower1.position.z > (tower1.height * 0.3) +require always tower2.position.z > (tower2.height * 0.3) +require always tower3.position.z > (tower3.height * 0.3) + +# Hard requirement - temporal operators can't be probabilistic +require eventually projectile.position.z < 0.1 \ No newline at end of file diff --git a/examples/robosuite/basic_object_placement/grid_formation.scenic b/examples/robosuite/basic_object_placement/grid_formation.scenic new file mode 100644 index 000000000..4a53cb39b --- /dev/null +++ b/examples/robosuite/basic_object_placement/grid_formation.scenic @@ -0,0 +1,24 @@ +# test_suite_2_grid.scenic +"""Test Suite 2: Grid Formation.""" + +model scenic.simulators.robosuite.model + +class GridCube(Object): + width: 0.1 + length: 0.1 + height: 0.1 + +# 3x3 grid of cubes with different colors +cube_1_1 = new GridCube at (-0.2, -0.2, 0.05), with color (1, 0, 0) +cube_1_2 = new GridCube at (-0.2, 0.0, 0.05), with color (1, 0.5, 0) +cube_1_3 = new GridCube at (-0.2, 0.2, 0.05), with color (1, 1, 0) + +cube_2_1 = new GridCube at (0.0, -0.2, 0.05), with color (0, 1, 0) +cube_2_2 = new GridCube at (0.0, 0.0, 0.05), with color (0, 1, 1) +cube_2_3 = new GridCube at (0.0, 0.2, 0.05), with color (0, 0, 1) + +cube_3_1 = new GridCube at (0.2, -0.2, 0.05), with color (0.5, 0, 1) +cube_3_2 = new GridCube at (0.2, 0.0, 0.05), with color (1, 0, 1) +cube_3_3 = new GridCube at (0.2, 0.2, 0.05), with color (0.5, 0.5, 0.5) + +ego = cube_2_2 diff --git a/examples/robosuite/basic_object_placement/height_levels.scenic b/examples/robosuite/basic_object_placement/height_levels.scenic new file mode 100644 index 000000000..54ea89210 --- /dev/null +++ b/examples/robosuite/basic_object_placement/height_levels.scenic @@ -0,0 +1,23 @@ +# +"""Test Suite 3: Random Height Levels with Sampling.""" + +model scenic.simulators.robosuite.model + +class HeightCube(Object): + width: 0.12 + length: 0.12 + height: 0.12 + +# Random cube positions and heights +ground_cube = new HeightCube at (Range(-0.4, -0.2), Range(-0.2, 0.2), Range(0.05, 0.1)), with color (0, 0, 0) +low_cube = new HeightCube at (Range(-0.2, 0.0), Range(-0.2, 0.2), Range(0.15, 0.25)), with color (1, 0, 0) +mid_cube = new HeightCube at (Range(0.0, 0.2), Range(-0.2, 0.2), Range(0.3, 0.5)), with color (0, 1, 0) +high_cube = new HeightCube at (Range(0.2, 0.4), Range(-0.2, 0.2), Range(0.5, 0.7)), with color (0, 0, 1) + +# Ensure minimum separation between cubes +require (distance from ground_cube to low_cube) > 0.1 +require (distance from low_cube to mid_cube) > 0.1 +require (distance from mid_cube to high_cube) > 0.1 + +ego = mid_cube +record ego.position as egoPos \ No newline at end of file diff --git a/examples/robosuite/basic_object_placement/test_static.scenic b/examples/robosuite/basic_object_placement/test_static.scenic new file mode 100644 index 000000000..80075f096 --- /dev/null +++ b/examples/robosuite/basic_object_placement/test_static.scenic @@ -0,0 +1,7 @@ +# test_static.scenic +model scenic.simulators.robosuite.model + +# Create objects at known positions +new Cube at (0, 0, 0.025), with color (1, 0, 0) # Red cube on the ground +new Ball at (0.5, 0, 0.025), with color (0, 1, 0) # Green ball on the ground +new Cylinder at (-0.5, 0, 0.05), with color (0, 0, 1) # Blue cylinder on the ground \ No newline at end of file diff --git a/examples/robosuite/lift_custom_position.scenic b/examples/robosuite/lift_custom_position.scenic new file mode 100644 index 000000000..c95dd97bc --- /dev/null +++ b/examples/robosuite/lift_custom_position.scenic @@ -0,0 +1,155 @@ +# examples/robosuite/lift_custom_position.scenic +"""Lift task with custom cube position - demonstrates object positioning.""" + +param use_environment = "Lift" +model scenic.simulators.robosuite.model + +# Define cube class for Lift environment +class LiftCube(RoboSuiteObject): + """Cube in Lift environment.""" + _env_object_name: "cube" # Maps to RoboSuite's internal cube + width: 0.05 + length: 0.05 + height: 0.05 + color: (1, 0, 0) # Red + +# Create cube at custom position +# Default Lift places cube at (0, 0, ~0.82) +# Let's place it to the right and forward +cube = new LiftCube at (0.1, -0.1, 0.82) + +# Robot with modified lift behavior for new position +behavior CustomPositionLiftBehavior(): + print("=== LIFT WITH CUSTOM CUBE POSITION ===") + + # Wait for stabilization + wait + wait + + sim = simulation() + + # Print initial cube position + obs = sim._current_obs + if obs and 'cube_pos' in obs: + print(f"Initial cube position: {obs['cube_pos']}") + + # Phase 1: Open gripper + print("\nPhase 1: Opening gripper...") + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) + + # Phase 2: Move above cube + print("\nPhase 2: Moving above cube...") + for step in range(60): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Target is 10cm above cube + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.1 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if step % 10 == 0: + print(f" Step {step}: Moving to cube at ({cube_pos[0]:.3f}, {cube_pos[1]:.3f}, {cube_pos[2]:.3f})") + + if norm < 0.02: + print(f" Reached above position at step {step}") + break + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 3: Move down to cube + print("\nPhase 3: Moving to grasp position...") + for step in range(40): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Target is at cube center + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] - 0.02 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if norm < 0.01: + print(f" Reached grasp position at step {step}") + break + + delta = [max(-0.2, min(0.2, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 4: Close gripper + print("\nPhase 4: Closing gripper...") + for i in range(30): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Phase 5: Lift + print("\nPhase 5: Lifting...") + for step in range(60): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs: + wait + continue + + cube_height = obs['cube_pos'][2] + + if step % 10 == 0: + print(f" Step {step}: Cube height = {cube_height:.3f}") + + # Lift up + take OSCPositionAction(position_delta=[0, 0, 0.3], gripper=1) + + if cube_height > 0.9: # Success threshold + print(f" Lifted successfully at step {step}!") + break + + # Phase 6: Hold + print("\nPhase 6: Holding position...") + for i in range(50): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Check success + success = sim.checkSuccess() + print(f"\nTask Success: {success}") + + print("\nBehavior complete, continuing...") + while True: + wait + +ego = new PandaRobot with behavior CustomPositionLiftBehavior() + +# Monitor to show cube position +monitor CubePositionMonitor(): + print("\n=== Cube Position Monitor ===") + sim = simulation() + step = 0 + + while True: + obs = sim._current_obs + if obs and 'cube_pos' in obs: + if step % 20 == 0: + pos = obs['cube_pos'] + print(f"[Step {step:3d}] Cube at: ({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})") + step += 1 + wait + +require monitor CubePositionMonitor() + +terminate after 80 seconds \ No newline at end of file diff --git a/examples/robosuite/lift_task.scenic b/examples/robosuite/lift_task.scenic new file mode 100644 index 000000000..9b2fe7981 --- /dev/null +++ b/examples/robosuite/lift_task.scenic @@ -0,0 +1,77 @@ +"""Robosuite Lift task demonstration in Scenic.""" + +model scenic.simulators.robosuite.model + +arena = new Table at (0, 0, 0) + +# arena = new PegsArena + +# table = new PositionableTable at (0, 0, 0), +# facing 0 deg, +# with width 2, +# with length 1, +# with height 0.8 + +target_cube = new Cube at (0.5, -0.3, 0.825), + with color (0, 0, 1) + +# Create robot positioned next to table, facing it +ego = new PandaRobot at (0, -0.8, 0), + facing 90 deg # Face positive Y direction (toward table) + +# Create object on table surface (default table top is at z=0.8) +# target_cube = new Cube at (0, 0, 0.825), +# with width 0.05, +# with length 0.05, +# with height 0.05, +# with color (1, 0, 0) # Red + +# Define lift behavior +behavior LiftBehavior(): + # Move to pre-grasp position + take SetJointPositions([0, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.04, 0.04]) + for i in range(20): + wait + + # Open gripper + take SetJointPositions([0, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.08, 0.08]) + for i in range(10): + wait + + # Move down to object + take SetJointPositions([0.2, -0.3, 0, -1.8, 0, 1.4, 0.785, 0.08, 0.08]) + for i in range(20): + wait + + # Close gripper + take SetJointPositions([0.2, -0.3, 0, -1.8, 0, 1.4, 0.785, 0.01, 0.01]) + for i in range(10): + wait + + # Lift up + take SetJointPositions([0.2, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.01, 0.01]) + for i in range(20): + wait + +# Assign behavior to ego robot +ego.behavior = LiftBehavior() + +# Monitor to track progress +monitor LiftMonitor(): + for i in range(30): + wait + joint_pos = ego.joint_positions + if joint_pos: + print(f"Step {i}: Gripper: {joint_pos[-2]:.3f}, {joint_pos[-1]:.3f}") + print(f"Step {i}: Cube at z={target_cube.position.z:.3f}") + +# Record data for visualization +record ego.joint_positions as robot_joints +record ego.position as robot_position +record target_cube.position as cube_position + +# Simulation parameters +param timestep = 0.1 +terminate after 30 steps + +require monitor LiftMonitor() \ No newline at end of file diff --git a/examples/robosuite/make_osc_test.scenic b/examples/robosuite/make_osc_test.scenic new file mode 100644 index 000000000..0dc52972b --- /dev/null +++ b/examples/robosuite/make_osc_test.scenic @@ -0,0 +1,189 @@ +# examples/robosuite/make_osc_test.scenic +"""Test RoboSuite's make() function with OSC controller for Lift task.""" + + +model scenic.simulators.robosuite.model + +simulator RobosuiteSimulator( + render=True, + real_time=True, + speed=1.0, + use_environment="Lift" +) + +# cube = new Cube at (0.1, -0.1, 0.82) + +behavior WorkingLiftBehavior(): + print("[LIFT] Starting lift behavior") + + + wait + wait + + sim = simulation() + + + print("\n[LIFT] Phase 1: Opening gripper...") + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) + print("[LIFT] Gripper opened") + + + print("\n[LIFT] Phase 2: Moving above cube...") + moved_above = False + for step in range(100): + obs = sim._current_obs + if not obs: + print(f"[LIFT] Step {step}: No observation!") + wait + continue + + if 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + print(f"[LIFT] Step {step}: Missing data!") + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.1 - eef_pos[2] + ] + + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if step % 10 == 0: + print(f"[LIFT] Step {step}: Error = {norm:.3f}, EEF = {eef_pos}, Cube = {cube_pos}") + + if norm < 0.02: + print(f"[LIFT] Reached above position at step {step}") + moved_above = True + break + + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + if not moved_above: + print("[LIFT] Failed to reach above position!") + + + print("\n[LIFT] Phase 3: Moving to grasp position...") + moved_down = False + for step in range(80): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] - 0.02 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if step % 10 == 0: + print(f"[LIFT] Step {step}: Error = {norm:.3f}") + + if norm < 0.01: + print(f"[LIFT] Reached grasp position at step {step}") + moved_down = True + break + + delta = [max(-0.2, min(0.2, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + if not moved_down: + print("[LIFT] Failed to reach grasp position!") + + + print("\n[LIFT] Phase 4: Closing gripper...") + for i in range(30): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + print("[LIFT] Gripper closed") + + + print("\n[LIFT] Phase 5: Lifting...") + lifted = False + initial_cube_height = None + for step in range(100): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + + if initial_cube_height is None: + initial_cube_height = cube_pos[2] + print(f"[LIFT] Initial cube height: {initial_cube_height:.3f}") + + + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.15 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if step % 10 == 0: + cube_lift = cube_pos[2] - initial_cube_height + print(f"[LIFT] Step {step}: Error = {norm:.3f}, Cube lifted by {cube_lift:.3f}m") + + if norm < 0.02: + print(f"[LIFT] Reached lift position at step {step}") + lifted = True + break + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=1) + + if not lifted: + print("[LIFT] Failed to reach lift position!") + + + print("\n[LIFT] Phase 6: Holding position...") + for i in range(100): + if i % 20 == 0: + obs = sim._current_obs + if obs and 'cube_pos' in obs and initial_cube_height is not None: + cube_lift = obs['cube_pos'][2] - initial_cube_height + print(f"[LIFT] Holding step {i}/100, cube height delta: {cube_lift:.3f}m") + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + + obs = sim._current_obs + if obs and 'cube_pos' in obs and initial_cube_height is not None: + final_cube_height = obs['cube_pos'][2] + total_lift = final_cube_height - initial_cube_height + success = total_lift > 0.05 # Success if lifted more than 5cm + print(f"\n[LIFT] Final cube lift: {total_lift:.3f}m") + print(f"[LIFT] Task Success: {success}") + + print(f"\n[LIFT] RoboSuite Success Check: {sim.checkSuccess()}") + print("[LIFT] Behavior complete, continuing...") + + + step_count = 0 + while True: + wait + step_count += 1 + if step_count % 100 == 0: + print(f"[LIFT] Still running... (step {step_count})") + +ego = new PandaRobot with behavior WorkingLiftBehavior() + +terminate after 150 seconds \ No newline at end of file diff --git a/examples/robosuite/tower_collapse.scenic b/examples/robosuite/tower_collapse.scenic new file mode 100644 index 000000000..e04864390 --- /dev/null +++ b/examples/robosuite/tower_collapse.scenic @@ -0,0 +1,83 @@ +# param use_table = False +# table = new Table at (20, 20, 20) + + +# examples/robosuite/tower_collapse.scenic +"""Tower Collapse Simulation - Fixed Version +Stops when tower falls (3+ cylinders down) or stabilizes. +""" + +model scenic.simulators.robosuite.model + +class Cylinder(Object): + width: 0.08 + length: 0.08 + height: 0.12 + color: Uniform((1,0,0), (0,1,0), (0,0,1), (1,1,0), (1,0,1)) + +# Build tower with random offsets for instability +cylinder1 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.10) +cylinder2 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.221) +cylinder3 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.342) +cylinder4 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.463) +cylinder5 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.584) +cylinder6 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.705) + +tower_cylinders = [cylinder1, cylinder2, cylinder3, cylinder4, cylinder5, cylinder6] + +# Set ego to bottom cylinder +ego = cylinder1 + +# Records +record [c.position[0] for c in tower_cylinders] as cylinder_x_positions +record [c.position[2] for c in tower_cylinders] as cylinder_heights +record max([abs(c.position[0]) for c in tower_cylinders]) as tower_sway +record len([c for c in tower_cylinders if c.position[2] < 0.05]) as fallen_count + +# Monitor for collapse and stability +monitor TowerMonitor(): + print(f"=== Tower Collapse Monitor Started ===") + step_count = 0 + stable_steps = 0 + prev_positions = None + + while True: + fallen = len([c for c in tower_cylinders if c.position[2] < 0.05]) + sway = max([abs(c.position[0]) for c in tower_cylinders]) + + # Check stability - if positions haven't changed much + current_positions = [(c.position[0], c.position[2]) for c in tower_cylinders] + if prev_positions: + max_change = max([abs(curr[0]-prev[0]) + abs(curr[1]-prev[1]) + for curr, prev in zip(current_positions, prev_positions)]) + if max_change < 0.001: # Very little movement + stable_steps += 1 + else: + stable_steps = 0 + prev_positions = current_positions + + # Print status every 10 steps + if step_count % 10 == 0: + print(f"[Step {step_count:3d}] Fallen={fallen}/6, Sway={sway:.3f}, Stable={stable_steps}") + if step_count % 30 == 0: # Detailed print every 30 steps + for i, c in enumerate(tower_cylinders): + print(f" Cylinder{i+1}: x={c.position[0]:.3f}, z={c.position[2]:.3f}") + + # Terminate on collapse (3+ fallen) + if fallen >= 3: + print(f"\n!!! TOWER COLLAPSED at step {step_count}: {fallen}/6 cylinders down !!!") + terminate + + # Terminate if stable for 20 steps + if stable_steps >= 20: + print(f"\n=== Tower STABILIZED at step {step_count} ===") + terminate + + step_count += 1 + wait + +require monitor TowerMonitor() + +# Termination conditions +terminate when len([c for c in tower_cylinders if c.position[2] < 0.05]) >= 3 +# terminate after 50 steps # Max simulation length \ No newline at end of file diff --git a/examples/robosuite/ur5e_lift_test.scenic b/examples/robosuite/ur5e_lift_test.scenic new file mode 100644 index 000000000..6542d4334 --- /dev/null +++ b/examples/robosuite/ur5e_lift_test.scenic @@ -0,0 +1,159 @@ +# examples/robosuite/ur5e_lift_test.scenic +"""Test UR5e robot cube pickup with optimized parameters.""" + +param use_environment = "Lift" + +model scenic.simulators.robosuite.model + +simulator RobosuiteSimulator( + render=True, + real_time=True, + speed=1.0, + use_environment=globalParameters.use_environment +) + +# Define UR5e robot class +class UR5eRobot(Robot): + """UR5e robot configuration.""" + robot_type: "UR5e" + initial_qpos: None # Use default + +behavior UR5eLiftBehavior(): + """Optimized lift behavior for UR5e robot.""" + + print("=== UR5e OPTIMIZED CUBE PICKUP ===") + + # Wait for stabilization + wait + wait + + sim = simulation() + + # Phase 1: Open gripper (20 steps) + print("\nOpening gripper...") + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) + + # Phase 2: Move above cube (40 steps max) + print("\n1. Moving above cube...") + for step in range(40): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Target is 10cm above cube + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.1 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if norm < 0.02: + print(f" Reached above position at step {step}") + break + + # Scale and clip + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 3: Move down to cube (30 steps max) + print("2. Moving to grasp position...") + for step in range(30): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Target is 2.5cm below cube center + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] - 0.025 - eef_pos[2] + ] + + norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 + + if norm < 0.01: + print(f" Reached grasp position at step {step}") + break + + # Slower approach + delta = [max(-0.1, min(0.1, e * 2.0)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 4: Grasp (20 steps) + print("3. Closing gripper...") + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Phase 5: Lift (50 steps max) + print("4. Lifting...") + initial_cube_height = None + success_achieved = False + + for step in range(50): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait + continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Track initial height + if initial_cube_height is None: + initial_cube_height = cube_pos[2] + print(f" Initial cube height: {initial_cube_height:.3f}") + + # Target is 25cm above original position + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.25 - eef_pos[2] + ] + + delta = [max(-0.25, min(0.25, e * 2.5)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=1) + + # Check success + if sim.checkSuccess() and step > 10 and not success_achieved: + print(f" Success achieved at lift step {step}!") + success_achieved = True + + # Phase 6: Hold position (30 steps) + print("5. Holding position...") + for i in range(30): + if i % 10 == 0: + obs = sim._current_obs + if obs and 'cube_pos' in obs: + print(f" Cube height: {obs['cube_pos'][2]:.3f} (needs > 0.84)") + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Final check + obs = sim._current_obs + if obs and 'cube_pos' in obs: + print(f"\nFinal Success: {sim.checkSuccess()}") + print(f"Cube height: {obs['cube_pos'][2]:.3f} (needs > 0.84)") + + print("\nBehavior complete, continuing...") + + # Keep running + while True: + try: + wait + except: + print("\nEpisode terminated. Exiting...") + break + +ego = new UR5eRobot with behavior UR5eLiftBehavior() + +terminate after 100 seconds # Longer time for UR5e \ No newline at end of file diff --git a/examples/robosuite/xml_test.scenic b/examples/robosuite/xml_test.scenic new file mode 100644 index 000000000..dc6471e88 --- /dev/null +++ b/examples/robosuite/xml_test.scenic @@ -0,0 +1,20 @@ +# examples/robosuite/xml_test.scenic +model scenic.simulators.robosuite.model + +# Place table at ground level (z=0) + +table2 = new PositionableTable at (0.5, 0, 0), + with width 1, + with length 0.6, + with height 0.6 + +# Cube on table - table height is 0.8, so cube should be at 0.8 + half cube height + +cube1 = new Cube at (-0.5, 0, 0.825), + with color (1, 0, 0) + +cube2 = new Cube at (0.5, 0, 0.625), + with color (0, 0, 1) + +ego = cube1 +terminate after 50 steps \ 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..0f82929f6 --- /dev/null +++ b/src/scenic/simulators/robosuite/__init__.py @@ -0,0 +1,11 @@ +"""Scenic world model and simulator interface for RoboSuite. + +This package provides: +- RobosuiteSimulator: Main simulator interface for Scenic +- World model definitions in model.scenic +- XML building utilities for custom objects +""" + +from .simulator import RobosuiteSimulator + +__all__ = ["RobosuiteSimulator"] \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic new file mode 100644 index 000000000..d009aa490 --- /dev/null +++ b/src/scenic/simulators/robosuite/model.scenic @@ -0,0 +1,208 @@ +# src/scenic/simulators/robosuite/model.scenic + +"""Scenic world model for RoboSuite simulator.""" + +from .simulator import RobosuiteSimulator + +# Simulator +simulator RobosuiteSimulator(render=True, real_time=False, speed=1.0) + +# Global parameters +param use_environment = None +param env_config = {} +param controller_config = None + +# Base class +class RoboSuiteObject(Object): + 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) + +# Objects +class XMLObject(RoboSuiteObject): + """Object from XML file or string.""" + xml_path: None + xml_string: None + material: {} + +class Cube(RoboSuiteObject): + width: 0.05 + length: 0.05 + height: 0.05 + color: (0.8, 0.2, 0.2) + +class Ball(RoboSuiteObject): + width: 0.05 + color: (0.2, 0.8, 0.2) + +class Cylinder(RoboSuiteObject): + width: 0.05 + height: 0.1 + color: (0.2, 0.2, 0.8) + +# Robots +class Robot(RoboSuiteObject): + robot_type: "Panda" + gripper_type: "default" + controller_config: None + initial_qpos: None + base_type: "default" + width: 0.2 + length: 0.2 + height: 0.5 + + joint_positions[dynamic]: [] + eef_pos[dynamic]: [0, 0, 0] + gripper_state[dynamic]: [0, 0] + +class PandaRobot(Robot): + robot_type: "Panda" + gripper_type: "PandaGripper" + initial_qpos: [0, -0.785, 0, -2.356, 0, 1.571, 0.785, 0.04, 0.04] + height: 0.9 + +class SawyerRobot(Robot): + robot_type: "Sawyer" + gripper_type: "RethinkGripper" + initial_qpos: [0, -0.785, 0, 1.571, 0, -0.785, 0] + height: 1.0 + +# Arenas +class Arena(RoboSuiteObject): + pass + +class EmptyArena(Arena): + pass + +class TableArena(Arena): + table_height: 0.8 + table_width: 1.0 + table_length: 0.8 + +class BinsArena(Arena): + bin_size: 0.3 + bin_height: 0.1 + +class PegsArena(Arena): + board_size: 0.3 + peg_radius: 0.015 + +class WipeArena(Arena): + table_height: 0.8 + marker_size: 0.08 + +class CustomArena(Arena): + xml_string: None + xml_path: None + objects: [] + +# Furniture +class Table(RoboSuiteObject): + width: 1.0 + length: 0.8 + height: 0.8 + +class PositionableTable(RoboSuiteObject): + width: 1.0 + length: 0.8 + height: 0.8 + +# Actions +import scenic.core.dynamics as dynamics + +class SetJointPositions(dynamics.Action): + """Set robot joint positions.""" + def __init__(self, positions): + self.positions = positions + + def applyTo(self, agent, sim): + pass + +class OSCPositionAction(dynamics.Action): + """Operational Space Control for end-effector.""" + def __init__(self, position_delta=None, orientation_delta=None, gripper=None): + self.position_delta = position_delta + self.orientation_delta = orientation_delta + self.gripper = gripper + + def applyTo(self, agent, sim): + pass + +# Behaviors +behavior RobustLiftBehavior(): + """Pick and place behavior using visual servoing.""" + wait; wait # Initialize + + sim = simulation() + + # Open gripper + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) + + # Move above object + for step in range(100): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + target = [cube_pos[0], cube_pos[1], cube_pos[2] + 0.1] + error = [target[i] - eef_pos[i] for i in range(3)] + + if sum(e**2 for e in error)**0.5 < 0.02: + break + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Move down + for step in range(80): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + target = [cube_pos[0], cube_pos[1], cube_pos[2] - 0.02] + error = [target[i] - eef_pos[i] for i in range(3)] + + if sum(e**2 for e in error)**0.5 < 0.01: + break + + delta = [max(-0.2, min(0.2, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Close gripper + for i in range(30): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Lift + for step in range(100): + obs = sim._current_obs + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + target = [cube_pos[0], cube_pos[1], cube_pos[2] + 0.15] + error = [target[i] - eef_pos[i] for i in range(3)] + + if sum(e**2 for e in error)**0.5 < 0.02: + break + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=1) + + # Hold + for i in range(100): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + success = sim.checkSuccess() + + while True: + wait + +behavior SimpleLiftBehavior(): + do RobustLiftBehavior() \ 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..d031bba18 --- /dev/null +++ b/src/scenic/simulators/robosuite/simulator.py @@ -0,0 +1,371 @@ +# src/scenic/simulators/robosuite/simulator.py +"""RoboSuite Simulator for Scenic - Version 3.0 (Reduced)""" + +import numpy as np +import mujoco +import time +import math +from typing import Dict, List, Any + +try: + import robosuite as suite + from robosuite.models import MujocoWorldBase + from robosuite.models.arenas import EmptyArena, TableArena + from robosuite.models.objects import BoxObject, BallObject, CylinderObject + from robosuite.robots import ROBOT_CLASS_MAPPING +except ImportError as e: + suite = None + _import_error = e # why not throw error early (s) + +from scenic.core.simulators import Simulation, Simulator +from scenic.core.vectors import Vector +from .utils import scenic_to_rgba # why only one module from .utils, that means other are redundent (s) + + +class RobosuiteSimulator(Simulator): + def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, + env_config=None, controller_config=None): + super().__init__() + if suite is None: + raise RuntimeError(f"Unable to import RoboSuite: {_import_error}") + self.render, self.real_time, self.speed = render, real_time, speed + self.use_environment = use_environment + self.env_config = env_config or {} + self.controller_config = controller_config + + def createSimulation(self, scene, **kwargs): + return RobosuiteSimulation(scene, self.render, self.real_time, self.speed, + self.use_environment, self.env_config, self.controller_config, **kwargs) + + +class RobosuiteSimulation(Simulation): + ENV_OBJECTS = {"Lift": {"cube": "cube_main"}, "Stack": {"cubeA": "cubeA_main", "cubeB": "cubeB_main"}} + + def __init__(self, scene, render, real_time, speed, use_environment=None, + env_config=None, controller_config=None, **kwargs): + self.render, self.real_time, self.speed = render, real_time, speed + self.use_environment, self.env_config = use_environment, env_config or {} + self.controller_config = controller_config + self.robosuite_env = self.world = self.model = self.data = self.viewer = None + self._body_id_map, self._prev_positions, self._robots = {}, {}, [] + self._current_obs, self._pending_actions = None, {} + self.timestep = kwargs.get('timestep') or 0.01 + self.physics_timestep = 0.002 + self.physics_steps = int(self.timestep / self.physics_timestep) + self.agents = [] + super().__init__(scene, **kwargs) + + def setup(self): + if self.use_environment: + # Pre-configured environment + robots = [obj for obj in self.scene.objects if hasattr(obj, 'robot_type')] + config = {**self.env_config, 'has_renderer': self.render, 'render_camera': "frontview", + 'camera_names': ["frontview", "robot0_eye_in_hand"], 'controller_configs': self.controller_config} + if robots: + config['robots'] = [r.robot_type for r in robots] if len(robots) > 1 else robots[0].robot_type + + self.robosuite_env = suite.make(self.use_environment, **config) + self._current_obs = self.robosuite_env.reset() + self.model = self.robosuite_env.sim.model._model + self.data = self.robosuite_env.sim.data._data + + for i, r in enumerate(robots[:len(self.robosuite_env.robots)]): + r._robosuite_robot = self.robosuite_env.robots[i] + r._robot_model = r._robosuite_robot.robot_model + self._robots.append(r) + + # Modify env objects + for obj in self.scene.objects: + if hasattr(obj, '_env_object_name') and hasattr(obj, 'position'): + env_obj = getattr(self.robosuite_env, obj._env_object_name, None) + if env_obj and hasattr(env_obj, 'get_obj'): + body = env_obj.get_obj().find(f".//body[@name='{obj._env_object_name}_main']") + if body is not None: + body.set('pos', f"{obj.position.x} {obj.position.y} {obj.position.z}") + + # Add custom objects + world = self.robosuite_env.model + for obj in self.scene.objects: + if not any([hasattr(obj, attr) for attr in ['robot_type', '_env_object_name']]) and \ + type(obj).__name__ not in ['Arena', 'EmptyArena', 'TableArena']: + self._add_to_env(world, obj) + + self._current_obs = self.robosuite_env.reset() + else: + # Custom world + self.world = MujocoWorldBase() + from .xml_builder import RoboSuiteXMLBuilder + self.xml_builder = RoboSuiteXMLBuilder() + + arena = TableArena() if any(type(o).__name__ == 'Table' for o in self.scene.objects) else EmptyArena() + self.world.merge(arena) + super().setup() + + self.model = self.world.get_model(mode="mujoco") + self.data = mujoco.MjData(self.model) + self.model.opt.gravity[2] = -9.81 + self.model.opt.timestep = self.physics_timestep + + if self.render: + try: self.viewer = mujoco.viewer.launch_passive(self.model, self.data) + except: self.viewer = None + + # Setup body mapping + if self.use_environment in self.ENV_OBJECTS: + for name, body in self.ENV_OBJECTS[self.use_environment].items(): + id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body) + if id != -1: self._body_id_map[name] = id + + for obj in self.objects: + self._map_body(obj) + + # Initialize robots + for r in self._robots: + if hasattr(r, '_robot_model') and hasattr(r, '_initial_qpos'): + for joint, qpos in zip(r._robot_model.joints, r._initial_qpos): + jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint) + if jid != -1: self.data.qpos[self.model.jnt_qposadr[jid]] = qpos + + if not self.robosuite_env: + mujoco.mj_forward(self.model, self.data) + + # Configure dynamics + for obj in self.scene.objects: + if hasattr(obj, 'robot_type'): + obj._dynamicProperties.update({'joint_positions': True, 'eef_pos': True, 'gripper_state': True}) + if hasattr(obj, 'behavior') and obj.behavior: + self.agents.append(obj) + else: + obj._dynamicProperties['position'] = True + + def _map_body(self, obj): + id = -1 + if hasattr(obj, '_robosuite_name'): + for suffix in ["_main", "", "_body"]: + id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, obj._robosuite_name + suffix) + if id != -1: + self._body_id_map[obj._robosuite_name] = id + break + elif hasattr(obj, '_robot_model') or hasattr(obj, '_robosuite_robot'): + name = (obj._robosuite_robot.robot_model.naming_prefix if hasattr(obj, '_robosuite_robot') + else obj._robot_model.naming_prefix) + id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name + "base_link") + if id != -1: self._body_id_map[name] = id + + if id != -1: + key = obj._robosuite_name if hasattr(obj, '_robosuite_name') else name + self._prev_positions[key] = self.data.xpos[id].copy() + + def _add_to_env(self, world, obj): + cls = type(obj).__name__.lower() + name = f"custom_{id(obj)}" + + # Create object + sizes = {'ball': [obj.width/2], 'cylinder': [obj.width/2, obj.height/2]} + size = sizes.get(cls.split('_')[0], [obj.width/2, obj.length/2, obj.height/2]) + + ObjClass = {'ball': BallObject, 'cylinder': CylinderObject}.get(cls.split('_')[0], BoxObject) + rs_obj = ObjClass(name, size=size, density=getattr(obj, 'density', 1000), + rgba=scenic_to_rgba(getattr(obj, 'color', (0.5, 0.5, 0.5)))) + + # Position + z = obj.height/2 if hasattr(obj, 'height') else 0 + body = rs_obj.get_obj().find(f".//body[@name='{name}_main']") or rs_obj.get_obj().find(f".//body[@name='{name}']") + if body is not None: + body.set('pos', f'{obj.position.x} {obj.position.y} {obj.position.z + z}') + + world.merge(rs_obj) + obj._robosuite_name = name + + def createObjectInSimulator(self, obj): + if self.robosuite_env: return + + obj_type = type(obj).__name__ + if obj_type in ['Arena', 'EmptyArena', 'TableArena', 'BinsArena', 'PegsArena', 'WipeArena', 'CustomArena', 'Table']: + return + + if hasattr(obj, 'robot_type'): + robot_class = ROBOT_CLASS_MAPPING.get(obj.robot_type) + if not robot_class: raise ValueError(f"Unknown robot type: {obj.robot_type}") + + robot = robot_class(robot_type=obj.robot_type, idn=len(self._robots), + initial_qpos=getattr(obj, 'initial_qpos', None), control_freq=20) + robot.load_model() + robot.robot_model.set_base_xpos([obj.position.x, obj.position.y, obj.position.z]) + if hasattr(obj, 'yaw'): robot.robot_model.set_base_ori([0, 0, obj.yaw]) + + self.world.merge(robot.robot_model) + obj._robot = robot + obj._robot_model = robot.robot_model + obj._initial_qpos = getattr(obj, 'initial_qpos', None) or robot.init_qpos + self._robots.append(obj) + elif obj_type == 'PositionableTable': + import xml.etree.ElementTree as ET + name = f"table_{id(obj)}" + h, w, l = obj.height, obj.width/2, obj.length/2 + + legs = "" + for i, (x, y) in enumerate([(w-0.1, l-0.1), (-w+0.1, l-0.1), (w-0.1, -l+0.1), (-w+0.1, -l+0.1)]): + legs += f'' + + xml = f''' + + + + {legs} + ''' + + self.world.worldbody.append(ET.fromstring(xml).find(".//body")) + obj._robosuite_name = name + elif hasattr(obj, 'xml_path') or hasattr(obj, 'xml_string'): + rs_obj = self.xml_builder.create_scenic_object(obj) + z = obj.height/2 if hasattr(obj, 'height') else 0 + rs_obj.set_base_xpos([obj.position.x, obj.position.y, obj.position.z + z]) + if hasattr(obj, 'yaw'): rs_obj.set_base_ori([0, 0, obj.yaw]) + self.world.merge(rs_obj) + obj._robosuite_name = rs_obj.name + else: + self._create_geom(obj) + + def _create_geom(self, obj): + cls = type(obj).__name__.lower() + name = f"obj_{id(obj)}" + physics = {'density': getattr(obj, 'density', 1000), 'friction': getattr(obj, 'friction', (1.0, 0.005, 0.0001)), + 'rgba': scenic_to_rgba(getattr(obj, 'color', (0.5, 0.5, 0.5)))} + + if 'ball' in cls: + rs_obj, z = BallObject(name, size=[obj.width/2], **physics), obj.width/2 + elif 'cylinder' in cls: + rs_obj, z = CylinderObject(name, size=[obj.width/2, obj.height/2], **physics), obj.height/2 + else: + rs_obj, z = BoxObject(name, size=[obj.width/2, obj.length/2, obj.height/2], **physics), obj.height/2 + + rs_obj.get_obj().set('pos', f'{obj.position.x} {obj.position.y} {obj.position.z + z}') + self.world.worldbody.append(rs_obj.get_obj()) + obj._robosuite_name = name + + def executeActions(self, allActions): + super().executeActions(allActions) + self._pending_actions = {} + for r in self._robots: + if r in allActions and allActions[r]: + for action in allActions[r]: + if action: + if action.__class__.__name__ == 'SetJointPositions': + self._pending_actions[r] = ('joint', action.positions) + elif action.__class__.__name__ == 'OSCPositionAction': + self._pending_actions[r] = ('osc', action) + + def step(self): + for name, id in self._body_id_map.items(): + self._prev_positions[name] = self.data.xpos[id].copy() + + if self.robosuite_env: + action = np.zeros(7) + for r, (t, d) in self._pending_actions.items(): + if t == 'osc': + if d.position_delta: action[:3] = d.position_delta + if d.orientation_delta: action[3:6] = d.orientation_delta + if d.gripper is not None: action[6] = d.gripper + else: + action = np.array(d) + break + + for _ in range(self.physics_steps): + self._current_obs, _, _, _ = self.robosuite_env.step(action) + if self.render: self.robosuite_env.render() + else: + # Apply actions + for r, (t, d) in self._pending_actions.items(): + if t == 'joint': + for i, act in enumerate(r._robot_model.actuators[:len(d)]): + aid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, act) + if aid != -1: self.data.ctrl[aid] = float(d[i]) + + # Step + start = None + for i in range(self.physics_steps): + mujoco.mj_step(self.model, self.data) + if self.real_time and self.viewer and i == 0: start = time.monotonic() + if self.viewer and i % 25 == 0: self.viewer.sync() + + if self.real_time and self.viewer and start: + time.sleep(max(0, self.timestep / self.speed - (time.monotonic() - start))) + + mujoco.mj_forward(self.model, self.data) + + self._pending_actions = {} + + # Update robots + for r in self._robots: + if hasattr(r, '_robosuite_robot'): + r.joint_positions = r._robosuite_robot._joint_positions + if self._current_obs: + r.eef_pos = self._current_obs.get('robot0_eef_pos', [0, 0, 0]) + r.gripper_state = self._current_obs.get('robot0_gripper_qpos', [0, 0]) + else: + r.joint_positions = [self.data.qpos[self.model.jnt_qposadr[mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, j)]] + for j in r._robot_model.joints if mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, j) != -1] + + def getProperties(self, obj, properties): + values = {} + + # Get body ID + id = -1 + if hasattr(obj, '_robosuite_name'): + id = self._body_id_map.get(obj._robosuite_name, -1) + elif hasattr(obj, '_robot_model') or hasattr(obj, '_robosuite_robot'): + name = (obj._robosuite_robot.robot_model.naming_prefix if hasattr(obj, '_robosuite_robot') + else obj._robot_model.naming_prefix) + id = self._body_id_map.get(name, -1) + elif hasattr(obj, '_env_object_name'): + id = self._body_id_map.get(obj._env_object_name, -1) + + for prop in properties: + if prop == 'position': + if id != -1: + pos = self.data.xpos[id] + z_off = obj.height/2 if hasattr(obj, 'height') and not hasattr(obj, 'robot_type') and \ + any(s in type(obj).__name__.lower() for s in ['cube', 'box', 'ball', 'cylinder']) else 0 + values[prop] = Vector(pos[0], pos[1], pos[2] - z_off) + elif hasattr(obj, '_env_object_name') and self._current_obs and f"{obj._env_object_name}_pos" in self._current_obs: + pos = self._current_obs[f"{obj._env_object_name}_pos"] + values[prop] = Vector(pos[0], pos[1], pos[2]) + else: + values[prop] = getattr(obj, prop) + elif prop == 'velocity' and id != -1: + if self.model.body_dofnum[id] >= 3: + v = self.data.qvel[self.model.body_dofadr[id]:self.model.body_dofadr[id]+3] + else: + name = obj._robosuite_name if hasattr(obj, '_robosuite_name') else '' + v = (self.data.xpos[id] - self._prev_positions.get(name, self.data.xpos[id])) / self.timestep + values[prop] = Vector(v[0], v[1], v[2]) + elif prop == 'speed' and id != -1: + v = values.get('velocity') or self.getProperties(obj, ['velocity']).get('velocity', Vector(0,0,0)) + values[prop] = np.linalg.norm([v.x, v.y, v.z]) + elif prop in ['yaw', 'pitch', 'roll'] and id != -1: + w, x, y, z = self.data.xquat[id] + if prop == 'yaw': values[prop] = math.atan2(2*(w*z+x*y), 1-2*(y*y+z*z)) + elif prop == 'pitch': values[prop] = math.asin(np.clip(2*(w*y-z*x), -1, 1)) + elif prop == 'roll': values[prop] = math.atan2(2*(w*x+y*z), 1-2*(x*x+y*y)) + else: + values[prop] = getattr(obj, prop) + + return values + + def checkSuccess(self): + return self.robosuite_env._check_success() if self.robosuite_env and hasattr(self.robosuite_env, '_check_success') else False + + def destroy(self): + if self.render and (self.robosuite_env or self.viewer): + print("Simulation complete. Window will close in 2 seconds...") + time.sleep(2) + + if self.robosuite_env: + self.robosuite_env.close() + self.robosuite_env = None + + if self.viewer: + try: self.viewer.close() + except: pass + self.viewer = None \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/utils.py b/src/scenic/simulators/robosuite/utils.py new file mode 100644 index 000000000..8dcb2e32c --- /dev/null +++ b/src/scenic/simulators/robosuite/utils.py @@ -0,0 +1,11 @@ +"""Utility functions for RoboSuite-Scenic integration.""" + +from typing import List + + +def scenic_to_rgba(color, alpha=1.0) -> List[float]: + """Convert Scenic color to RGBA format.""" + if hasattr(color, '__iter__') and len(color) >= 3: + rgba = list(color[:4]) if len(color) >= 4 else list(color[:3]) + [alpha] + return [float(x) for x in rgba] + return [0.5, 0.5, 0.5, float(alpha)] \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/xml_builder.py b/src/scenic/simulators/robosuite/xml_builder.py new file mode 100644 index 000000000..a929120f2 --- /dev/null +++ b/src/scenic/simulators/robosuite/xml_builder.py @@ -0,0 +1,115 @@ +# src/scenic/simulators/robosuite/xml_builder.py +"""XML builder for RoboSuite-Scenic integration.""" + +import os +import tempfile +import xml.etree.ElementTree as ET +from robosuite.models.objects import MujocoObject +from robosuite.models.arenas import Arena +from robosuite.utils.mjcf_utils import array_to_string, xml_path_completion + + +class XMLModifiableObject(MujocoObject): + """MujocoObject that can be modified at runtime.""" + + def __init__(self, name, xml_path=None, xml_string=None, **kwargs): + self.name = self.naming_prefix = name + self.obj_type = kwargs.get('obj_type', 'all') + + if xml_string: + self.root = ET.fromstring(xml_string) + self.worldbody = self.root.find("worldbody") + self.asset = self.root.find("asset") + else: + super().__init__(xml_path_completion(xml_path)) + + def get_obj(self): + """Get the XML tree with name prefix.""" + from robosuite.utils.mjcf_utils import add_prefix + add_prefix(self.root, prefix=self.naming_prefix) + return self.root + + +class RoboSuiteXMLBuilder: + """Builder for creating RoboSuite objects from Scenic specifications.""" + + def create_scenic_object(self, scenic_obj): + """Create RoboSuite object from Scenic object.""" + name = f"{type(scenic_obj).__name__}_{id(scenic_obj)}" + + # XML-based object + if hasattr(scenic_obj, 'xml_path') and scenic_obj.xml_path: + obj = XMLModifiableObject(name, xml_path=scenic_obj.xml_path) + elif hasattr(scenic_obj, 'xml_string') and scenic_obj.xml_string: + obj = XMLModifiableObject(name, xml_string=scenic_obj.xml_string) + else: + # Procedural object + obj = XMLModifiableObject(name, xml_string=self._build_procedural_xml(scenic_obj, name)) + + # Apply properties + self._apply_properties(obj, scenic_obj) + return obj + + def create_arena_from_xml(self, xml_source, is_string=True): + """Create arena from XML string or path.""" + with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f: + if is_string: + f.write(xml_source) + else: + with open(xml_path_completion(xml_source)) as src: + f.write(src.read()) + temp_path = f.name + + try: + return Arena(temp_path) + finally: + os.unlink(temp_path) + + def _build_procedural_xml(self, scenic_obj, name): + """Build XML for geometric objects.""" + obj_type = type(scenic_obj).__name__.lower() + + # Geometry + if 'ball' in obj_type or 'sphere' in obj_type: + geom_type, size_str = "sphere", str(scenic_obj.width/2) + elif 'cylinder' in obj_type: + geom_type, size_str = "cylinder", f"{scenic_obj.width/2} {scenic_obj.height/2}" + else: # box/cube + geom_type, size_str = "box", f"{scenic_obj.width/2} {scenic_obj.length/2} {scenic_obj.height/2}" + + # Properties + rgba = array_to_string(list(getattr(scenic_obj, 'color', [0.5, 0.5, 0.5])) + [1.0]) + density = getattr(scenic_obj, 'density', 1000) + friction = array_to_string(getattr(scenic_obj, 'friction', [1.0, 0.005, 0.0001])) + + return f""" + + + + + + + + """ + + def _apply_properties(self, obj, scenic_obj): + """Apply Scenic properties to object.""" + # Color + if hasattr(scenic_obj, 'color'): + rgba = array_to_string(list(scenic_obj.color[:3]) + [1.0]) + for geom in obj.worldbody.findall(".//geom[@group='1']") or []: + geom.set("rgba", rgba) + if "material" in geom.attrib: + del geom.attrib["material"] + + # Physics + if hasattr(scenic_obj, 'density'): + for geom in obj.worldbody.findall(".//geom[@group='0']") or []: + geom.set("density", str(scenic_obj.density)) + + if hasattr(scenic_obj, 'friction'): + for geom in obj.worldbody.findall(".//geom[@group='0']") or []: + geom.set("friction", array_to_string(scenic_obj.friction)) \ No newline at end of file From 8ec7beb16ee4282550a50c9556e23e85b183ef01 Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Mon, 4 Aug 2025 20:35:41 +0530 Subject: [PATCH 2/8] Joint Space Controll --- .../robosuite/cube_random_position.scenic | 132 +++++ src/scenic/simulators/robosuite/model.scenic | 55 +- src/scenic/simulators/robosuite/simulator.py | 484 +++++++----------- 3 files changed, 372 insertions(+), 299 deletions(-) create mode 100644 examples/robosuite/cube_random_position.scenic diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic new file mode 100644 index 000000000..02d96cad3 --- /dev/null +++ b/examples/robosuite/cube_random_position.scenic @@ -0,0 +1,132 @@ +# examples/robosuite/cube_random_position.scenic +"""Randomized cube position with bounded lift behavior.""" + +model scenic.simulators.robosuite.model + +param use_environment = "Lift" +param render = True +param real_time = False # Override default: run as fast as possible +param camera_view = "birdview" + +# Use parameters from above +simulator RobosuiteSimulator( + render=globalParameters.render, + real_time=globalParameters.real_time, + use_environment=globalParameters.use_environment, + camera_view=globalParameters.camera_view +) + +# SCENIC RANDOMIZATION: cube position randomly sampled each run +cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) + +behavior AdaptiveLiftBehavior(): + """Fast pickup behavior with bounded lift.""" + + wait; wait # Initial stabilization only + + sim = simulation() + + # Get initial cube position + obs = sim.getCurrentObservation() + if obs and 'cube_pos' in obs: + print(f"\n=== RANDOMIZED CUBE PICKUP ===") + print(f"Cube spawned at: ({obs['cube_pos'][0]:.3f}, {obs['cube_pos'][1]:.3f}, {obs['cube_pos'][2]:.3f})") + + # Phase 1: Open gripper (20 steps) + print("\nOpening gripper...") + for i in range(20): + take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) + + # Phase 2: Move above cube + print("Moving above cube...") + for step in range(100): + obs = sim.getCurrentObservation() + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.1 - eef_pos[2] + ] + + if (error[0]**2 + error[1]**2 + error[2]**2)**0.5 < 0.02: + print(f" Reached above position at step {step}") + break + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 3: Move down to grasp + print("Moving to grasp position...") + for step in range(80): + obs = sim.getCurrentObservation() + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] - 0.02 - eef_pos[2] + ] + + if (error[0]**2 + error[1]**2 + error[2]**2)**0.5 < 0.01: + print(f" Reached grasp position at step {step}") + break + + delta = [max(-0.2, min(0.2, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=-1) + + # Phase 4: Close gripper (minimal steps) + print("Closing gripper...") + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) + + # Phase 5: Lift with hard boundary + print("Lifting...") + TARGET_HEIGHT = 1.0 # RoboSuite success threshold + MAX_LIFT_HEIGHT = 1.0 # Hard boundary + + for step in range(100): + obs = sim.getCurrentObservation() + if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: + wait; continue + + cube_pos = obs['cube_pos'] + eef_pos = obs['robot0_eef_pos'] + + # Success check + if cube_pos[2] > TARGET_HEIGHT: + success = sim.checkSuccess() + print(f"\nSUCCESS: {success}") + print(f"Cube height: {cube_pos[2]:.3f}m (> {TARGET_HEIGHT}m)") + print(f"Completed in {step} lift steps") + terminate simulation + + # Safety check + if cube_pos[2] > MAX_LIFT_HEIGHT: + print(f"\nMax height reached: {cube_pos[2]:.3f}m") + terminate simulation + + # Target: 15cm above initial + error = [ + cube_pos[0] - eef_pos[0], + cube_pos[1] - eef_pos[1], + cube_pos[2] + 0.15 - eef_pos[2] + ] + + delta = [max(-0.3, min(0.3, e * 3)) for e in error] + take OSCPositionAction(position_delta=delta, gripper=1) + + print("Failed to reach target height") + terminate simulation + +# ego = new PandaRobot with behavior AdaptiveLiftBehavior() +ego = new UR5eRobot with behavior AdaptiveLiftBehavior() \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index d009aa490..7ecdad4e0 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,23 +1,34 @@ -# src/scenic/simulators/robosuite/model.scenic - """Scenic world model for RoboSuite simulator.""" from .simulator import RobosuiteSimulator -# Simulator -simulator RobosuiteSimulator(render=True, real_time=False, speed=1.0) - -# Global parameters +# Global parameters with defaults param use_environment = None param env_config = {} param controller_config = None - -# Base class +param camera_view = None +param render = True +param real_time = True # Default: real-time ON +param speed = 1.0 + +# Simulator - uses parameters from above +simulator RobosuiteSimulator( + render=globalParameters.render, + real_time=globalParameters.real_time, + speed=globalParameters.speed, + use_environment=globalParameters.use_environment, + env_config=globalParameters.env_config, + controller_config=globalParameters.controller_config, + camera_view=globalParameters.camera_view +) + +# Base class class RoboSuiteObject(Object): 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) + shape: BoxShape() # Default shape # Objects class XMLObject(RoboSuiteObject): @@ -41,6 +52,22 @@ class Cylinder(RoboSuiteObject): height: 0.1 color: (0.2, 0.2, 0.8) +# Environment-specific objects +class LiftCube(Cube): + """Cube in Lift environment.""" + envObjectName: "cube" # Public attribute + allowCollisions: True + +class StackCubeA(Cube): + """First cube in Stack environment.""" + envObjectName: "cubeA" # Public attribute + allowCollisions: True + +class StackCubeB(Cube): + """Second cube in Stack environment.""" + envObjectName: "cubeB" # Public attribute + allowCollisions: True + # Robots class Robot(RoboSuiteObject): robot_type: "Panda" @@ -68,6 +95,12 @@ class SawyerRobot(Robot): initial_qpos: [0, -0.785, 0, 1.571, 0, -0.785, 0] height: 1.0 +class UR5eRobot(Robot): + robot_type: "UR5e" + gripper_type: "Robotiq85Gripper" + initial_qpos: None # Use default + height: 0.85 + # Arenas class Arena(RoboSuiteObject): pass @@ -142,7 +175,7 @@ behavior RobustLiftBehavior(): # Move above object for step in range(100): - obs = sim._current_obs + obs = sim.getCurrentObservation() if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: wait; continue @@ -159,7 +192,7 @@ behavior RobustLiftBehavior(): # Move down for step in range(80): - obs = sim._current_obs + obs = sim.getCurrentObservation() if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: wait; continue @@ -180,7 +213,7 @@ behavior RobustLiftBehavior(): # Lift for step in range(100): - obs = sim._current_obs + obs = sim.getCurrentObservation() if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: wait; continue diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py index d031bba18..b946224b5 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -1,5 +1,4 @@ -# src/scenic/simulators/robosuite/simulator.py -"""RoboSuite Simulator for Scenic - Version 3.0 (Reduced)""" +"""RoboSuite Simulator for Scenic - Version 3.2 (Clean)""" import numpy as np import mujoco @@ -9,46 +8,69 @@ try: import robosuite as suite - from robosuite.models import MujocoWorldBase - from robosuite.models.arenas import EmptyArena, TableArena - from robosuite.models.objects import BoxObject, BallObject, CylinderObject from robosuite.robots import ROBOT_CLASS_MAPPING except ImportError as e: suite = None - _import_error = e # why not throw error early (s) + _import_error = e from scenic.core.simulators import Simulation, Simulator from scenic.core.vectors import Vector -from .utils import scenic_to_rgba # why only one module from .utils, that means other are redundent (s) +from .utils import scenic_to_rgba class RobosuiteSimulator(Simulator): def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, - env_config=None, controller_config=None): + env_config=None, controller_config=None, camera_view=None): super().__init__() if suite is None: raise RuntimeError(f"Unable to import RoboSuite: {_import_error}") - self.render, self.real_time, self.speed = render, real_time, speed + self.render = render + self.real_time = real_time + self.speed = speed self.use_environment = use_environment self.env_config = env_config or {} self.controller_config = controller_config + self.camera_view = camera_view def createSimulation(self, scene, **kwargs): return RobosuiteSimulation(scene, self.render, self.real_time, self.speed, - self.use_environment, self.env_config, self.controller_config, **kwargs) + self.use_environment, self.env_config, self.controller_config, + self.camera_view, **kwargs) class RobosuiteSimulation(Simulation): ENV_OBJECTS = {"Lift": {"cube": "cube_main"}, "Stack": {"cubeA": "cubeA_main", "cubeB": "cubeB_main"}} + CAMERA_VIEWS = { + "frontview": 0, + "birdview": 1, + "agentview": 2, + "sideview": 3, + "robot0_robotview": 4, + "robot0_eye_in_hand": 5 + } def __init__(self, scene, render, real_time, speed, use_environment=None, - env_config=None, controller_config=None, **kwargs): - self.render, self.real_time, self.speed = render, real_time, speed - self.use_environment, self.env_config = use_environment, env_config or {} + env_config=None, controller_config=None, camera_view=None, **kwargs): + self.render = render + self.real_time = real_time + self.speed = speed + self.use_environment = use_environment + self.env_config = env_config or {} self.controller_config = controller_config - self.robosuite_env = self.world = self.model = self.data = self.viewer = None - self._body_id_map, self._prev_positions, self._robots = {}, {}, [] - self._current_obs, self._pending_actions = None, {} + self.camera_view = camera_view + self.robosuite_env = None + self.model = None + self.data = None + self.viewer = None + + # Mapping objects to RoboSuite IDs (instead of storing on objects) + self.body_id_map = {} + self.object_names = {} # Scenic object -> RoboSuite name + self.robots = [] + self.prev_positions = {} + self.pending_actions = {} + self._current_obs = None + self.timestep = kwargs.get('timestep') or 0.01 self.physics_timestep = 0.002 self.physics_steps = int(self.timestep / self.physics_timestep) @@ -56,316 +78,202 @@ def __init__(self, scene, render, real_time, speed, use_environment=None, super().__init__(scene, **kwargs) def setup(self): - if self.use_environment: - # Pre-configured environment - robots = [obj for obj in self.scene.objects if hasattr(obj, 'robot_type')] - config = {**self.env_config, 'has_renderer': self.render, 'render_camera': "frontview", - 'camera_names': ["frontview", "robot0_eye_in_hand"], 'controller_configs': self.controller_config} - if robots: - config['robots'] = [r.robot_type for r in robots] if len(robots) > 1 else robots[0].robot_type - - self.robosuite_env = suite.make(self.use_environment, **config) - self._current_obs = self.robosuite_env.reset() - self.model = self.robosuite_env.sim.model._model - self.data = self.robosuite_env.sim.data._data - - for i, r in enumerate(robots[:len(self.robosuite_env.robots)]): - r._robosuite_robot = self.robosuite_env.robots[i] - r._robot_model = r._robosuite_robot.robot_model - self._robots.append(r) - - # Modify env objects - for obj in self.scene.objects: - if hasattr(obj, '_env_object_name') and hasattr(obj, 'position'): - env_obj = getattr(self.robosuite_env, obj._env_object_name, None) - if env_obj and hasattr(env_obj, 'get_obj'): - body = env_obj.get_obj().find(f".//body[@name='{obj._env_object_name}_main']") - if body is not None: - body.set('pos', f"{obj.position.x} {obj.position.y} {obj.position.z}") - - # Add custom objects - world = self.robosuite_env.model - for obj in self.scene.objects: - if not any([hasattr(obj, attr) for attr in ['robot_type', '_env_object_name']]) and \ - type(obj).__name__ not in ['Arena', 'EmptyArena', 'TableArena']: - self._add_to_env(world, obj) - - self._current_obs = self.robosuite_env.reset() - else: - # Custom world - self.world = MujocoWorldBase() - from .xml_builder import RoboSuiteXMLBuilder - self.xml_builder = RoboSuiteXMLBuilder() - - arena = TableArena() if any(type(o).__name__ == 'Table' for o in self.scene.objects) else EmptyArena() - self.world.merge(arena) - super().setup() - - self.model = self.world.get_model(mode="mujoco") - self.data = mujoco.MjData(self.model) - self.model.opt.gravity[2] = -9.81 - self.model.opt.timestep = self.physics_timestep - - if self.render: - try: self.viewer = mujoco.viewer.launch_passive(self.model, self.data) - except: self.viewer = None + super().setup() # Call parent setup first to populate objects - # Setup body mapping - if self.use_environment in self.ENV_OBJECTS: - for name, body in self.ENV_OBJECTS[self.use_environment].items(): - id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body) - if id != -1: self._body_id_map[name] = id + if not self.use_environment: + raise ValueError("Only pre-configured RoboSuite environments are supported") + + # Get robots from scene + robots = [obj for obj in self.objects if hasattr(obj, 'robot_type')] - for obj in self.objects: - self._map_body(obj) + # Configure environment + config = { + **self.env_config, + 'has_renderer': self.render, + 'render_camera': "frontview", + 'camera_names': ["frontview", "robot0_eye_in_hand"], + 'controller_configs': self.controller_config + } - # Initialize robots - for r in self._robots: - if hasattr(r, '_robot_model') and hasattr(r, '_initial_qpos'): - for joint, qpos in zip(r._robot_model.joints, r._initial_qpos): - jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint) - if jid != -1: self.data.qpos[self.model.jnt_qposadr[jid]] = qpos + # Prepare robots argument + robot_arg = "Panda" # Default robot + if robots: + robot_arg = [r.robot_type for r in robots] if len(robots) > 1 else robots[0].robot_type - if not self.robosuite_env: - mujoco.mj_forward(self.model, self.data) + # Create RoboSuite environment + self.robosuite_env = suite.make(self.use_environment, robots=robot_arg, **config) + self._current_obs = self.robosuite_env.reset() + self.model = self.robosuite_env.sim.model._model + self.data = self.robosuite_env.sim.data._data - # Configure dynamics - for obj in self.scene.objects: - if hasattr(obj, 'robot_type'): - obj._dynamicProperties.update({'joint_positions': True, 'eef_pos': True, 'gripper_state': True}) - if hasattr(obj, 'behavior') and obj.behavior: - self.agents.append(obj) - else: - obj._dynamicProperties['position'] = True - - def _map_body(self, obj): - id = -1 - if hasattr(obj, '_robosuite_name'): - for suffix in ["_main", "", "_body"]: - id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, obj._robosuite_name + suffix) - if id != -1: - self._body_id_map[obj._robosuite_name] = id - break - elif hasattr(obj, '_robot_model') or hasattr(obj, '_robosuite_robot'): - name = (obj._robosuite_robot.robot_model.naming_prefix if hasattr(obj, '_robosuite_robot') - else obj._robot_model.naming_prefix) - id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name + "base_link") - if id != -1: self._body_id_map[name] = id + # Set camera view + if self.render and self.camera_view is not None: + camera_id = self.camera_view + if isinstance(self.camera_view, str): + camera_id = self.CAMERA_VIEWS.get(self.camera_view.lower(), 0) + if self.robosuite_env.viewer: + self.robosuite_env.viewer.set_camera(camera_id=camera_id) - if id != -1: - key = obj._robosuite_name if hasattr(obj, '_robosuite_name') else name - self._prev_positions[key] = self.data.xpos[id].copy() - - def _add_to_env(self, world, obj): - cls = type(obj).__name__.lower() - name = f"custom_{id(obj)}" - - # Create object - sizes = {'ball': [obj.width/2], 'cylinder': [obj.width/2, obj.height/2]} - size = sizes.get(cls.split('_')[0], [obj.width/2, obj.length/2, obj.height/2]) + # Store robot references + for i, r in enumerate(robots[:len(self.robosuite_env.robots)]): + self.robots.append(r) + self.object_names[r] = f"robot{i}" - ObjClass = {'ball': BallObject, 'cylinder': CylinderObject}.get(cls.split('_')[0], BoxObject) - rs_obj = ObjClass(name, size=size, density=getattr(obj, 'density', 1000), - rgba=scenic_to_rgba(getattr(obj, 'color', (0.5, 0.5, 0.5)))) + # Apply initial positions for environment objects + self._apply_initial_positions() - # Position - z = obj.height/2 if hasattr(obj, 'height') else 0 - body = rs_obj.get_obj().find(f".//body[@name='{name}_main']") or rs_obj.get_obj().find(f".//body[@name='{name}']") - if body is not None: - body.set('pos', f'{obj.position.x} {obj.position.y} {obj.position.z + z}') + # Setup body mapping + if self.use_environment in self.ENV_OBJECTS: + for name, body in self.ENV_OBJECTS[self.use_environment].items(): + id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body) + if id != -1: + self.body_id_map[name] = id + self.prev_positions[name] = self.data.xpos[id].copy() - world.merge(rs_obj) - obj._robosuite_name = name + # Identify agents + for obj in self.objects: + if hasattr(obj, 'behavior') and obj.behavior: + self.agents.append(obj) - def createObjectInSimulator(self, obj): - if self.robosuite_env: return + def _apply_initial_positions(self): + """Apply initial positions for environment objects.""" + # Apply positions for objects with envObjectName + for obj in self.objects: + if hasattr(obj, 'envObjectName') and hasattr(obj, 'position'): + joint_name = f"{obj.envObjectName}_joint0" + try: + qpos = np.concatenate([ + [obj.position.x, obj.position.y, obj.position.z], + [1, 0, 0, 0] # identity quaternion + ]) + self.robosuite_env.sim.data.set_joint_qpos(joint_name, qpos) + except Exception as e: + print(f"Warning: Could not set joint position for {obj.envObjectName}: {e}") - obj_type = type(obj).__name__ - if obj_type in ['Arena', 'EmptyArena', 'TableArena', 'BinsArena', 'PegsArena', 'WipeArena', 'CustomArena', 'Table']: - return + # Update physics + self.robosuite_env.sim.forward() - if hasattr(obj, 'robot_type'): - robot_class = ROBOT_CLASS_MAPPING.get(obj.robot_type) - if not robot_class: raise ValueError(f"Unknown robot type: {obj.robot_type}") - - robot = robot_class(robot_type=obj.robot_type, idn=len(self._robots), - initial_qpos=getattr(obj, 'initial_qpos', None), control_freq=20) - robot.load_model() - robot.robot_model.set_base_xpos([obj.position.x, obj.position.y, obj.position.z]) - if hasattr(obj, 'yaw'): robot.robot_model.set_base_ori([0, 0, obj.yaw]) - - self.world.merge(robot.robot_model) - obj._robot = robot - obj._robot_model = robot.robot_model - obj._initial_qpos = getattr(obj, 'initial_qpos', None) or robot.init_qpos - self._robots.append(obj) - elif obj_type == 'PositionableTable': - import xml.etree.ElementTree as ET - name = f"table_{id(obj)}" - h, w, l = obj.height, obj.width/2, obj.length/2 - - legs = "" - for i, (x, y) in enumerate([(w-0.1, l-0.1), (-w+0.1, l-0.1), (w-0.1, -l+0.1), (-w+0.1, -l+0.1)]): - legs += f'' + # Let physics settle + for _ in range(10): + self.robosuite_env.step(np.zeros(7)) - xml = f''' - - - - {legs} - ''' - - self.world.worldbody.append(ET.fromstring(xml).find(".//body")) - obj._robosuite_name = name - elif hasattr(obj, 'xml_path') or hasattr(obj, 'xml_string'): - rs_obj = self.xml_builder.create_scenic_object(obj) - z = obj.height/2 if hasattr(obj, 'height') else 0 - rs_obj.set_base_xpos([obj.position.x, obj.position.y, obj.position.z + z]) - if hasattr(obj, 'yaw'): rs_obj.set_base_ori([0, 0, obj.yaw]) - self.world.merge(rs_obj) - obj._robosuite_name = rs_obj.name - else: - self._create_geom(obj) + # Get fresh observations + self._current_obs = self.robosuite_env._get_observations() - def _create_geom(self, obj): - cls = type(obj).__name__.lower() - name = f"obj_{id(obj)}" - physics = {'density': getattr(obj, 'density', 1000), 'friction': getattr(obj, 'friction', (1.0, 0.005, 0.0001)), - 'rgba': scenic_to_rgba(getattr(obj, 'color', (0.5, 0.5, 0.5)))} - - if 'ball' in cls: - rs_obj, z = BallObject(name, size=[obj.width/2], **physics), obj.width/2 - elif 'cylinder' in cls: - rs_obj, z = CylinderObject(name, size=[obj.width/2, obj.height/2], **physics), obj.height/2 - else: - rs_obj, z = BoxObject(name, size=[obj.width/2, obj.length/2, obj.height/2], **physics), obj.height/2 - - rs_obj.get_obj().set('pos', f'{obj.position.x} {obj.position.y} {obj.position.z + z}') - self.world.worldbody.append(rs_obj.get_obj()) - obj._robosuite_name = name + def createObjectInSimulator(self, obj): + # For pre-configured environments, objects are already created + pass def executeActions(self, allActions): super().executeActions(allActions) - self._pending_actions = {} - for r in self._robots: - if r in allActions and allActions[r]: - for action in allActions[r]: - if action: - if action.__class__.__name__ == 'SetJointPositions': - self._pending_actions[r] = ('joint', action.positions) - elif action.__class__.__name__ == 'OSCPositionAction': - self._pending_actions[r] = ('osc', action) + self.pending_actions = {} + + for agent in self.agents: + if agent in allActions and allActions[agent]: + for action in allActions[agent]: + if action and hasattr(action, 'applyTo'): + # Store action for this agent + if hasattr(action, 'position_delta'): + self.pending_actions[agent] = ('osc', action) + elif hasattr(action, 'positions'): + self.pending_actions[agent] = ('joint', action.positions) def step(self): - for name, id in self._body_id_map.items(): - self._prev_positions[name] = self.data.xpos[id].copy() - - if self.robosuite_env: - action = np.zeros(7) - for r, (t, d) in self._pending_actions.items(): - if t == 'osc': - if d.position_delta: action[:3] = d.position_delta - if d.orientation_delta: action[3:6] = d.orientation_delta - if d.gripper is not None: action[6] = d.gripper - else: - action = np.array(d) - break - - for _ in range(self.physics_steps): - self._current_obs, _, _, _ = self.robosuite_env.step(action) - if self.render: self.robosuite_env.render() - else: - # Apply actions - for r, (t, d) in self._pending_actions.items(): - if t == 'joint': - for i, act in enumerate(r._robot_model.actuators[:len(d)]): - aid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, act) - if aid != -1: self.data.ctrl[aid] = float(d[i]) - - # Step - start = None - for i in range(self.physics_steps): - mujoco.mj_step(self.model, self.data) - if self.real_time and self.viewer and i == 0: start = time.monotonic() - if self.viewer and i % 25 == 0: self.viewer.sync() - - if self.real_time and self.viewer and start: - time.sleep(max(0, self.timestep / self.speed - (time.monotonic() - start))) + # Check if episode is done + if hasattr(self, '_done') and self._done: + return - mujoco.mj_forward(self.model, self.data) - - self._pending_actions = {} + # Store previous positions + for name, id in self.body_id_map.items(): + self.prev_positions[name] = self.data.xpos[id].copy() - # Update robots - for r in self._robots: - if hasattr(r, '_robosuite_robot'): - r.joint_positions = r._robosuite_robot._joint_positions - if self._current_obs: - r.eef_pos = self._current_obs.get('robot0_eef_pos', [0, 0, 0]) - r.gripper_state = self._current_obs.get('robot0_gripper_qpos', [0, 0]) + # Execute actions + action = np.zeros(7) + for agent, (action_type, data) in self.pending_actions.items(): + if action_type == 'osc': + if hasattr(data, 'position_delta') and data.position_delta: + action[:3] = data.position_delta + if hasattr(data, 'orientation_delta') and data.orientation_delta: + action[3:6] = data.orientation_delta + if hasattr(data, 'gripper') and data.gripper is not None: + action[6] = data.gripper else: - r.joint_positions = [self.data.qpos[self.model.jnt_qposadr[mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, j)]] - for j in r._robot_model.joints if mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, j) != -1] + action = np.array(data) + break # RoboSuite only supports one robot action per step + + # Step simulation + 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_actions = {} def getProperties(self, obj, properties): values = {} - # Get body ID - id = -1 - if hasattr(obj, '_robosuite_name'): - id = self._body_id_map.get(obj._robosuite_name, -1) - elif hasattr(obj, '_robot_model') or hasattr(obj, '_robosuite_robot'): - name = (obj._robosuite_robot.robot_model.naming_prefix if hasattr(obj, '_robosuite_robot') - else obj._robot_model.naming_prefix) - id = self._body_id_map.get(name, -1) - elif hasattr(obj, '_env_object_name'): - id = self._body_id_map.get(obj._env_object_name, -1) - for prop in properties: if prop == 'position': - if id != -1: - pos = self.data.xpos[id] - z_off = obj.height/2 if hasattr(obj, 'height') and not hasattr(obj, 'robot_type') and \ - any(s in type(obj).__name__.lower() for s in ['cube', 'box', 'ball', 'cylinder']) else 0 - values[prop] = Vector(pos[0], pos[1], pos[2] - z_off) - elif hasattr(obj, '_env_object_name') and self._current_obs and f"{obj._env_object_name}_pos" in self._current_obs: - pos = self._current_obs[f"{obj._env_object_name}_pos"] - values[prop] = Vector(pos[0], pos[1], pos[2]) + # Check if it's an environment object + if hasattr(obj, 'envObjectName') and self._current_obs: + obs_key = f"{obj.envObjectName}_pos" + if obs_key in self._current_obs: + pos = self._current_obs[obs_key] + values[prop] = Vector(pos[0], pos[1], pos[2]) + else: + values[prop] = obj.position else: - values[prop] = getattr(obj, prop) - elif prop == 'velocity' and id != -1: - if self.model.body_dofnum[id] >= 3: - v = self.data.qvel[self.model.body_dofadr[id]:self.model.body_dofadr[id]+3] + values[prop] = obj.position + + elif prop == 'joint_positions' and obj in self.robots: + # Get joint positions for robots + robot_idx = self.robots.index(obj) + if robot_idx < len(self.robosuite_env.robots): + positions = self.robosuite_env.robots[robot_idx]._joint_positions + values[prop] = list(positions) # Convert numpy array to list else: - name = obj._robosuite_name if hasattr(obj, '_robosuite_name') else '' - v = (self.data.xpos[id] - self._prev_positions.get(name, self.data.xpos[id])) / self.timestep - values[prop] = Vector(v[0], v[1], v[2]) - elif prop == 'speed' and id != -1: - v = values.get('velocity') or self.getProperties(obj, ['velocity']).get('velocity', Vector(0,0,0)) - values[prop] = np.linalg.norm([v.x, v.y, v.z]) - elif prop in ['yaw', 'pitch', 'roll'] and id != -1: - w, x, y, z = self.data.xquat[id] - if prop == 'yaw': values[prop] = math.atan2(2*(w*z+x*y), 1-2*(y*y+z*z)) - elif prop == 'pitch': values[prop] = math.asin(np.clip(2*(w*y-z*x), -1, 1)) - elif prop == 'roll': values[prop] = math.atan2(2*(w*x+y*z), 1-2*(x*x+y*y)) + values[prop] = [] + + elif prop == 'eef_pos' and obj in self.robots and self._current_obs: + pos = self._current_obs.get('robot0_eef_pos', [0, 0, 0]) + values[prop] = list(pos) # Convert numpy array to list + + elif prop == 'gripper_state' and obj in self.robots and self._current_obs: + state = self._current_obs.get('robot0_gripper_qpos', [0, 0]) + values[prop] = list(state) # Convert numpy array to list + else: - values[prop] = getattr(obj, prop) + # Default to object's own property + values[prop] = getattr(obj, prop, None) return values + def getCurrentObservation(self): + """Public method to get current observations.""" + return self._current_obs + def checkSuccess(self): - return self.robosuite_env._check_success() if self.robosuite_env and hasattr(self.robosuite_env, '_check_success') else False + return self.robosuite_env._check_success() if hasattr(self.robosuite_env, '_check_success') else False def destroy(self): - if self.render and (self.robosuite_env or self.viewer): + if self.render and self.robosuite_env: print("Simulation complete. Window will close in 2 seconds...") time.sleep(2) - if self.robosuite_env: - self.robosuite_env.close() - self.robosuite_env = None + try: + if self.robosuite_env: + self.robosuite_env.close() + self.robosuite_env = None + except Exception as e: + print(f"Warning: Error closing RoboSuite environment: {e}") - if self.viewer: - try: self.viewer.close() - except: pass - self.viewer = None \ No newline at end of file + try: + if self.viewer: + self.viewer.close() + self.viewer = None + except Exception as e: + print(f"Warning: Error closing viewer: {e}") \ No newline at end of file From bde69a3a32702a2293be9b5895dfa46d2b5e8011 Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Thu, 7 Aug 2025 23:46:43 +0530 Subject: [PATCH 3/8] Robosuite Pre-config env only --- .../basic_shapes.scenic | 29 -- .../circular_formation.scenic | 26 -- .../deifferent_shape_objects.scenic | 60 ---- .../grid_formation.scenic | 24 -- .../height_levels.scenic | 23 -- .../basic_object_placement/test_static.scenic | 7 - .../robosuite/cube_random_position.scenic | 143 ++------ .../robosuite/lift_custom_position.scenic | 155 --------- examples/robosuite/lift_task.scenic | 77 ---- examples/robosuite/make_osc_test.scenic | 189 ---------- examples/robosuite/tower_collapse.scenic | 83 ----- examples/robosuite/ur5e_lift_test.scenic | 159 --------- examples/robosuite/xml_test.scenic | 20 -- src/scenic/simulators/robosuite/__init__.py | 12 +- src/scenic/simulators/robosuite/model.scenic | 328 ++++++++++-------- src/scenic/simulators/robosuite/simulator.py | 237 ++++++++----- src/scenic/simulators/robosuite/utils.py | 11 - .../simulators/robosuite/xml_builder.py | 115 ------ 18 files changed, 342 insertions(+), 1356 deletions(-) delete mode 100644 examples/robosuite/basic_object_placement/basic_shapes.scenic delete mode 100644 examples/robosuite/basic_object_placement/circular_formation.scenic delete mode 100644 examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic delete mode 100644 examples/robosuite/basic_object_placement/grid_formation.scenic delete mode 100644 examples/robosuite/basic_object_placement/height_levels.scenic delete mode 100644 examples/robosuite/basic_object_placement/test_static.scenic delete mode 100644 examples/robosuite/lift_custom_position.scenic delete mode 100644 examples/robosuite/lift_task.scenic delete mode 100644 examples/robosuite/make_osc_test.scenic delete mode 100644 examples/robosuite/tower_collapse.scenic delete mode 100644 examples/robosuite/ur5e_lift_test.scenic delete mode 100644 examples/robosuite/xml_test.scenic delete mode 100644 src/scenic/simulators/robosuite/utils.py delete mode 100644 src/scenic/simulators/robosuite/xml_builder.py diff --git a/examples/robosuite/basic_object_placement/basic_shapes.scenic b/examples/robosuite/basic_object_placement/basic_shapes.scenic deleted file mode 100644 index 833a36f62..000000000 --- a/examples/robosuite/basic_object_placement/basic_shapes.scenic +++ /dev/null @@ -1,29 +0,0 @@ -# test_suite_1_shapes.scenic -"""Test Suite 1: Different Shapes and Colors - FIXED.""" - -model scenic.simulators.robosuite.model - -class TestCube(Object): - width: 0.15 - length: 0.15 - height: 0.15 - # Remove shape property - use class name instead - -class TestBall(Object): - width: 0.15 - length: 0.15 - height: 0.15 - # Remove shape property - use class name instead - -class TestCylinder(Object): - width: 0.15 - length: 0.15 - height: 0.2 - # Remove shape property - use class name instead - -# Different shapes in a line -red_cube = new TestCube at (-0.4, 0.0, 0.1), with color (1, 0, 0) -green_ball = new TestBall at (0.0, 0.0, 0.1), with color (0, 1, 0) -blue_cylinder = new TestCylinder at (0.4, 0.0, 0.1), with color (0, 0, 1) - -ego = green_ball diff --git a/examples/robosuite/basic_object_placement/circular_formation.scenic b/examples/robosuite/basic_object_placement/circular_formation.scenic deleted file mode 100644 index 72271b295..000000000 --- a/examples/robosuite/basic_object_placement/circular_formation.scenic +++ /dev/null @@ -1,26 +0,0 @@ - -# test_suite_4_circle.scenic -"""Test Suite 4: Circular Formation.""" - -model scenic.simulators.robosuite.model - -class CircleCube(Object): - width: 0.08 - length: 0.08 - height: 0.08 - -# Arrange cubes in a circle -import math - -radius = 0.3 -cube_1 = new CircleCube at (radius * cos(0), radius * sin(0), 0.04), with color (1, 0, 0) -cube_2 = new CircleCube at (radius * cos(60 deg), radius * sin(60 deg), 0.04), with color (1, 0.5, 0) -cube_3 = new CircleCube at (radius * cos(120 deg), radius * sin(120 deg), 0.04), with color (1, 1, 0) -cube_4 = new CircleCube at (radius * cos(180 deg), radius * sin(180 deg), 0.04), with color (0, 1, 0) -cube_5 = new CircleCube at (radius * cos(240 deg), radius * sin(240 deg), 0.04), with color (0, 0, 1) -cube_6 = new CircleCube at (radius * cos(300 deg), radius * sin(300 deg), 0.04), with color (0.5, 0, 1) - -# Center cube -center_cube = new CircleCube at (0.0, 0.0, 0.04), with color (1, 1, 1) - -ego = center_cube diff --git a/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic b/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic deleted file mode 100644 index 2d3f3c723..000000000 --- a/examples/robosuite/basic_object_placement/deifferent_shape_objects.scenic +++ /dev/null @@ -1,60 +0,0 @@ -"""Advanced RoboSuite Test: Probabilistic Tower Collapse Scenario -Tests bidirectional data flow with physics-based constraints and temporal logic. -""" - -model scenic.simulators.robosuite.model - -# Probabilistic object types -class Tower(Object): - width: Range(0.08, 0.15) - length: Range(0.08, 0.15) - height: Range(0.2, 0.5) - color: Uniform((1, 0, 0), (0, 1, 0), (0, 0, 1)) - -class Ball(Object): - width: Range(0.06, 0.12) - length: self.width - height: self.width - color: (1, 1, 0) - -# Fixed towers with probabilistic properties -tower1 = new Tower at (Range(-0.3, -0.1), Range(-0.2, 0.2), Range(0.3, 0.8)) -tower2 = new Tower at (Range(-0.1, 0.1), Range(-0.2, 0.2), Range(0.3, 0.8)) -tower3 = new Tower at (Range(0.1, 0.3), Range(-0.2, 0.2), Range(0.3, 0.8)) - -# Ensure separation -require (distance from tower1 to tower2) > 0.15 -require (distance from tower2 to tower3) > 0.15 -require (distance from tower1 to tower3) > 0.15 - -# Projectile ball -projectile = new Ball at (Range(-0.4, -0.3), 0, Range(0.5, 1.0)) - -# Target selection - probabilistic -target_tower = Uniform(tower1, tower2, tower3) -ego = tower1 # Fixed ego object - -# Physics-based requirements using bidirectional data flow -require always projectile.position.z > -0.1 -require eventually (distance from projectile to target_tower) < 0.15 - -# Temporal monitoring -require always (target_tower.position.z > 0.05) until ( - (distance from projectile to target_tower) < 0.2 -) - -# Record bidirectional data -record projectile.position as "ball_trajectory" -record target_tower.position as "tower_position" -record projectile.velocity as "ball_velocity" -record tower1.position.z as "tower1_height" -record tower2.position.z as "tower2_height" -record tower3.position.z as "tower3_height" - -# Stability requirements -require always tower1.position.z > (tower1.height * 0.3) -require always tower2.position.z > (tower2.height * 0.3) -require always tower3.position.z > (tower3.height * 0.3) - -# Hard requirement - temporal operators can't be probabilistic -require eventually projectile.position.z < 0.1 \ No newline at end of file diff --git a/examples/robosuite/basic_object_placement/grid_formation.scenic b/examples/robosuite/basic_object_placement/grid_formation.scenic deleted file mode 100644 index 4a53cb39b..000000000 --- a/examples/robosuite/basic_object_placement/grid_formation.scenic +++ /dev/null @@ -1,24 +0,0 @@ -# test_suite_2_grid.scenic -"""Test Suite 2: Grid Formation.""" - -model scenic.simulators.robosuite.model - -class GridCube(Object): - width: 0.1 - length: 0.1 - height: 0.1 - -# 3x3 grid of cubes with different colors -cube_1_1 = new GridCube at (-0.2, -0.2, 0.05), with color (1, 0, 0) -cube_1_2 = new GridCube at (-0.2, 0.0, 0.05), with color (1, 0.5, 0) -cube_1_3 = new GridCube at (-0.2, 0.2, 0.05), with color (1, 1, 0) - -cube_2_1 = new GridCube at (0.0, -0.2, 0.05), with color (0, 1, 0) -cube_2_2 = new GridCube at (0.0, 0.0, 0.05), with color (0, 1, 1) -cube_2_3 = new GridCube at (0.0, 0.2, 0.05), with color (0, 0, 1) - -cube_3_1 = new GridCube at (0.2, -0.2, 0.05), with color (0.5, 0, 1) -cube_3_2 = new GridCube at (0.2, 0.0, 0.05), with color (1, 0, 1) -cube_3_3 = new GridCube at (0.2, 0.2, 0.05), with color (0.5, 0.5, 0.5) - -ego = cube_2_2 diff --git a/examples/robosuite/basic_object_placement/height_levels.scenic b/examples/robosuite/basic_object_placement/height_levels.scenic deleted file mode 100644 index 54ea89210..000000000 --- a/examples/robosuite/basic_object_placement/height_levels.scenic +++ /dev/null @@ -1,23 +0,0 @@ -# -"""Test Suite 3: Random Height Levels with Sampling.""" - -model scenic.simulators.robosuite.model - -class HeightCube(Object): - width: 0.12 - length: 0.12 - height: 0.12 - -# Random cube positions and heights -ground_cube = new HeightCube at (Range(-0.4, -0.2), Range(-0.2, 0.2), Range(0.05, 0.1)), with color (0, 0, 0) -low_cube = new HeightCube at (Range(-0.2, 0.0), Range(-0.2, 0.2), Range(0.15, 0.25)), with color (1, 0, 0) -mid_cube = new HeightCube at (Range(0.0, 0.2), Range(-0.2, 0.2), Range(0.3, 0.5)), with color (0, 1, 0) -high_cube = new HeightCube at (Range(0.2, 0.4), Range(-0.2, 0.2), Range(0.5, 0.7)), with color (0, 0, 1) - -# Ensure minimum separation between cubes -require (distance from ground_cube to low_cube) > 0.1 -require (distance from low_cube to mid_cube) > 0.1 -require (distance from mid_cube to high_cube) > 0.1 - -ego = mid_cube -record ego.position as egoPos \ No newline at end of file diff --git a/examples/robosuite/basic_object_placement/test_static.scenic b/examples/robosuite/basic_object_placement/test_static.scenic deleted file mode 100644 index 80075f096..000000000 --- a/examples/robosuite/basic_object_placement/test_static.scenic +++ /dev/null @@ -1,7 +0,0 @@ -# test_static.scenic -model scenic.simulators.robosuite.model - -# Create objects at known positions -new Cube at (0, 0, 0.025), with color (1, 0, 0) # Red cube on the ground -new Ball at (0.5, 0, 0.025), with color (0, 1, 0) # Green ball on the ground -new Cylinder at (-0.5, 0, 0.05), with color (0, 0, 1) # Blue cylinder on the ground \ No newline at end of file diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic index 02d96cad3..b590978d7 100644 --- a/examples/robosuite/cube_random_position.scenic +++ b/examples/robosuite/cube_random_position.scenic @@ -1,132 +1,33 @@ -# examples/robosuite/cube_random_position.scenic -"""Randomized cube position with bounded lift behavior.""" +"""Randomized cube position using behavior library.""" model scenic.simulators.robosuite.model param use_environment = "Lift" param render = True -param real_time = False # Override default: run as fast as possible +param real_time = False param camera_view = "birdview" +# press escape in simulater inrterface to reset to robosuite default frontview,for seeing the cube actually being lifted. +# press ']' or '[' to cycle between different fixed camera_views. (pre defined/set in robosuits's task enviroments) +# other options for camera_view: + # "frontview" + # "birdview" + # "agentview" + # "sideview" + # "robot0_robotview" + # "robot0_eye_in_hand" -# Use parameters from above -simulator RobosuiteSimulator( - render=globalParameters.render, - real_time=globalParameters.real_time, - use_environment=globalParameters.use_environment, - camera_view=globalParameters.camera_view -) -# SCENIC RANDOMIZATION: cube position randomly sampled each run +# Randomize cube position cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) -behavior AdaptiveLiftBehavior(): - """Fast pickup behavior with bounded lift.""" - - wait; wait # Initial stabilization only - - sim = simulation() - - # Get initial cube position - obs = sim.getCurrentObservation() - if obs and 'cube_pos' in obs: - print(f"\n=== RANDOMIZED CUBE PICKUP ===") - print(f"Cube spawned at: ({obs['cube_pos'][0]:.3f}, {obs['cube_pos'][1]:.3f}, {obs['cube_pos'][2]:.3f})") - - # Phase 1: Open gripper (20 steps) - print("\nOpening gripper...") - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) - - # Phase 2: Move above cube - print("Moving above cube...") - for step in range(100): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.1 - eef_pos[2] - ] - - if (error[0]**2 + error[1]**2 + error[2]**2)**0.5 < 0.02: - print(f" Reached above position at step {step}") - break - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 3: Move down to grasp - print("Moving to grasp position...") - for step in range(80): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] - 0.02 - eef_pos[2] - ] - - if (error[0]**2 + error[1]**2 + error[2]**2)**0.5 < 0.01: - print(f" Reached grasp position at step {step}") - break - - delta = [max(-0.2, min(0.2, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 4: Close gripper (minimal steps) - print("Closing gripper...") - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Phase 5: Lift with hard boundary - print("Lifting...") - TARGET_HEIGHT = 1.0 # RoboSuite success threshold - MAX_LIFT_HEIGHT = 1.0 # Hard boundary - - for step in range(100): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Success check - if cube_pos[2] > TARGET_HEIGHT: - success = sim.checkSuccess() - print(f"\nSUCCESS: {success}") - print(f"Cube height: {cube_pos[2]:.3f}m (> {TARGET_HEIGHT}m)") - print(f"Completed in {step} lift steps") - terminate simulation - - # Safety check - if cube_pos[2] > MAX_LIFT_HEIGHT: - print(f"\nMax height reached: {cube_pos[2]:.3f}m") - terminate simulation - - # Target: 15cm above initial - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.15 - eef_pos[2] - ] - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=1) - - print("Failed to reach target height") - terminate simulation +# # Clean approach using object reference +ego = new PandaRobot with behavior PickAndLift(cube, height=1.05) -# ego = new PandaRobot with behavior AdaptiveLiftBehavior() -ego = new UR5eRobot with behavior AdaptiveLiftBehavior() \ No newline at end of file +# Alternative: Custom composition +# behavior CustomLift(): +# do PickObject(cube) +# do LiftToHeight(1.05) +# if simulation().checkSuccess(): +# terminate simulation + +# ego = new PandaRobot with behavior CustomLift() \ No newline at end of file diff --git a/examples/robosuite/lift_custom_position.scenic b/examples/robosuite/lift_custom_position.scenic deleted file mode 100644 index c95dd97bc..000000000 --- a/examples/robosuite/lift_custom_position.scenic +++ /dev/null @@ -1,155 +0,0 @@ -# examples/robosuite/lift_custom_position.scenic -"""Lift task with custom cube position - demonstrates object positioning.""" - -param use_environment = "Lift" -model scenic.simulators.robosuite.model - -# Define cube class for Lift environment -class LiftCube(RoboSuiteObject): - """Cube in Lift environment.""" - _env_object_name: "cube" # Maps to RoboSuite's internal cube - width: 0.05 - length: 0.05 - height: 0.05 - color: (1, 0, 0) # Red - -# Create cube at custom position -# Default Lift places cube at (0, 0, ~0.82) -# Let's place it to the right and forward -cube = new LiftCube at (0.1, -0.1, 0.82) - -# Robot with modified lift behavior for new position -behavior CustomPositionLiftBehavior(): - print("=== LIFT WITH CUSTOM CUBE POSITION ===") - - # Wait for stabilization - wait - wait - - sim = simulation() - - # Print initial cube position - obs = sim._current_obs - if obs and 'cube_pos' in obs: - print(f"Initial cube position: {obs['cube_pos']}") - - # Phase 1: Open gripper - print("\nPhase 1: Opening gripper...") - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) - - # Phase 2: Move above cube - print("\nPhase 2: Moving above cube...") - for step in range(60): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Target is 10cm above cube - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.1 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if step % 10 == 0: - print(f" Step {step}: Moving to cube at ({cube_pos[0]:.3f}, {cube_pos[1]:.3f}, {cube_pos[2]:.3f})") - - if norm < 0.02: - print(f" Reached above position at step {step}") - break - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 3: Move down to cube - print("\nPhase 3: Moving to grasp position...") - for step in range(40): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Target is at cube center - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] - 0.02 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if norm < 0.01: - print(f" Reached grasp position at step {step}") - break - - delta = [max(-0.2, min(0.2, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 4: Close gripper - print("\nPhase 4: Closing gripper...") - for i in range(30): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Phase 5: Lift - print("\nPhase 5: Lifting...") - for step in range(60): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs: - wait - continue - - cube_height = obs['cube_pos'][2] - - if step % 10 == 0: - print(f" Step {step}: Cube height = {cube_height:.3f}") - - # Lift up - take OSCPositionAction(position_delta=[0, 0, 0.3], gripper=1) - - if cube_height > 0.9: # Success threshold - print(f" Lifted successfully at step {step}!") - break - - # Phase 6: Hold - print("\nPhase 6: Holding position...") - for i in range(50): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Check success - success = sim.checkSuccess() - print(f"\nTask Success: {success}") - - print("\nBehavior complete, continuing...") - while True: - wait - -ego = new PandaRobot with behavior CustomPositionLiftBehavior() - -# Monitor to show cube position -monitor CubePositionMonitor(): - print("\n=== Cube Position Monitor ===") - sim = simulation() - step = 0 - - while True: - obs = sim._current_obs - if obs and 'cube_pos' in obs: - if step % 20 == 0: - pos = obs['cube_pos'] - print(f"[Step {step:3d}] Cube at: ({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})") - step += 1 - wait - -require monitor CubePositionMonitor() - -terminate after 80 seconds \ No newline at end of file diff --git a/examples/robosuite/lift_task.scenic b/examples/robosuite/lift_task.scenic deleted file mode 100644 index 9b2fe7981..000000000 --- a/examples/robosuite/lift_task.scenic +++ /dev/null @@ -1,77 +0,0 @@ -"""Robosuite Lift task demonstration in Scenic.""" - -model scenic.simulators.robosuite.model - -arena = new Table at (0, 0, 0) - -# arena = new PegsArena - -# table = new PositionableTable at (0, 0, 0), -# facing 0 deg, -# with width 2, -# with length 1, -# with height 0.8 - -target_cube = new Cube at (0.5, -0.3, 0.825), - with color (0, 0, 1) - -# Create robot positioned next to table, facing it -ego = new PandaRobot at (0, -0.8, 0), - facing 90 deg # Face positive Y direction (toward table) - -# Create object on table surface (default table top is at z=0.8) -# target_cube = new Cube at (0, 0, 0.825), -# with width 0.05, -# with length 0.05, -# with height 0.05, -# with color (1, 0, 0) # Red - -# Define lift behavior -behavior LiftBehavior(): - # Move to pre-grasp position - take SetJointPositions([0, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.04, 0.04]) - for i in range(20): - wait - - # Open gripper - take SetJointPositions([0, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.08, 0.08]) - for i in range(10): - wait - - # Move down to object - take SetJointPositions([0.2, -0.3, 0, -1.8, 0, 1.4, 0.785, 0.08, 0.08]) - for i in range(20): - wait - - # Close gripper - take SetJointPositions([0.2, -0.3, 0, -1.8, 0, 1.4, 0.785, 0.01, 0.01]) - for i in range(10): - wait - - # Lift up - take SetJointPositions([0.2, -0.5, 0, -2.0, 0, 1.5, 0.785, 0.01, 0.01]) - for i in range(20): - wait - -# Assign behavior to ego robot -ego.behavior = LiftBehavior() - -# Monitor to track progress -monitor LiftMonitor(): - for i in range(30): - wait - joint_pos = ego.joint_positions - if joint_pos: - print(f"Step {i}: Gripper: {joint_pos[-2]:.3f}, {joint_pos[-1]:.3f}") - print(f"Step {i}: Cube at z={target_cube.position.z:.3f}") - -# Record data for visualization -record ego.joint_positions as robot_joints -record ego.position as robot_position -record target_cube.position as cube_position - -# Simulation parameters -param timestep = 0.1 -terminate after 30 steps - -require monitor LiftMonitor() \ No newline at end of file diff --git a/examples/robosuite/make_osc_test.scenic b/examples/robosuite/make_osc_test.scenic deleted file mode 100644 index 0dc52972b..000000000 --- a/examples/robosuite/make_osc_test.scenic +++ /dev/null @@ -1,189 +0,0 @@ -# examples/robosuite/make_osc_test.scenic -"""Test RoboSuite's make() function with OSC controller for Lift task.""" - - -model scenic.simulators.robosuite.model - -simulator RobosuiteSimulator( - render=True, - real_time=True, - speed=1.0, - use_environment="Lift" -) - -# cube = new Cube at (0.1, -0.1, 0.82) - -behavior WorkingLiftBehavior(): - print("[LIFT] Starting lift behavior") - - - wait - wait - - sim = simulation() - - - print("\n[LIFT] Phase 1: Opening gripper...") - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) - print("[LIFT] Gripper opened") - - - print("\n[LIFT] Phase 2: Moving above cube...") - moved_above = False - for step in range(100): - obs = sim._current_obs - if not obs: - print(f"[LIFT] Step {step}: No observation!") - wait - continue - - if 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - print(f"[LIFT] Step {step}: Missing data!") - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.1 - eef_pos[2] - ] - - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if step % 10 == 0: - print(f"[LIFT] Step {step}: Error = {norm:.3f}, EEF = {eef_pos}, Cube = {cube_pos}") - - if norm < 0.02: - print(f"[LIFT] Reached above position at step {step}") - moved_above = True - break - - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - if not moved_above: - print("[LIFT] Failed to reach above position!") - - - print("\n[LIFT] Phase 3: Moving to grasp position...") - moved_down = False - for step in range(80): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] - 0.02 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if step % 10 == 0: - print(f"[LIFT] Step {step}: Error = {norm:.3f}") - - if norm < 0.01: - print(f"[LIFT] Reached grasp position at step {step}") - moved_down = True - break - - delta = [max(-0.2, min(0.2, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - if not moved_down: - print("[LIFT] Failed to reach grasp position!") - - - print("\n[LIFT] Phase 4: Closing gripper...") - for i in range(30): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - print("[LIFT] Gripper closed") - - - print("\n[LIFT] Phase 5: Lifting...") - lifted = False - initial_cube_height = None - for step in range(100): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - - if initial_cube_height is None: - initial_cube_height = cube_pos[2] - print(f"[LIFT] Initial cube height: {initial_cube_height:.3f}") - - - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.15 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if step % 10 == 0: - cube_lift = cube_pos[2] - initial_cube_height - print(f"[LIFT] Step {step}: Error = {norm:.3f}, Cube lifted by {cube_lift:.3f}m") - - if norm < 0.02: - print(f"[LIFT] Reached lift position at step {step}") - lifted = True - break - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=1) - - if not lifted: - print("[LIFT] Failed to reach lift position!") - - - print("\n[LIFT] Phase 6: Holding position...") - for i in range(100): - if i % 20 == 0: - obs = sim._current_obs - if obs and 'cube_pos' in obs and initial_cube_height is not None: - cube_lift = obs['cube_pos'][2] - initial_cube_height - print(f"[LIFT] Holding step {i}/100, cube height delta: {cube_lift:.3f}m") - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - - obs = sim._current_obs - if obs and 'cube_pos' in obs and initial_cube_height is not None: - final_cube_height = obs['cube_pos'][2] - total_lift = final_cube_height - initial_cube_height - success = total_lift > 0.05 # Success if lifted more than 5cm - print(f"\n[LIFT] Final cube lift: {total_lift:.3f}m") - print(f"[LIFT] Task Success: {success}") - - print(f"\n[LIFT] RoboSuite Success Check: {sim.checkSuccess()}") - print("[LIFT] Behavior complete, continuing...") - - - step_count = 0 - while True: - wait - step_count += 1 - if step_count % 100 == 0: - print(f"[LIFT] Still running... (step {step_count})") - -ego = new PandaRobot with behavior WorkingLiftBehavior() - -terminate after 150 seconds \ No newline at end of file diff --git a/examples/robosuite/tower_collapse.scenic b/examples/robosuite/tower_collapse.scenic deleted file mode 100644 index e04864390..000000000 --- a/examples/robosuite/tower_collapse.scenic +++ /dev/null @@ -1,83 +0,0 @@ -# param use_table = False -# table = new Table at (20, 20, 20) - - -# examples/robosuite/tower_collapse.scenic -"""Tower Collapse Simulation - Fixed Version -Stops when tower falls (3+ cylinders down) or stabilizes. -""" - -model scenic.simulators.robosuite.model - -class Cylinder(Object): - width: 0.08 - length: 0.08 - height: 0.12 - color: Uniform((1,0,0), (0,1,0), (0,0,1), (1,1,0), (1,0,1)) - -# Build tower with random offsets for instability -cylinder1 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.10) -cylinder2 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.221) -cylinder3 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.342) -cylinder4 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.463) -cylinder5 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.584) -cylinder6 = new Cylinder at (Range(-0.01, 0.01), Range(-0.01, 0.01), 0.705) - -tower_cylinders = [cylinder1, cylinder2, cylinder3, cylinder4, cylinder5, cylinder6] - -# Set ego to bottom cylinder -ego = cylinder1 - -# Records -record [c.position[0] for c in tower_cylinders] as cylinder_x_positions -record [c.position[2] for c in tower_cylinders] as cylinder_heights -record max([abs(c.position[0]) for c in tower_cylinders]) as tower_sway -record len([c for c in tower_cylinders if c.position[2] < 0.05]) as fallen_count - -# Monitor for collapse and stability -monitor TowerMonitor(): - print(f"=== Tower Collapse Monitor Started ===") - step_count = 0 - stable_steps = 0 - prev_positions = None - - while True: - fallen = len([c for c in tower_cylinders if c.position[2] < 0.05]) - sway = max([abs(c.position[0]) for c in tower_cylinders]) - - # Check stability - if positions haven't changed much - current_positions = [(c.position[0], c.position[2]) for c in tower_cylinders] - if prev_positions: - max_change = max([abs(curr[0]-prev[0]) + abs(curr[1]-prev[1]) - for curr, prev in zip(current_positions, prev_positions)]) - if max_change < 0.001: # Very little movement - stable_steps += 1 - else: - stable_steps = 0 - prev_positions = current_positions - - # Print status every 10 steps - if step_count % 10 == 0: - print(f"[Step {step_count:3d}] Fallen={fallen}/6, Sway={sway:.3f}, Stable={stable_steps}") - if step_count % 30 == 0: # Detailed print every 30 steps - for i, c in enumerate(tower_cylinders): - print(f" Cylinder{i+1}: x={c.position[0]:.3f}, z={c.position[2]:.3f}") - - # Terminate on collapse (3+ fallen) - if fallen >= 3: - print(f"\n!!! TOWER COLLAPSED at step {step_count}: {fallen}/6 cylinders down !!!") - terminate - - # Terminate if stable for 20 steps - if stable_steps >= 20: - print(f"\n=== Tower STABILIZED at step {step_count} ===") - terminate - - step_count += 1 - wait - -require monitor TowerMonitor() - -# Termination conditions -terminate when len([c for c in tower_cylinders if c.position[2] < 0.05]) >= 3 -# terminate after 50 steps # Max simulation length \ No newline at end of file diff --git a/examples/robosuite/ur5e_lift_test.scenic b/examples/robosuite/ur5e_lift_test.scenic deleted file mode 100644 index 6542d4334..000000000 --- a/examples/robosuite/ur5e_lift_test.scenic +++ /dev/null @@ -1,159 +0,0 @@ -# examples/robosuite/ur5e_lift_test.scenic -"""Test UR5e robot cube pickup with optimized parameters.""" - -param use_environment = "Lift" - -model scenic.simulators.robosuite.model - -simulator RobosuiteSimulator( - render=True, - real_time=True, - speed=1.0, - use_environment=globalParameters.use_environment -) - -# Define UR5e robot class -class UR5eRobot(Robot): - """UR5e robot configuration.""" - robot_type: "UR5e" - initial_qpos: None # Use default - -behavior UR5eLiftBehavior(): - """Optimized lift behavior for UR5e robot.""" - - print("=== UR5e OPTIMIZED CUBE PICKUP ===") - - # Wait for stabilization - wait - wait - - sim = simulation() - - # Phase 1: Open gripper (20 steps) - print("\nOpening gripper...") - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) - - # Phase 2: Move above cube (40 steps max) - print("\n1. Moving above cube...") - for step in range(40): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Target is 10cm above cube - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.1 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if norm < 0.02: - print(f" Reached above position at step {step}") - break - - # Scale and clip - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 3: Move down to cube (30 steps max) - print("2. Moving to grasp position...") - for step in range(30): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Target is 2.5cm below cube center - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] - 0.025 - eef_pos[2] - ] - - norm = (error[0]**2 + error[1]**2 + error[2]**2)**0.5 - - if norm < 0.01: - print(f" Reached grasp position at step {step}") - break - - # Slower approach - delta = [max(-0.1, min(0.1, e * 2.0)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Phase 4: Grasp (20 steps) - print("3. Closing gripper...") - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Phase 5: Lift (50 steps max) - print("4. Lifting...") - initial_cube_height = None - success_achieved = False - - for step in range(50): - obs = sim._current_obs - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait - continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - - # Track initial height - if initial_cube_height is None: - initial_cube_height = cube_pos[2] - print(f" Initial cube height: {initial_cube_height:.3f}") - - # Target is 25cm above original position - error = [ - cube_pos[0] - eef_pos[0], - cube_pos[1] - eef_pos[1], - cube_pos[2] + 0.25 - eef_pos[2] - ] - - delta = [max(-0.25, min(0.25, e * 2.5)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=1) - - # Check success - if sim.checkSuccess() and step > 10 and not success_achieved: - print(f" Success achieved at lift step {step}!") - success_achieved = True - - # Phase 6: Hold position (30 steps) - print("5. Holding position...") - for i in range(30): - if i % 10 == 0: - obs = sim._current_obs - if obs and 'cube_pos' in obs: - print(f" Cube height: {obs['cube_pos'][2]:.3f} (needs > 0.84)") - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Final check - obs = sim._current_obs - if obs and 'cube_pos' in obs: - print(f"\nFinal Success: {sim.checkSuccess()}") - print(f"Cube height: {obs['cube_pos'][2]:.3f} (needs > 0.84)") - - print("\nBehavior complete, continuing...") - - # Keep running - while True: - try: - wait - except: - print("\nEpisode terminated. Exiting...") - break - -ego = new UR5eRobot with behavior UR5eLiftBehavior() - -terminate after 100 seconds # Longer time for UR5e \ No newline at end of file diff --git a/examples/robosuite/xml_test.scenic b/examples/robosuite/xml_test.scenic deleted file mode 100644 index dc6471e88..000000000 --- a/examples/robosuite/xml_test.scenic +++ /dev/null @@ -1,20 +0,0 @@ -# examples/robosuite/xml_test.scenic -model scenic.simulators.robosuite.model - -# Place table at ground level (z=0) - -table2 = new PositionableTable at (0.5, 0, 0), - with width 1, - with length 0.6, - with height 0.6 - -# Cube on table - table height is 0.8, so cube should be at 0.8 + half cube height - -cube1 = new Cube at (-0.5, 0, 0.825), - with color (1, 0, 0) - -cube2 = new Cube at (0.5, 0, 0.625), - with color (0, 0, 1) - -ego = cube1 -terminate after 50 steps \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/__init__.py b/src/scenic/simulators/robosuite/__init__.py index 0f82929f6..287e36abe 100644 --- a/src/scenic/simulators/robosuite/__init__.py +++ b/src/scenic/simulators/robosuite/__init__.py @@ -1,11 +1,5 @@ -"""Scenic world model and simulator interface for RoboSuite. +"""RoboSuite simulator interface for Scenic.""" -This package provides: -- RobosuiteSimulator: Main simulator interface for Scenic -- World model definitions in model.scenic -- XML building utilities for custom objects -""" +from .simulator import RobosuiteSimulator, RobosuiteSimulation -from .simulator import RobosuiteSimulator - -__all__ = ["RobosuiteSimulator"] \ No newline at end of file +__all__ = ['RobosuiteSimulator', 'RobosuiteSimulation'] \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index 7ecdad4e0..bba0f7c62 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -8,10 +8,10 @@ param env_config = {} param controller_config = None param camera_view = None param render = True -param real_time = True # Default: real-time ON +param real_time = True param speed = 1.0 -# Simulator - uses parameters from above +# Simulator simulator RobosuiteSimulator( render=globalParameters.render, real_time=globalParameters.real_time, @@ -22,220 +22,240 @@ simulator RobosuiteSimulator( camera_view=globalParameters.camera_view ) +# Default values dictionary +DEFAULTS = { + # Object properties + 'object_size': 0.05, + '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), + 'default_color': (0.5, 0.5, 0.5), + + # Arena properties + 'table_height': 0.8, + 'table_width': 1.0, + 'table_length': 0.8, + + # Robot properties + 'robot_width': 0.2, + 'robot_length': 0.2, + 'robot_height': 0.5, + + # Control parameters + 'control_gain': 3.0, + 'control_limit': 0.3, + 'position_tolerance': 0.02, + 'height_tolerance': 0.02, + 'gripper_open_steps': 20, + 'gripper_close_steps': 30, + 'max_control_steps': 100, + 'max_lift_steps': 200 +} + # Base class class RoboSuiteObject(Object): - 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) - shape: BoxShape() # Default shape - -# Objects -class XMLObject(RoboSuiteObject): - """Object from XML file or string.""" - xml_path: None - xml_string: None - material: {} + density: DEFAULTS['density'] + friction: DEFAULTS['friction'] + solref: DEFAULTS['solref'] + solimp: DEFAULTS['solimp'] + shape: BoxShape() class Cube(RoboSuiteObject): - width: 0.05 - length: 0.05 - height: 0.05 - color: (0.8, 0.2, 0.2) + """Cubic object with uniform dimensions.""" + width: DEFAULTS['object_size'] + length: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] + color: DEFAULTS['default_color'] +""" +# currently only Cube works within preconfigured env, we can't change the type of object, WIP, will add support for them soon class Ball(RoboSuiteObject): - width: 0.05 - color: (0.2, 0.8, 0.2) + """Spherical object.""" + width: DEFAULTS['object_size'] + color: DEFAULTS['default_color'] class Cylinder(RoboSuiteObject): - width: 0.05 - height: 0.1 - color: (0.2, 0.2, 0.8) + """Cylindrical object.""" + width: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] * 2 + color: DEFAULTS['default_color'] +""" + +# Environment-specific wrapper +class EnvironmentObject(RoboSuiteObject): + """Base class for objects in RoboSuite environments.""" + envObjectName: None + allowCollisions: True -# Environment-specific objects +# Convenience classes for specific environments class LiftCube(Cube): """Cube in Lift environment.""" - envObjectName: "cube" # Public attribute + envObjectName: "cube" allowCollisions: True class StackCubeA(Cube): """First cube in Stack environment.""" - envObjectName: "cubeA" # Public attribute + envObjectName: "cubeA" allowCollisions: True class StackCubeB(Cube): """Second cube in Stack environment.""" - envObjectName: "cubeB" # Public attribute + envObjectName: "cubeB" allowCollisions: True # Robots class Robot(RoboSuiteObject): - robot_type: "Panda" + """Base robot class.""" + robot_type: "Panda" # Default type gripper_type: "default" controller_config: None initial_qpos: None base_type: "default" - width: 0.2 - length: 0.2 - height: 0.5 + width: DEFAULTS['robot_width'] + length: DEFAULTS['robot_length'] + height: DEFAULTS['robot_height'] + # Dynamic properties joint_positions[dynamic]: [] eef_pos[dynamic]: [0, 0, 0] gripper_state[dynamic]: [0, 0] +# Generic robot templates class PandaRobot(Robot): + """Franka Emika Panda robot.""" robot_type: "Panda" gripper_type: "PandaGripper" - initial_qpos: [0, -0.785, 0, -2.356, 0, 1.571, 0.785, 0.04, 0.04] - height: 0.9 class SawyerRobot(Robot): + """Rethink Sawyer robot.""" robot_type: "Sawyer" gripper_type: "RethinkGripper" - initial_qpos: [0, -0.785, 0, 1.571, 0, -0.785, 0] - height: 1.0 class UR5eRobot(Robot): + """Universal Robots UR5e.""" robot_type: "UR5e" gripper_type: "Robotiq85Gripper" - initial_qpos: None # Use default - height: 0.85 - -# Arenas -class Arena(RoboSuiteObject): - pass - -class EmptyArena(Arena): - pass - -class TableArena(Arena): - table_height: 0.8 - table_width: 1.0 - table_length: 0.8 - -class BinsArena(Arena): - bin_size: 0.3 - bin_height: 0.1 - -class PegsArena(Arena): - board_size: 0.3 - peg_radius: 0.015 - -class WipeArena(Arena): - table_height: 0.8 - marker_size: 0.08 - -class CustomArena(Arena): - xml_string: None - xml_path: None - objects: [] - -# Furniture -class Table(RoboSuiteObject): - width: 1.0 - length: 0.8 - height: 0.8 - -class PositionableTable(RoboSuiteObject): - width: 1.0 - length: 0.8 - height: 0.8 # Actions import scenic.core.dynamics as dynamics +import numpy as np class SetJointPositions(dynamics.Action): - """Set robot joint positions.""" + """Set robot joint positions. + + Args: + positions: Target joint positions + """ def __init__(self, positions): self.positions = positions def applyTo(self, agent, sim): - pass + """Apply joint position control to the robot.""" + if hasattr(sim, 'robots') and agent in sim.robots: + robot_idx = sim.robots.index(agent) + if robot_idx < len(sim.robosuite_env.robots): + action = np.array(self.positions) + sim.pending_robot_action = action class OSCPositionAction(dynamics.Action): - """Operational Space Control for end-effector.""" + """Operational Space Control for end-effector. + + Args: + position_delta: Cartesian position change [x, y, z] + orientation_delta: Orientation change [roll, pitch, yaw] + gripper: Gripper command (-1=open, 1=close) + """ def __init__(self, position_delta=None, orientation_delta=None, gripper=None): - self.position_delta = position_delta - self.orientation_delta = orientation_delta - self.gripper = gripper + self.position_delta = position_delta if position_delta else [0, 0, 0] + self.orientation_delta = orientation_delta if orientation_delta else [0, 0, 0] + self.gripper = gripper if gripper is not None else 0 def applyTo(self, agent, sim): - pass + """Apply OSC control to the robot.""" + if hasattr(sim, 'robots') and agent in sim.robots: + robot_idx = sim.robots.index(agent) + if robot_idx < len(sim.robosuite_env.robots): + # Build action array based on controller type + if hasattr(sim, 'controller_type') and sim.controller_type == 'JOINT_POSITION': + action = np.zeros(sim.action_dim) + action[:3] = self.position_delta + else: + # Default OSC action [position(3), orientation(3), gripper(1)] + action = np.zeros(7) + action[:3] = self.position_delta + action[3:6] = self.orientation_delta + action[6] = self.gripper + + sim.pending_robot_action = action -# Behaviors -behavior RobustLiftBehavior(): - """Pick and place behavior using visual servoing.""" - wait; wait # Initialize - - sim = simulation() - - # Open gripper - for i in range(20): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=-1) - - # Move above object - for step in range(100): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - target = [cube_pos[0], cube_pos[1], cube_pos[2] + 0.1] - error = [target[i] - eef_pos[i] for i in range(3)] - - if sum(e**2 for e in error)**0.5 < 0.02: - break - - delta = [max(-0.3, min(0.3, e * 3)) for e in error] - take OSCPositionAction(position_delta=delta, gripper=-1) - - # Move down - for step in range(80): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - target = [cube_pos[0], cube_pos[1], cube_pos[2] - 0.02] - error = [target[i] - eef_pos[i] for i in range(3)] +# Behavior Library - Reusable components +behavior OpenGripper(steps=DEFAULTS['gripper_open_steps']): + """Open gripper over multiple steps.""" + for _ in range(steps): + take OSCPositionAction(gripper=-1) + +behavior CloseGripper(steps=DEFAULTS['gripper_close_steps']): + """Close gripper over multiple steps.""" + for _ in range(steps): + take OSCPositionAction(gripper=1) + +behavior MoveToPosition(target_pos, + tolerance=DEFAULTS['position_tolerance'], + max_steps=DEFAULTS['max_control_steps'], + gain=DEFAULTS['control_gain']): + """Move end-effector to target position.""" + for _ in range(max_steps): + eef_pos = self.eef_pos + error = [target_pos[i] - eef_pos[i] for i in range(3)] - if sum(e**2 for e in error)**0.5 < 0.01: - break + if sum(e**2 for e in error)**0.5 < tolerance: + return - delta = [max(-0.2, min(0.2, e * 3)) for e in error] + limit = DEFAULTS['control_limit'] + delta = [max(-limit, min(limit, e * gain)) for e in error] take OSCPositionAction(position_delta=delta, gripper=-1) - - # Close gripper - for i in range(30): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - # Lift - for step in range(100): - obs = sim.getCurrentObservation() - if not obs or 'cube_pos' not in obs or 'robot0_eef_pos' not in obs: - wait; continue - - cube_pos = obs['cube_pos'] - eef_pos = obs['robot0_eef_pos'] - target = [cube_pos[0], cube_pos[1], cube_pos[2] + 0.15] - error = [target[i] - eef_pos[i] for i in range(3)] + +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=DEFAULTS['max_lift_steps']): + """Lift to absolute height.""" + for _ in range(max_steps): + eef_pos = self.eef_pos + error = [0, 0, target_height - eef_pos[2]] - if sum(e**2 for e in error)**0.5 < 0.02: - break + if abs(error[2]) < DEFAULTS['height_tolerance']: + return - delta = [max(-0.3, min(0.3, e * 3)) for e in error] + limit = DEFAULTS['control_limit'] + gain = DEFAULTS['control_gain'] + delta = [max(-limit, min(limit, e * gain)) for e in error] take OSCPositionAction(position_delta=delta, gripper=1) - - # Hold - for i in range(100): - take OSCPositionAction(position_delta=[0, 0, 0], gripper=1) - - success = sim.checkSuccess() - - while True: - wait -behavior SimpleLiftBehavior(): - do RobustLiftBehavior() \ No newline at end of file +# Object-aware behaviors +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) + + if simulation().checkSuccess(): + terminate simulation \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py index b946224b5..5ba515b25 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -1,10 +1,9 @@ -"""RoboSuite Simulator for Scenic - Version 3.2 (Clean)""" +"""RoboSuite Simulator interface for Scenic.""" import numpy as np import mujoco import time -import math -from typing import Dict, List, Any +from typing import Dict, List, Any, Optional try: import robosuite as suite @@ -15,10 +14,44 @@ from scenic.core.simulators import Simulation, Simulator from scenic.core.vectors import Vector -from .utils import scenic_to_rgba + +# Constants +DEFAULT_PHYSICS_TIMESTEP = 0.002 +DEFAULT_TIMESTEP = 0.01 +DEFAULT_ACTION_DIM = 7 +PHYSICS_SETTLE_STEPS = 10 +WINDOW_CLOSE_DELAY = 2 + +# RoboSuite naming patterns +ROBOSUITE_SUFFIXES = { + 'body': ['_main', '_body', ''], + 'joint': '_joint0' +} + +# Camera view mapping +CAMERA_VIEWS = { + "frontview": 0, + "birdview": 1, + "agentview": 2, + "sideview": 3, + "robot0_robotview": 4, + "robot0_eye_in_hand": 5 +} class RobosuiteSimulator(Simulator): + """Simulator interface for RoboSuite. + + Args: + render: Enable visualization + real_time: Run simulation in real-time + speed: Simulation speed multiplier + use_environment: RoboSuite environment name + env_config: Additional environment configuration + controller_config: Robot controller configuration + camera_view: Camera perspective for rendering + """ + def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, env_config=None, controller_config=None, camera_view=None): super().__init__() @@ -33,24 +66,24 @@ def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, self.camera_view = camera_view def createSimulation(self, scene, **kwargs): + """Create a RoboSuite simulation instance.""" return RobosuiteSimulation(scene, self.render, self.real_time, self.speed, self.use_environment, self.env_config, self.controller_config, self.camera_view, **kwargs) class RobosuiteSimulation(Simulation): - ENV_OBJECTS = {"Lift": {"cube": "cube_main"}, "Stack": {"cubeA": "cubeA_main", "cubeB": "cubeB_main"}} - CAMERA_VIEWS = { - "frontview": 0, - "birdview": 1, - "agentview": 2, - "sideview": 3, - "robot0_robotview": 4, - "robot0_eye_in_hand": 5 - } + """Simulation instance for RoboSuite. - def __init__(self, scene, render, real_time, speed, use_environment=None, - env_config=None, controller_config=None, camera_view=None, **kwargs): + Manages the interface between Scenic's simulation loop and RoboSuite's + environment, handling object tracking, action execution, and property updates. + """ + + def __init__(self, scene, render: bool, real_time: bool, speed: float, + use_environment: Optional[str] = None, + env_config: Optional[Dict] = None, + controller_config: Optional[Dict] = None, + camera_view: Optional[str] = None, **kwargs): self.render = render self.real_time = real_time self.speed = speed @@ -61,27 +94,33 @@ def __init__(self, scene, render, real_time, speed, use_environment=None, self.robosuite_env = None self.model = None self.data = None - self.viewer = None - # Mapping objects to RoboSuite IDs (instead of storing on objects) + # Object tracking self.body_id_map = {} - self.object_names = {} # Scenic object -> RoboSuite name + self.object_names = {} self.robots = [] self.prev_positions = {} - self.pending_actions = {} self._current_obs = None - self.timestep = kwargs.get('timestep') or 0.01 - self.physics_timestep = 0.002 + # Action handling + self.pending_robot_action = None + self.action_dim = DEFAULT_ACTION_DIM + self.controller_type = None + + # Timing + self.timestep = kwargs.get('timestep') if kwargs.get('timestep') is not None else DEFAULT_TIMESTEP + self.physics_timestep = kwargs.get('physics_timestep') if kwargs.get('physics_timestep') is not None else DEFAULT_PHYSICS_TIMESTEP self.physics_steps = int(self.timestep / self.physics_timestep) self.agents = [] + super().__init__(scene, **kwargs) def setup(self): - super().setup() # Call parent setup first to populate objects + """Initialize the RoboSuite environment.""" + super().setup() if not self.use_environment: - raise ValueError("Only pre-configured RoboSuite environments are supported") + raise ValueError("Environment name must be specified via 'use_environment' parameter") # Get robots from scene robots = [obj for obj in self.objects if hasattr(obj, 'robot_type')] @@ -95,22 +134,29 @@ def setup(self): 'controller_configs': self.controller_config } - # Prepare robots argument - robot_arg = "Panda" # Default robot + # Prepare robots + robot_arg = "Panda" # Default if robots: robot_arg = [r.robot_type for r in robots] if len(robots) > 1 else robots[0].robot_type - # Create RoboSuite environment + # Create environment self.robosuite_env = suite.make(self.use_environment, robots=robot_arg, **config) self._current_obs = self.robosuite_env.reset() self.model = self.robosuite_env.sim.model._model self.data = self.robosuite_env.sim.data._data - # Set camera view + # Detect controller type and action dimension + if robots and self.robosuite_env.robots: + first_robot = self.robosuite_env.robots[0] + if hasattr(first_robot, 'controller'): + controller_name = type(first_robot.controller).__name__ + self.controller_type = controller_name + if hasattr(first_robot.controller, 'control_dim'): + self.action_dim = first_robot.controller.control_dim + + # Set camera if self.render and self.camera_view is not None: - camera_id = self.camera_view - if isinstance(self.camera_view, str): - camera_id = self.CAMERA_VIEWS.get(self.camera_view.lower(), 0) + 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) @@ -119,28 +165,38 @@ def setup(self): self.robots.append(r) self.object_names[r] = f"robot{i}" - # Apply initial positions for environment objects + # Apply initial positions self._apply_initial_positions() - # Setup body mapping - if self.use_environment in self.ENV_OBJECTS: - for name, body in self.ENV_OBJECTS[self.use_environment].items(): - id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body) - if id != -1: - self.body_id_map[name] = id - self.prev_positions[name] = self.data.xpos[id].copy() + # Setup body mapping for environment objects + self._setup_body_mapping() # Identify agents for obj in self.objects: if hasattr(obj, 'behavior') and obj.behavior: self.agents.append(obj) + def _setup_body_mapping(self): + """Map environment objects to MuJoCo body IDs.""" + for obj in self.objects: + if hasattr(obj, 'envObjectName'): + # Try to find body with standard naming patterns + for suffix in ROBOSUITE_SUFFIXES['body']: + body_name = f"{obj.envObjectName}{suffix}" + try: + body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body_name) + if body_id != -1: + self.body_id_map[obj.envObjectName] = body_id + self.prev_positions[obj.envObjectName] = self.data.xpos[body_id].copy() + break + except: + continue + def _apply_initial_positions(self): """Apply initial positions for environment objects.""" - # Apply positions for objects with envObjectName for obj in self.objects: if hasattr(obj, 'envObjectName') and hasattr(obj, 'position'): - joint_name = f"{obj.envObjectName}_joint0" + joint_name = f"{obj.envObjectName}{ROBOSUITE_SUFFIXES['joint']}" try: qpos = np.concatenate([ [obj.position.x, obj.position.y, obj.position.z], @@ -148,58 +204,56 @@ def _apply_initial_positions(self): ]) self.robosuite_env.sim.data.set_joint_qpos(joint_name, qpos) except Exception as e: - print(f"Warning: Could not set joint position for {obj.envObjectName}: {e}") + raise RuntimeError( + f"Failed to set position for object '{obj.envObjectName}'. " + f"Ensure the object exists in the '{self.use_environment}' environment. " + f"Error: {e}" + ) # Update physics self.robosuite_env.sim.forward() # Let physics settle - for _ in range(10): - self.robosuite_env.step(np.zeros(7)) + for _ in range(PHYSICS_SETTLE_STEPS): + self.robosuite_env.step(np.zeros(self.action_dim)) # Get fresh observations self._current_obs = self.robosuite_env._get_observations() def createObjectInSimulator(self, obj): - # For pre-configured environments, objects are already created + """Required by Scenic's Simulator interface but not used. + + RoboSuite creates all objects at environment initialization, + so dynamic object creation is not supported. + """ pass - def executeActions(self, allActions): + def executeActions(self, allActions: Dict[Any, List]) -> None: + """Execute actions by calling their applyTo methods.""" super().executeActions(allActions) - self.pending_actions = {} + # Clear pending actions + self.pending_robot_action = None + + # Process actions for each agent for agent in self.agents: if agent in allActions and allActions[agent]: for action in allActions[agent]: if action and hasattr(action, 'applyTo'): - # Store action for this agent - if hasattr(action, 'position_delta'): - self.pending_actions[agent] = ('osc', action) - elif hasattr(action, 'positions'): - self.pending_actions[agent] = ('joint', action.positions) + action.applyTo(agent, self) + break # One action per agent per timestep def step(self): - # Check if episode is done + """Step the simulation forward one timestep.""" if hasattr(self, '_done') and self._done: return # Store previous positions - for name, id in self.body_id_map.items(): - self.prev_positions[name] = self.data.xpos[id].copy() + for name, body_id in self.body_id_map.items(): + self.prev_positions[name] = self.data.xpos[body_id].copy() - # Execute actions - action = np.zeros(7) - for agent, (action_type, data) in self.pending_actions.items(): - if action_type == 'osc': - if hasattr(data, 'position_delta') and data.position_delta: - action[:3] = data.position_delta - if hasattr(data, 'orientation_delta') and data.orientation_delta: - action[3:6] = data.orientation_delta - if hasattr(data, 'gripper') and data.gripper is not None: - action[6] = data.gripper - else: - action = np.array(data) - break # RoboSuite only supports one robot action per step + # Use pending action or zeros + action = self.pending_robot_action if self.pending_robot_action is not None else np.zeros(self.action_dim) # Step simulation for _ in range(self.physics_steps): @@ -211,14 +265,17 @@ def step(self): if done: break - self.pending_actions = {} + # Clear pending actions + self.pending_robot_action = None - def getProperties(self, obj, properties): + 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': - # Check if it's an environment object + # For environment objects, get from observations if hasattr(obj, 'envObjectName') and self._current_obs: obs_key = f"{obj.envObjectName}_pos" if obs_key in self._current_obs: @@ -229,51 +286,43 @@ def getProperties(self, obj, properties): else: values[prop] = obj.position - elif prop == 'joint_positions' and obj in self.robots: - # Get joint positions for robots - robot_idx = self.robots.index(obj) + elif prop == 'joint_positions' and robot_idx is not None: if robot_idx < len(self.robosuite_env.robots): positions = self.robosuite_env.robots[robot_idx]._joint_positions - values[prop] = list(positions) # Convert numpy array to list + values[prop] = list(positions) else: values[prop] = [] - elif prop == 'eef_pos' and obj in self.robots and self._current_obs: - pos = self._current_obs.get('robot0_eef_pos', [0, 0, 0]) - values[prop] = list(pos) # Convert numpy array to list + elif prop == 'eef_pos' and robot_idx is not None and self._current_obs: + pos = self._current_obs.get(f'robot{robot_idx}_eef_pos', [0, 0, 0]) + values[prop] = list(pos) - elif prop == 'gripper_state' and obj in self.robots and self._current_obs: - state = self._current_obs.get('robot0_gripper_qpos', [0, 0]) - values[prop] = list(state) # Convert numpy array to list + elif prop == 'gripper_state' and robot_idx is not None and self._current_obs: + state = self._current_obs.get(f'robot{robot_idx}_gripper_qpos', [0, 0]) + values[prop] = list(state) else: - # Default to object's own property values[prop] = getattr(obj, prop, None) return values - def getCurrentObservation(self): - """Public method to get current observations.""" + def getCurrentObservation(self) -> Optional[Dict]: + """Get current observation dictionary.""" return self._current_obs - def checkSuccess(self): + def checkSuccess(self) -> bool: + """Check if task is successfully completed.""" return self.robosuite_env._check_success() if hasattr(self.robosuite_env, '_check_success') else False def destroy(self): + """Clean up simulation resources.""" if self.render and self.robosuite_env: - print("Simulation complete. Window will close in 2 seconds...") - time.sleep(2) + print(f"Simulation complete. Window will close in {WINDOW_CLOSE_DELAY} seconds...") + time.sleep(WINDOW_CLOSE_DELAY) try: if self.robosuite_env: self.robosuite_env.close() self.robosuite_env = None except Exception as e: - print(f"Warning: Error closing RoboSuite environment: {e}") - - try: - if self.viewer: - self.viewer.close() - self.viewer = None - except Exception as e: - print(f"Warning: Error closing viewer: {e}") \ No newline at end of file + print(f"Warning: Error closing RoboSuite environment: {e}") \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/utils.py b/src/scenic/simulators/robosuite/utils.py deleted file mode 100644 index 8dcb2e32c..000000000 --- a/src/scenic/simulators/robosuite/utils.py +++ /dev/null @@ -1,11 +0,0 @@ -"""Utility functions for RoboSuite-Scenic integration.""" - -from typing import List - - -def scenic_to_rgba(color, alpha=1.0) -> List[float]: - """Convert Scenic color to RGBA format.""" - if hasattr(color, '__iter__') and len(color) >= 3: - rgba = list(color[:4]) if len(color) >= 4 else list(color[:3]) + [alpha] - return [float(x) for x in rgba] - return [0.5, 0.5, 0.5, float(alpha)] \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/xml_builder.py b/src/scenic/simulators/robosuite/xml_builder.py deleted file mode 100644 index a929120f2..000000000 --- a/src/scenic/simulators/robosuite/xml_builder.py +++ /dev/null @@ -1,115 +0,0 @@ -# src/scenic/simulators/robosuite/xml_builder.py -"""XML builder for RoboSuite-Scenic integration.""" - -import os -import tempfile -import xml.etree.ElementTree as ET -from robosuite.models.objects import MujocoObject -from robosuite.models.arenas import Arena -from robosuite.utils.mjcf_utils import array_to_string, xml_path_completion - - -class XMLModifiableObject(MujocoObject): - """MujocoObject that can be modified at runtime.""" - - def __init__(self, name, xml_path=None, xml_string=None, **kwargs): - self.name = self.naming_prefix = name - self.obj_type = kwargs.get('obj_type', 'all') - - if xml_string: - self.root = ET.fromstring(xml_string) - self.worldbody = self.root.find("worldbody") - self.asset = self.root.find("asset") - else: - super().__init__(xml_path_completion(xml_path)) - - def get_obj(self): - """Get the XML tree with name prefix.""" - from robosuite.utils.mjcf_utils import add_prefix - add_prefix(self.root, prefix=self.naming_prefix) - return self.root - - -class RoboSuiteXMLBuilder: - """Builder for creating RoboSuite objects from Scenic specifications.""" - - def create_scenic_object(self, scenic_obj): - """Create RoboSuite object from Scenic object.""" - name = f"{type(scenic_obj).__name__}_{id(scenic_obj)}" - - # XML-based object - if hasattr(scenic_obj, 'xml_path') and scenic_obj.xml_path: - obj = XMLModifiableObject(name, xml_path=scenic_obj.xml_path) - elif hasattr(scenic_obj, 'xml_string') and scenic_obj.xml_string: - obj = XMLModifiableObject(name, xml_string=scenic_obj.xml_string) - else: - # Procedural object - obj = XMLModifiableObject(name, xml_string=self._build_procedural_xml(scenic_obj, name)) - - # Apply properties - self._apply_properties(obj, scenic_obj) - return obj - - def create_arena_from_xml(self, xml_source, is_string=True): - """Create arena from XML string or path.""" - with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f: - if is_string: - f.write(xml_source) - else: - with open(xml_path_completion(xml_source)) as src: - f.write(src.read()) - temp_path = f.name - - try: - return Arena(temp_path) - finally: - os.unlink(temp_path) - - def _build_procedural_xml(self, scenic_obj, name): - """Build XML for geometric objects.""" - obj_type = type(scenic_obj).__name__.lower() - - # Geometry - if 'ball' in obj_type or 'sphere' in obj_type: - geom_type, size_str = "sphere", str(scenic_obj.width/2) - elif 'cylinder' in obj_type: - geom_type, size_str = "cylinder", f"{scenic_obj.width/2} {scenic_obj.height/2}" - else: # box/cube - geom_type, size_str = "box", f"{scenic_obj.width/2} {scenic_obj.length/2} {scenic_obj.height/2}" - - # Properties - rgba = array_to_string(list(getattr(scenic_obj, 'color', [0.5, 0.5, 0.5])) + [1.0]) - density = getattr(scenic_obj, 'density', 1000) - friction = array_to_string(getattr(scenic_obj, 'friction', [1.0, 0.005, 0.0001])) - - return f""" - - - - - - - - """ - - def _apply_properties(self, obj, scenic_obj): - """Apply Scenic properties to object.""" - # Color - if hasattr(scenic_obj, 'color'): - rgba = array_to_string(list(scenic_obj.color[:3]) + [1.0]) - for geom in obj.worldbody.findall(".//geom[@group='1']") or []: - geom.set("rgba", rgba) - if "material" in geom.attrib: - del geom.attrib["material"] - - # Physics - if hasattr(scenic_obj, 'density'): - for geom in obj.worldbody.findall(".//geom[@group='0']") or []: - geom.set("density", str(scenic_obj.density)) - - if hasattr(scenic_obj, 'friction'): - for geom in obj.worldbody.findall(".//geom[@group='0']") or []: - geom.set("friction", array_to_string(scenic_obj.friction)) \ No newline at end of file From 9641225cd4f0fee36b85eecb1407fc2e86552b38 Mon Sep 17 00:00:00 2001 From: Sahil Mandi <96978073+sahil-tgs@users.noreply.github.com> Date: Fri, 8 Aug 2025 00:29:37 +0530 Subject: [PATCH 4/8] import fix.scenic --- src/scenic/simulators/robosuite/model.scenic | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index bba0f7c62..9043ea5d3 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,6 +1,8 @@ """Scenic world model for RoboSuite simulator.""" from .simulator import RobosuiteSimulator +import scenic.core.dynamics as dynamics +import numpy as np # Global parameters with defaults param use_environment = None @@ -138,9 +140,6 @@ class UR5eRobot(Robot): gripper_type: "Robotiq85Gripper" # Actions -import scenic.core.dynamics as dynamics -import numpy as np - class SetJointPositions(dynamics.Action): """Set robot joint positions. @@ -258,4 +257,4 @@ behavior PickAndLift(target_object, height=1.05): do LiftToHeight(height) if simulation().checkSuccess(): - terminate simulation \ No newline at end of file + terminate simulation From d8dfc699fc14c7b9b592c7ba0d067250dd4926ba Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Fri, 8 Aug 2025 01:19:27 +0530 Subject: [PATCH 5/8] Robosuite Pre-config env only fixed --- .../robosuite/cube_random_position.scenic | 4 +- src/scenic/simulators/robosuite/model.scenic | 74 +------------------ src/scenic/simulators/robosuite/simulator.py | 55 +++++++++++++- 3 files changed, 58 insertions(+), 75 deletions(-) diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic index b590978d7..552623d00 100644 --- a/examples/robosuite/cube_random_position.scenic +++ b/examples/robosuite/cube_random_position.scenic @@ -20,9 +20,11 @@ param camera_view = "birdview" # Randomize cube position cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) -# # Clean approach using object reference +# Clean approach using object reference ego = new PandaRobot with behavior PickAndLift(cube, height=1.05) +# other robot options: UR5eRobot + # Alternative: Custom composition # behavior CustomLift(): # do PickObject(cube) diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index 9043ea5d3..656815564 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,8 +1,6 @@ """Scenic world model for RoboSuite simulator.""" -from .simulator import RobosuiteSimulator -import scenic.core.dynamics as dynamics -import numpy as np +from .simulator import RobosuiteSimulator, SetJointPositions, OSCPositionAction # Global parameters with defaults param use_environment = None @@ -70,20 +68,6 @@ class Cube(RoboSuiteObject): height: DEFAULTS['object_size'] color: DEFAULTS['default_color'] -""" -# currently only Cube works within preconfigured env, we can't change the type of object, WIP, will add support for them soon -class Ball(RoboSuiteObject): - """Spherical object.""" - width: DEFAULTS['object_size'] - color: DEFAULTS['default_color'] - -class Cylinder(RoboSuiteObject): - """Cylindrical object.""" - width: DEFAULTS['object_size'] - height: DEFAULTS['object_size'] * 2 - color: DEFAULTS['default_color'] -""" - # Environment-specific wrapper class EnvironmentObject(RoboSuiteObject): """Base class for objects in RoboSuite environments.""" @@ -129,65 +113,11 @@ class PandaRobot(Robot): robot_type: "Panda" gripper_type: "PandaGripper" -class SawyerRobot(Robot): - """Rethink Sawyer robot.""" - robot_type: "Sawyer" - gripper_type: "RethinkGripper" - class UR5eRobot(Robot): """Universal Robots UR5e.""" robot_type: "UR5e" gripper_type: "Robotiq85Gripper" -# Actions -class SetJointPositions(dynamics.Action): - """Set robot joint positions. - - Args: - positions: Target joint positions - """ - def __init__(self, positions): - self.positions = positions - - def applyTo(self, agent, sim): - """Apply joint position control to the robot.""" - if hasattr(sim, 'robots') and agent in sim.robots: - robot_idx = sim.robots.index(agent) - if robot_idx < len(sim.robosuite_env.robots): - action = np.array(self.positions) - sim.pending_robot_action = action - -class OSCPositionAction(dynamics.Action): - """Operational Space Control for end-effector. - - Args: - position_delta: Cartesian position change [x, y, z] - orientation_delta: Orientation change [roll, pitch, yaw] - gripper: Gripper command (-1=open, 1=close) - """ - def __init__(self, position_delta=None, orientation_delta=None, gripper=None): - self.position_delta = position_delta if position_delta else [0, 0, 0] - self.orientation_delta = orientation_delta if orientation_delta else [0, 0, 0] - self.gripper = gripper if gripper is not None else 0 - - def applyTo(self, agent, sim): - """Apply OSC control to the robot.""" - if hasattr(sim, 'robots') and agent in sim.robots: - robot_idx = sim.robots.index(agent) - if robot_idx < len(sim.robosuite_env.robots): - # Build action array based on controller type - if hasattr(sim, 'controller_type') and sim.controller_type == 'JOINT_POSITION': - action = np.zeros(sim.action_dim) - action[:3] = self.position_delta - else: - # Default OSC action [position(3), orientation(3), gripper(1)] - action = np.zeros(7) - action[:3] = self.position_delta - action[3:6] = self.orientation_delta - action[6] = self.gripper - - sim.pending_robot_action = action - # Behavior Library - Reusable components behavior OpenGripper(steps=DEFAULTS['gripper_open_steps']): """Open gripper over multiple steps.""" @@ -257,4 +187,4 @@ behavior PickAndLift(target_object, height=1.05): do LiftToHeight(height) if simulation().checkSuccess(): - terminate simulation + terminate simulation \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py index 5ba515b25..929447957 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -7,13 +7,13 @@ try: import robosuite as suite - from robosuite.robots import ROBOT_CLASS_MAPPING except ImportError as e: suite = None _import_error = e from scenic.core.simulators import Simulation, Simulator from scenic.core.vectors import Vector +from scenic.core.dynamics import Action # Constants DEFAULT_PHYSICS_TIMESTEP = 0.002 @@ -39,6 +39,9 @@ } + + + class RobosuiteSimulator(Simulator): """Simulator interface for RoboSuite. @@ -325,4 +328,52 @@ def destroy(self): self.robosuite_env.close() self.robosuite_env = None except Exception as e: - print(f"Warning: Error closing RoboSuite environment: {e}") \ No newline at end of file + print(f"Warning: Error closing RoboSuite environment: {e}") + +class SetJointPositions(Action): + """Set robot joint positions. + + Args: + positions: Target joint positions + """ + def __init__(self, positions): + self.positions = positions + + def applyTo(self, agent, sim): + """Apply joint position control to the robot.""" + if hasattr(sim, 'robots') and agent in sim.robots: + robot_idx = sim.robots.index(agent) + if robot_idx < len(sim.robosuite_env.robots): + action = np.array(self.positions) + sim.pending_robot_action = action + +class OSCPositionAction(Action): + """Operational Space Control for end-effector. + + Args: + position_delta: Cartesian position change [x, y, z] + orientation_delta: Orientation change [roll, pitch, yaw] + gripper: Gripper command (-1=open, 1=close) + """ + def __init__(self, position_delta=None, orientation_delta=None, gripper=None): + self.position_delta = position_delta if position_delta else [0, 0, 0] + self.orientation_delta = orientation_delta if orientation_delta else [0, 0, 0] + self.gripper = gripper if gripper is not None else 0 + + def applyTo(self, agent, sim): + """Apply OSC control to the robot.""" + if hasattr(sim, 'robots') and agent in sim.robots: + robot_idx = sim.robots.index(agent) + if robot_idx < len(sim.robosuite_env.robots): + # Build action array based on controller type + if hasattr(sim, 'controller_type') and sim.controller_type == 'JOINT_POSITION': + action = np.zeros(sim.action_dim) + action[:3] = self.position_delta + else: + # Default OSC action [position(3), orientation(3), gripper(1)] + 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 From 6ba582d4233289aafc0b8adfb71ff3a36f54bbd7 Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Thu, 4 Sep 2025 22:10:55 +0530 Subject: [PATCH 6/8] Custom Env --- .../robosuite/cube_random_position.scenic | 5 +- .../robosuite/dual_table_workspace.scenic | 25 + .../robosuite/four_table_workspace.scenic | 62 ++ examples/robosuite/stack_lift.scenic | 44 ++ examples/robosuite/stacked_cubes.scenic | 32 + src/scenic/simulators/robosuite/model.scenic | 150 ++++- src/scenic/simulators/robosuite/simulator.py | 629 +++++++++++++----- 7 files changed, 741 insertions(+), 206 deletions(-) create mode 100644 examples/robosuite/dual_table_workspace.scenic create mode 100644 examples/robosuite/four_table_workspace.scenic create mode 100644 examples/robosuite/stack_lift.scenic create mode 100644 examples/robosuite/stacked_cubes.scenic diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic index 552623d00..573530c4b 100644 --- a/examples/robosuite/cube_random_position.scenic +++ b/examples/robosuite/cube_random_position.scenic @@ -1,11 +1,10 @@ +# examples/robosuite/cube_random_position.scenic """Randomized cube position using behavior library.""" model scenic.simulators.robosuite.model param use_environment = "Lift" -param render = True -param real_time = False -param camera_view = "birdview" +# param camera_view = "sideview" # press escape in simulater inrterface to reset to robosuite default frontview,for seeing the cube actually being lifted. # press ']' or '[' to cycle between different fixed camera_views. (pre defined/set in robosuits's task enviroments) # other options for camera_view: diff --git a/examples/robosuite/dual_table_workspace.scenic b/examples/robosuite/dual_table_workspace.scenic new file mode 100644 index 000000000..19ffbc802 --- /dev/null +++ b/examples/robosuite/dual_table_workspace.scenic @@ -0,0 +1,25 @@ +# examples/robosuite/dual_table_workspace.scenic +model scenic.simulators.robosuite.model + +# SCENARIO CONFIGURATION +param use_environment = "Custom" + +# WORKSPACE SETUP +back_table = new Table at (-0.6, 0, 0.8) +front_table = new Table at (0.6, 0, 0.8) + +# OBJECTS WITH RANDOM PLACEMENT +red_cube = new CustomBox at (-0.6, 0, 0.82), + with envObjectName "cube_back", + with color (1, 0, 0, 1), + with width 0.04, with length 0.04, with height 0.04, + with randomPlacement True, with tableIndex 0 + +green_cube = new CustomBox at (0.6, 0, 0.82), + with envObjectName "cube_front", + with color (0, 1, 0, 1), + with width 0.05, with length 0.05, with height 0.05, + with randomPlacement True, with tableIndex 1 + +# ROBOT +ego = new PandaRobot at (0, 0, 0) \ No newline at end of file diff --git a/examples/robosuite/four_table_workspace.scenic b/examples/robosuite/four_table_workspace.scenic new file mode 100644 index 000000000..16a83bfc6 --- /dev/null +++ b/examples/robosuite/four_table_workspace.scenic @@ -0,0 +1,62 @@ +# examples/robosuite/four_table_workspace.scenic +model scenic.simulators.robosuite.model + +# SCENARIO CONFIGURATION +param use_environment = "Custom" +param camera_view = "sideview" + +TABLE_DISTANCE = 1.0 + +# WORKSPACE: Four tables in cross formation +back_table = new Table at (-TABLE_DISTANCE, 0, 0.8) +front_table = new Table at (TABLE_DISTANCE, 0, 0.8) +right_table = new Table at (0, TABLE_DISTANCE, 0.8) +left_table = new Table at (0, -TABLE_DISTANCE, 0.8) + +# TABLE 1 (Back): Primitive objects +ball = new CustomBall at (-1.0, 0, 0.82), + with envObjectName "ball", with color (1, 0.5, 0, 1), + with randomPlacement True, with tableIndex 0 + +box = new CustomBox at (-1.0, 0, 0.82), + with envObjectName "box", with color (0, 0, 1, 1), + with randomPlacement True, with tableIndex 0 + +capsule = new CustomCapsule at (-1.0, 0, 0.82), + with envObjectName "capsule", with color (0.5, 0.5, 0.5, 1), + with randomPlacement True, with tableIndex 0 + +cylinder = new CustomCylinder at (-1.0, 0, 0.82), + with envObjectName "cylinder", with color (0, 1, 1, 1), + with randomPlacement True, with tableIndex 0 + +# TABLE 2 (Front): cutsom objects 2 +square_nut = new CustomSquareNut at (1.0, 0, 0.85), + with envObjectName "square_nut", with randomPlacement True, with tableIndex 1 + +round_nut = new CustomRoundNut at (1.0, 0, 0.85), + with envObjectName "round_nut", with randomPlacement True, with tableIndex 1 + +milk = new CustomMilk at (1.0, 0, 0.85), + with envObjectName "milk", with randomPlacement True, with tableIndex 1 + +cereal = new CustomCereal at (1.0, 0, 0.85), + with envObjectName "cereal", with randomPlacement True, with tableIndex 1 + +# TABLE 3 (Right): custom objects 1 +can = new CustomCan at (0, 1.0, 0.85), + with envObjectName "can", with randomPlacement True, with tableIndex 2 + +bread = new CustomBread at (0, 1.0, 0.85), + with envObjectName "bread", with randomPlacement True, with tableIndex 2 + +bottle = new CustomBottle at (0, 1.0, 0.85), + with envObjectName "bottle", with randomPlacement True, with tableIndex 2 + +hammer = new CustomHammer at (0, 1.0, 0.85), + with envObjectName "hammer", with randomPlacement True, with tableIndex 2 + +# TABLE 4 (Left): Empty is left empty + +# ROBOT +ego = new PandaRobot at (0, 0, 0) \ 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..47298a73d --- /dev/null +++ b/examples/robosuite/stack_lift.scenic @@ -0,0 +1,44 @@ +# examples/robosuite/stack_lift.scenic +model scenic.simulators.robosuite.model + +# SCENARIO CONFIGURATION +param use_environment = "Custom" +# param camera_view = "sideview" + +# WORKSPACE: Single rectangular table positioned in front (rotated 90 degrees) +work_table = new Table at (0.6, 0, 0.8), + with width 0.6, # Rotated dimensions + with length 1.2, + with height 0.05 + +# OBJECTS +# Bottom cube (larger, stable base) +bottom_cube = new CustomBox at (0.6, 0, 0.83), + with envObjectName "cube_bottom", + with color (0.2, 0.3, 0.8, 1), + with width 0.06, with length 0.06, with height 0.06 + +# Top cube (smaller, stacked on bottom) +top_cube = new CustomBox at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom + with envObjectName "cube_top", + with color (0.8, 0.2, 0.2, 1), + with width 0.04, with length 0.04, with height 0.04 + +# Bottle placed separately on table +bottle = new CustomBottle at (0.6, 0.3, 0.83), + with envObjectName "bottle" + +# Custom lift behavior defined locally +behavior CustomLift(): + do PickObject(top_cube) + do LiftToHeight(1.05) + + # Check if cube is above threshold height + for _ in range(10): # Check for 10 steps + if top_cube.position.z > 1.0: + terminate simulation + wait + +# ROBOT: Using UR5e with custom behavior +ego = new UR5eRobot at (0, 0, 0), + with behavior CustomLift() \ No newline at end of file diff --git a/examples/robosuite/stacked_cubes.scenic b/examples/robosuite/stacked_cubes.scenic new file mode 100644 index 000000000..46f5572bd --- /dev/null +++ b/examples/robosuite/stacked_cubes.scenic @@ -0,0 +1,32 @@ +# examples/robosuite/stacked_cubes.scenic +model scenic.simulators.robosuite.model + +# SCENARIO CONFIGURATION +param use_environment = "Custom" +param camera_view = "sideview" + +# WORKSPACE: Single rectangular table positioned in front (rotated 90 degrees) +work_table = new Table at (0.6, 0, 0.8), + with width 0.6, # Rotated dimensions + with length 1.2, + with height 0.05 + +# OBJECTS +# Bottom cube (larger, stable base) +bottom_cube = new CustomBox at (0.6, 0, 0.83), + with envObjectName "cube_bottom", + with color (0.2, 0.3, 0.8, 1), + with width 0.06, with length 0.06, with height 0.06 + +# Top cube (smaller, stacked on bottom) +top_cube = new CustomBox at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom + with envObjectName "cube_top", + with color (0.8, 0.2, 0.2, 1), + with width 0.04, with length 0.04, with height 0.04 + +# Bottle placed separately on table +bottle = new CustomBottle at (0.6, 0.3, 0.83), + with envObjectName "bottle" + +# ROBOT: Using UR5e, positioned at origin +ego = new UR5eRobot at (0, 0, 0) \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index 656815564..d51264ad8 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,15 +1,16 @@ -"""Scenic world model for RoboSuite simulator.""" +"""Scenic world model for RoboSuite.""" from .simulator import RobosuiteSimulator, SetJointPositions, OSCPositionAction -# Global parameters with defaults +# Global parameters with defaults matching Robosuite's defaults param use_environment = None param env_config = {} param controller_config = None param camera_view = None param render = True -param real_time = True +param real_time = True # Robosuite default param speed = 1.0 +param lite_physics = None # None = use Robosuite default (True) # Simulator simulator RobosuiteSimulator( @@ -19,7 +20,8 @@ simulator RobosuiteSimulator( use_environment=globalParameters.use_environment, env_config=globalParameters.env_config, controller_config=globalParameters.controller_config, - camera_view=globalParameters.camera_view + camera_view=globalParameters.camera_view, + lite_physics=globalParameters.lite_physics ) # Default values dictionary @@ -30,12 +32,13 @@ DEFAULTS = { 'friction': (1.0, 0.005, 0.0001), 'solref': (0.02, 1.0), 'solimp': (0.9, 0.95, 0.001, 0.5, 2.0), - 'default_color': (0.5, 0.5, 0.5), + 'default_color': (0.5, 0.5, 0.5, 1.0), # Arena properties 'table_height': 0.8, - 'table_width': 1.0, + 'table_width': 0.8, 'table_length': 0.8, + 'table_thickness': 0.05, # Robot properties 'robot_width': 0.2, @@ -53,14 +56,99 @@ DEFAULTS = { 'max_lift_steps': 200 } -# Base class +# Base classes class RoboSuiteObject(Object): + """Base class for all RoboSuite objects.""" density: DEFAULTS['density'] friction: DEFAULTS['friction'] solref: DEFAULTS['solref'] solimp: DEFAULTS['solimp'] shape: BoxShape() + allowCollisions: True + +# Table class for custom environments +class Table(RoboSuiteObject): + """Table in custom environment.""" + isTable: True + width: DEFAULTS['table_width'] + length: DEFAULTS['table_length'] + height: DEFAULTS['table_thickness'] + position: (0, 0, DEFAULTS['table_height']) + +# Base class for custom environment objects +class CustomObject(RoboSuiteObject): + """Base class for custom environment objects.""" + envObjectName: None + randomPlacement: False + tableIndex: 0 + xRange: (-0.1, 0.1) + yRange: (-0.1, 0.1) + color: DEFAULTS['default_color'] + +# Primitive objects for custom environments +class CustomBox(CustomObject): + """Box object for custom environments.""" + objectType: "Box" + width: DEFAULTS['object_size'] + length: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] + +class CustomBall(CustomObject): + """Ball object for custom environments.""" + objectType: "Ball" + radius: DEFAULTS['object_size'] + width: DEFAULTS['object_size'] * 2 # For compatibility + length: DEFAULTS['object_size'] * 2 + height: DEFAULTS['object_size'] * 2 + +class CustomCylinder(CustomObject): + """Cylinder object for custom environments.""" + objectType: "Cylinder" + width: DEFAULTS['object_size'] * 2 # Diameter + length: DEFAULTS['object_size'] * 2 + height: DEFAULTS['object_size'] * 4 # Height + +class CustomCapsule(CustomObject): + """Capsule object for custom environments.""" + objectType: "Capsule" + width: DEFAULTS['object_size'] * 1.5 + length: DEFAULTS['object_size'] * 1.5 + height: DEFAULTS['object_size'] * 3 + +# Complex objects for custom environments +class CustomMilk(CustomObject): + """Milk carton for custom environments.""" + objectType: "Milk" + +class CustomCereal(CustomObject): + """Cereal box for custom environments.""" + objectType: "Cereal" + +class CustomCan(CustomObject): + """Can object for custom environments.""" + objectType: "Can" + +class CustomBread(CustomObject): + """Bread object for custom environments.""" + objectType: "Bread" + +class CustomBottle(CustomObject): + """Bottle object for custom environments.""" + objectType: "Bottle" + +class CustomHammer(CustomObject): + """Hammer object for custom environments.""" + objectType: "Hammer" + +class CustomSquareNut(CustomObject): + """Square nut for custom environments.""" + objectType: "SquareNut" +class CustomRoundNut(CustomObject): + """Round nut for custom environments.""" + objectType: "RoundNut" + +# Standard environment objects class Cube(RoboSuiteObject): """Cubic object with uniform dimensions.""" width: DEFAULTS['object_size'] @@ -68,32 +156,36 @@ class Cube(RoboSuiteObject): height: DEFAULTS['object_size'] color: DEFAULTS['default_color'] -# Environment-specific wrapper class EnvironmentObject(RoboSuiteObject): - """Base class for objects in RoboSuite environments.""" - envObjectName: None + """Base class for standard environment objects.""" + envObjectName: None allowCollisions: True -# Convenience classes for specific environments -class LiftCube(Cube): +class LiftCube(EnvironmentObject): """Cube in Lift environment.""" envObjectName: "cube" - allowCollisions: True + width: DEFAULTS['object_size'] + length: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] -class StackCubeA(Cube): +class StackCubeA(EnvironmentObject): """First cube in Stack environment.""" envObjectName: "cubeA" - allowCollisions: True + width: DEFAULTS['object_size'] + length: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] -class StackCubeB(Cube): +class StackCubeB(EnvironmentObject): """Second cube in Stack environment.""" envObjectName: "cubeB" - allowCollisions: True + width: DEFAULTS['object_size'] + length: DEFAULTS['object_size'] + height: DEFAULTS['object_size'] # Robots class Robot(RoboSuiteObject): """Base robot class.""" - robot_type: "Panda" # Default type + robot_type: "Panda" gripper_type: "default" controller_config: None initial_qpos: None @@ -107,7 +199,6 @@ class Robot(RoboSuiteObject): eef_pos[dynamic]: [0, 0, 0] gripper_state[dynamic]: [0, 0] -# Generic robot templates class PandaRobot(Robot): """Franka Emika Panda robot.""" robot_type: "Panda" @@ -118,7 +209,22 @@ class UR5eRobot(Robot): robot_type: "UR5e" gripper_type: "Robotiq85Gripper" -# Behavior Library - Reusable components +class SawyerRobot(Robot): + """Rethink Robotics Sawyer.""" + robot_type: "Sawyer" + gripper_type: "RethinkGripper" + +class JacoRobot(Robot): + """Kinova Jaco robot.""" + robot_type: "Jaco" + gripper_type: "JacoThreeFingerGripper" + +class IIWARobot(Robot): + """KUKA IIWA robot.""" + robot_type: "IIWA" + gripper_type: "Robotiq140Gripper" + +# Behavior Library behavior OpenGripper(steps=DEFAULTS['gripper_open_steps']): """Open gripper over multiple steps.""" for _ in range(steps): @@ -173,7 +279,6 @@ behavior LiftToHeight(target_height=1.0, max_steps=DEFAULTS['max_lift_steps']): delta = [max(-limit, min(limit, e * gain)) for e in error] take OSCPositionAction(position_delta=delta, gripper=1) -# Object-aware behaviors behavior PickObject(target_object): """Pick up a specific object.""" do OpenGripper() @@ -187,4 +292,5 @@ behavior PickAndLift(target_object, height=1.05): do LiftToHeight(height) if simulation().checkSuccess(): - terminate simulation \ No newline at end of file + terminate simulation + diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py index 929447957..273a5b6e5 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -1,12 +1,21 @@ """RoboSuite Simulator interface for Scenic.""" +from typing import Dict, List, Any, Optional, Union import numpy as np import mujoco -import time -from typing import Dict, List, Any, Optional -try: +try: import robosuite as suite + from robosuite.environments.manipulation.manipulation_env import ManipulationEnv + from robosuite.models.arenas import TableArena, MultiTableArena + from robosuite.models.tasks import ManipulationTask + from robosuite.models.objects import ( + BallObject, BoxObject, CylinderObject, CapsuleObject, + MilkObject, CerealObject, CanObject, BreadObject, + HammerObject, SquareNutObject, RoundNutObject, BottleObject + ) + from robosuite.utils.placement_samplers import UniformRandomSampler + from robosuite.utils.observables import Observable, sensor except ImportError as e: suite = None _import_error = e @@ -20,46 +29,214 @@ DEFAULT_TIMESTEP = 0.01 DEFAULT_ACTION_DIM = 7 PHYSICS_SETTLE_STEPS = 10 -WINDOW_CLOSE_DELAY = 2 # RoboSuite naming patterns -ROBOSUITE_SUFFIXES = { - 'body': ['_main', '_body', ''], - 'joint': '_joint0' -} +BODY_SUFFIXES = ['_main', '_body', ''] +JOINT_SUFFIX = '_joint0' # Camera view mapping CAMERA_VIEWS = { "frontview": 0, - "birdview": 1, + "birdview": 1, "agentview": 2, "sideview": 3, "robot0_robotview": 4, "robot0_eye_in_hand": 5 } +# Object type mapping +OBJECT_FACTORIES = { + 'Ball': lambda cfg: BallObject( + name=cfg['name'], + size=[cfg.get('radius', cfg.get('size', [0.02])[0])], + rgba=cfg.get('color', [1, 0, 0, 1]) + ), + 'Box': lambda cfg: BoxObject( + name=cfg['name'], + size=cfg.get('size', [0.025, 0.025, 0.025]), + rgba=cfg.get('color', [1, 0, 0, 1]) + ), + 'Cylinder': lambda cfg: CylinderObject( + name=cfg['name'], + size=_extract_cylinder_size(cfg) + ), + 'Capsule': lambda cfg: CapsuleObject( + name=cfg['name'], + size=_extract_cylinder_size(cfg) + ), + 'Milk': lambda cfg: MilkObject(name=cfg['name']), + 'Cereal': lambda cfg: CerealObject(name=cfg['name']), + 'Can': lambda cfg: CanObject(name=cfg['name']), + 'Bread': lambda cfg: BreadObject(name=cfg['name']), + 'Hammer': lambda cfg: HammerObject(name=cfg['name']), + 'SquareNut': lambda cfg: SquareNutObject(name=cfg['name']), + 'RoundNut': lambda cfg: RoundNutObject(name=cfg['name']), + 'Bottle': lambda cfg: BottleObject(name=cfg['name']) +} +def _extract_cylinder_size(config: Dict) -> List[float]: + """Extract [radius, height] for cylinder-like objects.""" + size = config.get('size', [0.02, 0.04]) + if len(size) == 3: # Convert from [width, length, height] + return [size[0] / 2, size[2]] + return size + + +class CustomManipulationEnv(ManipulationEnv): + """Custom manipulation environment for Scenic-defined scenarios.""" + + def __init__(self, scenic_config: Dict, **kwargs): + self.scenic_config = scenic_config + self.scenic_objects = scenic_config.get('objects', []) + self.scenic_tables = scenic_config.get('tables', []) + super().__init__(**kwargs) + + def _load_model(self): + """Load models and create arena.""" + super()._load_model() + + self._position_robots() + mujoco_arena = self._create_arena() + mujoco_arena.set_origin([0, 0, 0]) + + self.mujoco_objects, self.placement_samplers = 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 _position_robots(self): + """Position robots based on scenic configuration.""" + for i, robot_config in enumerate(self.scenic_config.get('robots', [])): + if i < len(self.robots): + pos = robot_config.get('position', [0, 0, 0]) + self.robots[i].robot_model.set_base_xpos(pos) + + def _create_arena(self): + """Create arena based on table configuration.""" + if not self.scenic_tables: + # No tables - use empty arena (just floor) + from robosuite.models.arenas import EmptyArena + return EmptyArena() + + # Use MultiTableArena for any number of tables (1+) + from robosuite.models.arenas import MultiTableArena + return MultiTableArena( + table_offsets=[t.get('position', [0, 0, 0.8]) for t in self.scenic_tables], + table_full_sizes=[t.get('size', (0.8, 0.8, 0.05)) for t in self.scenic_tables], + has_legs=[True] * len(self.scenic_tables) + ) + + def _create_objects(self) -> tuple: + """Create Robosuite objects from Scenic configuration.""" + mujoco_objects = [] + placement_samplers = [] + + for obj_config in self.scenic_objects: + obj_type = obj_config['type'] + + # Create object using factory or default to box + factory = OBJECT_FACTORIES.get(obj_type, OBJECT_FACTORIES['Box']) + mj_obj = factory(obj_config) + mujoco_objects.append(mj_obj) + + # Create placement sampler if needed + if obj_config.get('random_placement', False): + sampler = self._create_placement_sampler(obj_config, mj_obj) + placement_samplers.append(sampler) + + return mujoco_objects, placement_samplers + + def _create_placement_sampler(self, obj_config: Dict, mj_obj) -> UniformRandomSampler: + """Create placement sampler for randomized object placement.""" + table_idx = obj_config.get('table_index', 0) + ref_pos = (self.scenic_tables[table_idx]['position'] + if table_idx < len(self.scenic_tables) + else [0, 0, 0.8]) + + return UniformRandomSampler( + name=f"{obj_config['name']}_sampler", + mujoco_objects=mj_obj, + x_range=obj_config.get('x_range', [-0.1, 0.1]), + y_range=obj_config.get('y_range', [-0.1, 0.1]), + ensure_object_boundary_in_range=False, + ensure_valid_placement=True, + reference_pos=ref_pos, + z_offset=0.01 + ) + + def _setup_references(self): + """Setup references to simulation objects.""" + super()._setup_references() + self.obj_body_ids = { + obj.name: self.sim.model.body_name2id(obj.root_body) + for obj in self.mujoco_objects + } + + def _setup_observables(self) -> Dict: + """Setup observables for objects.""" + observables = super()._setup_observables() + + for obj in self.mujoco_objects: + @sensor(modality="object") + def obj_pos(obs_cache, name=obj.name): + return np.array(self.sim.data.body_xpos[self.obj_body_ids[name]]) + + obj_pos.__name__ = f"{obj.name}_pos" + observables[obj_pos.__name__] = Observable( + name=obj_pos.__name__, + sensor=obj_pos, + sampling_rate=self.control_freq + ) + + return observables + + def _reset_internal(self): + """Reset environment internals.""" + super()._reset_internal() + + # Apply placement samplers + for sampler in self.placement_samplers: + placement = sampler.sample() + for obj_pos, obj_quat, obj in placement.values(): + self.sim.data.set_joint_qpos( + obj.joints[0], + np.concatenate([np.array(obj_pos), np.array(obj_quat)]) + ) + + # Set fixed positions + for obj_config, mj_obj in zip(self.scenic_objects, self.mujoco_objects): + if not obj_config.get('random_placement', False) and 'position' in obj_config: + pos = obj_config['position'] + quat = obj_config.get('quaternion', [1, 0, 0, 0]) + self.sim.data.set_joint_qpos( + mj_obj.joints[0], + np.concatenate([np.array(pos), np.array(quat)]) + ) + + def reward(self, action=None) -> float: + """Compute reward.""" + return 0.0 + + def _check_success(self) -> bool: + """Check task success.""" + return False class RobosuiteSimulator(Simulator): - """Simulator interface for RoboSuite. - - Args: - render: Enable visualization - real_time: Run simulation in real-time - speed: Simulation speed multiplier - use_environment: RoboSuite environment name - env_config: Additional environment configuration - controller_config: Robot controller configuration - camera_view: Camera perspective for rendering - """ - - def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, - env_config=None, controller_config=None, camera_view=None): + """Simulator for RoboSuite environments.""" + + def __init__(self, render: bool = True, real_time: bool = True, speed: float = 1.0, + use_environment: Optional[str] = None, env_config: Optional[Dict] = None, + controller_config: Optional[Dict] = None, camera_view: Optional[str] = None, + lite_physics: Optional[bool] = None): super().__init__() if suite is None: raise RuntimeError(f"Unable to import RoboSuite: {_import_error}") + self.render = render self.real_time = real_time self.speed = speed @@ -67,26 +244,25 @@ def __init__(self, render=True, real_time=True, speed=1.0, use_environment=None, 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 RoboSuite simulation instance.""" - return RobosuiteSimulation(scene, self.render, self.real_time, self.speed, - self.use_environment, self.env_config, self.controller_config, - self.camera_view, **kwargs) + """Create a simulation instance.""" + return RobosuiteSimulation( + scene, self.render, self.real_time, self.speed, + self.use_environment, self.env_config, + self.controller_config, self.camera_view, + self.lite_physics, **kwargs + ) class RobosuiteSimulation(Simulation): - """Simulation instance for RoboSuite. - - Manages the interface between Scenic's simulation loop and RoboSuite's - environment, handling object tracking, action execution, and property updates. - """ + """Simulation for RoboSuite environments.""" - def __init__(self, scene, render: bool, real_time: bool, speed: float, - use_environment: Optional[str] = None, - env_config: Optional[Dict] = None, - controller_config: Optional[Dict] = None, - camera_view: Optional[str] = None, **kwargs): + def __init__(self, scene, render: bool, real_time: bool, speed: float, + use_environment: Optional[str], env_config: Optional[Dict], + controller_config: Optional[Dict], camera_view: Optional[str], + lite_physics: Optional[bool], **kwargs): self.render = render self.real_time = real_time self.speed = speed @@ -94,6 +270,9 @@ def __init__(self, scene, render: bool, real_time: bool, speed: float, self.env_config = env_config or {} self.controller_config = controller_config self.camera_view = camera_view + self.lite_physics = lite_physics + + # Environment state self.robosuite_env = None self.model = None self.data = None @@ -111,8 +290,8 @@ def __init__(self, scene, render: bool, real_time: bool, speed: float, self.controller_type = None # Timing - self.timestep = kwargs.get('timestep') if kwargs.get('timestep') is not None else DEFAULT_TIMESTEP - self.physics_timestep = kwargs.get('physics_timestep') if kwargs.get('physics_timestep') is not None else DEFAULT_PHYSICS_TIMESTEP + 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 = [] @@ -124,139 +303,227 @@ def setup(self): if not self.use_environment: raise ValueError("Environment name must be specified via 'use_environment' parameter") - - # Get robots from scene + + if self.use_environment.lower() == "custom": + self._setup_custom_environment() + else: + self._setup_standard_environment() + + 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() + + # Identify agents + 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 _setup_custom_environment(self): + """Setup custom manipulation environment.""" + scenic_config = self._extract_scenic_config() + robot_arg = self._get_robot_arg(scenic_config['robots']) + + env_kwargs = { + 'scenic_config': scenic_config, + '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 = CustomManipulationEnv(**env_kwargs) + self._current_obs = self.robosuite_env.reset() + self._detect_controller_type(scenic_config['robots']) + + def _extract_scenic_config(self) -> Dict: + """Extract configuration from Scenic scene.""" + config = {'robots': [], 'tables': [], 'objects': []} + + for obj in self.objects: + if hasattr(obj, 'robot_type'): + self._add_robot_config(config['robots'], obj) + elif hasattr(obj, 'isTable') and obj.isTable: + self._add_table_config(config['tables'], obj) + elif hasattr(obj, 'objectType'): + self._add_object_config(config['objects'], obj) + + return config + + def _add_robot_config(self, robots: List, obj): + """Add robot configuration.""" + robots.append({ + 'type': obj.robot_type, + 'position': [obj.position.x, obj.position.y, obj.position.z] + }) + 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, obj.height) + }) + + def _add_object_config(self, objects: List, obj): + """Add object configuration.""" + config = { + 'type': obj.objectType, + 'name': getattr(obj, 'envObjectName', f"obj_{id(obj)}"), + 'position': [obj.position.x, obj.position.y, obj.position.z], + 'color': getattr(obj, 'color', [1, 0, 0, 1]), + 'random_placement': getattr(obj, 'randomPlacement', False), + 'table_index': getattr(obj, 'tableIndex', 0) + } + + # Add size/radius info + if hasattr(obj, 'radius'): + config['radius'] = obj.radius + elif hasattr(obj, 'width'): + config['size'] = [obj.width, obj.length, obj.height] + + # Add placement ranges + if config['random_placement']: + config['x_range'] = getattr(obj, 'xRange', [-0.1, 0.1]) + config['y_range'] = getattr(obj, 'yRange', [-0.1, 0.1]) + + objects.append(config) + + def _get_robot_arg(self, robots: List) -> Union[str, List[str]]: + """Get robot argument for environment creation.""" + if not robots: + return "Panda" + return ([r['type'] for r in robots] if len(robots) > 1 + else robots[0]['type']) + + def _setup_standard_environment(self): + """Setup standard RoboSuite environment.""" robots = [obj for obj in self.objects if hasattr(obj, 'robot_type')] + robot_arg = self._get_robot_arg([{'type': r.robot_type} for r in robots]) - # Configure environment config = { - **self.env_config, - 'has_renderer': self.render, - 'render_camera': "frontview", - 'camera_names': ["frontview", "robot0_eye_in_hand"], + **self.env_config, + 'has_renderer': self.render, + 'render_camera': self.camera_view, + 'camera_names': ["frontview", "robot0_eye_in_hand"], 'controller_configs': self.controller_config } - # Prepare robots - robot_arg = "Panda" # Default - if robots: - robot_arg = [r.robot_type for r in robots] if len(robots) > 1 else robots[0].robot_type - - # Create environment self.robosuite_env = suite.make(self.use_environment, robots=robot_arg, **config) self._current_obs = self.robosuite_env.reset() - self.model = self.robosuite_env.sim.model._model - self.data = self.robosuite_env.sim.data._data - # Detect controller type and action dimension - if robots and self.robosuite_env.robots: + # Store robot references + for i, robot in enumerate(robots[:len(self.robosuite_env.robots)]): + self.robots.append(robot) + self.object_names[robot] = f"robot{i}" + + self._apply_initial_positions() + self._detect_controller_type([{'type': r.robot_type} for r in robots]) + + def _detect_controller_type(self, robot_configs: List): + """Detect controller type from first robot.""" + if robot_configs and self.robosuite_env.robots: first_robot = self.robosuite_env.robots[0] if hasattr(first_robot, 'controller'): - controller_name = type(first_robot.controller).__name__ - self.controller_type = controller_name + self.controller_type = type(first_robot.controller).__name__ if hasattr(first_robot.controller, 'control_dim'): self.action_dim = first_robot.controller.control_dim - - # Set camera - if self.render and self.camera_view is not None: - 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) - - # Store robot references - for i, r in enumerate(robots[:len(self.robosuite_env.robots)]): - self.robots.append(r) - self.object_names[r] = f"robot{i}" - - # Apply initial positions - self._apply_initial_positions() - - # Setup body mapping for environment objects - self._setup_body_mapping() - - # Identify agents - for obj in self.objects: - if hasattr(obj, 'behavior') and obj.behavior: - self.agents.append(obj) def _setup_body_mapping(self): """Map environment objects to MuJoCo body IDs.""" for obj in self.objects: if hasattr(obj, 'envObjectName'): - # Try to find body with standard naming patterns - for suffix in ROBOSUITE_SUFFIXES['body']: - body_name = f"{obj.envObjectName}{suffix}" - try: - body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, body_name) - if body_id != -1: - self.body_id_map[obj.envObjectName] = body_id - self.prev_positions[obj.envObjectName] = self.data.xpos[body_id].copy() - break - except: - continue + self._map_object_body(obj) + + def _map_object_body(self, obj): + """Map single object to body ID.""" + for suffix in BODY_SUFFIXES: + body_name = f"{obj.envObjectName}{suffix}" + try: + body_id = mujoco.mj_name2id( + self.model, mujoco.mjtObj.mjOBJ_BODY, body_name + ) + if body_id != -1: + self.body_id_map[obj.envObjectName] = body_id + self.prev_positions[obj.envObjectName] = self.data.xpos[body_id].copy() + break + except: + continue def _apply_initial_positions(self): """Apply initial positions for environment objects.""" for obj in self.objects: if hasattr(obj, 'envObjectName') and hasattr(obj, 'position'): - joint_name = f"{obj.envObjectName}{ROBOSUITE_SUFFIXES['joint']}" - try: - qpos = np.concatenate([ - [obj.position.x, obj.position.y, obj.position.z], - [1, 0, 0, 0] # identity quaternion - ]) - self.robosuite_env.sim.data.set_joint_qpos(joint_name, qpos) - except Exception as e: - raise RuntimeError( - f"Failed to set position for object '{obj.envObjectName}'. " - f"Ensure the object exists in the '{self.use_environment}' environment. " - f"Error: {e}" - ) - - # Update physics + self._set_object_position(obj) + self.robosuite_env.sim.forward() # Let physics settle for _ in range(PHYSICS_SETTLE_STEPS): self.robosuite_env.step(np.zeros(self.action_dim)) - - # Get fresh observations + self._current_obs = self.robosuite_env._get_observations() + def _set_object_position(self, obj): + """Set position for single object.""" + joint_name = f"{obj.envObjectName}{JOINT_SUFFIX}" + try: + qpos = np.concatenate([ + [obj.position.x, obj.position.y, obj.position.z], + [1, 0, 0, 0] + ]) + self.robosuite_env.sim.data.set_joint_qpos(joint_name, qpos) + except: + pass + def createObjectInSimulator(self, obj): - """Required by Scenic's Simulator interface but not used. - - RoboSuite creates all objects at environment initialization, - so dynamic object creation is not supported. - """ + """Required by Scenic's Simulator interface.""" + """See _setup_custom_environment() and _setup_standard_environment() for object creation.""" pass def executeActions(self, allActions: Dict[Any, List]) -> None: """Execute actions by calling their applyTo methods.""" super().executeActions(allActions) - # Clear pending actions self.pending_robot_action = None - # Process actions for each agent for agent in self.agents: if agent in allActions and allActions[agent]: for action in allActions[agent]: if action and hasattr(action, 'applyTo'): action.applyTo(agent, self) - break # One action per agent per timestep + break def step(self): """Step the simulation forward one timestep.""" if hasattr(self, '_done') and self._done: return - + # Store previous positions for name, body_id in self.body_id_map.items(): self.prev_positions[name] = self.data.xpos[body_id].copy() - # Use pending action or zeros - action = self.pending_robot_action if self.pending_robot_action is not None else np.zeros(self.action_dim) + action = (self.pending_robot_action if self.pending_robot_action is not None + else np.zeros(self.action_dim)) # Step simulation for _ in range(self.physics_steps): @@ -268,7 +535,6 @@ def step(self): if done: break - # Clear pending actions self.pending_robot_action = None def getProperties(self, obj, properties: List[str]) -> Dict[str, Any]: @@ -278,99 +544,100 @@ def getProperties(self, obj, properties: List[str]) -> Dict[str, Any]: for prop in properties: if prop == 'position': - # For environment objects, get from observations - if hasattr(obj, 'envObjectName') and self._current_obs: - obs_key = f"{obj.envObjectName}_pos" - if obs_key in self._current_obs: - pos = self._current_obs[obs_key] - values[prop] = Vector(pos[0], pos[1], pos[2]) - else: - values[prop] = obj.position - else: - values[prop] = obj.position - + values[prop] = self._get_object_position(obj) elif prop == 'joint_positions' and robot_idx is not None: - if robot_idx < len(self.robosuite_env.robots): - positions = self.robosuite_env.robots[robot_idx]._joint_positions - values[prop] = list(positions) - else: - values[prop] = [] - - elif prop == 'eef_pos' and robot_idx is not None and self._current_obs: - pos = self._current_obs.get(f'robot{robot_idx}_eef_pos', [0, 0, 0]) - values[prop] = list(pos) - - elif prop == 'gripper_state' and robot_idx is not None and self._current_obs: - state = self._current_obs.get(f'robot{robot_idx}_gripper_qpos', [0, 0]) - values[prop] = list(state) - + values[prop] = self._get_robot_joints(robot_idx) + elif prop == 'eef_pos' and robot_idx is not None: + values[prop] = self._get_eef_position(robot_idx) + elif prop == 'gripper_state' and robot_idx is not None: + values[prop] = self._get_gripper_state(robot_idx) else: values[prop] = getattr(obj, prop, None) return values + def _get_object_position(self, obj) -> Vector: + """Get object position.""" + if hasattr(obj, 'envObjectName') and self._current_obs: + obs_key = f"{obj.envObjectName}_pos" + if obs_key in self._current_obs: + pos = self._current_obs[obs_key] + return Vector(pos[0], pos[1], pos[2]) + elif obj.envObjectName in self.body_id_map: + body_id = self.body_id_map[obj.envObjectName] + pos = self.data.xpos[body_id] + return Vector(pos[0], pos[1], pos[2]) + return obj.position + + 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 checkSuccess(self) -> bool: """Check if task is successfully completed.""" - return self.robosuite_env._check_success() if hasattr(self.robosuite_env, '_check_success') else False + if hasattr(self.robosuite_env, '_check_success'): + return self.robosuite_env._check_success() + return False def destroy(self): """Clean up simulation resources.""" - if self.render and self.robosuite_env: - print(f"Simulation complete. Window will close in {WINDOW_CLOSE_DELAY} seconds...") - time.sleep(WINDOW_CLOSE_DELAY) - - try: - if self.robosuite_env: - self.robosuite_env.close() - self.robosuite_env = None - except Exception as e: - print(f"Warning: Error closing RoboSuite environment: {e}") + if self.robosuite_env: + self.robosuite_env.close() + self.robosuite_env = None + +# Actions class SetJointPositions(Action): - """Set robot joint positions. + """Set robot joint positions.""" - Args: - positions: Target joint positions - """ - def __init__(self, positions): + def __init__(self, positions: List[float]): self.positions = positions - def applyTo(self, agent, sim): - """Apply joint position control to the robot.""" + def applyTo(self, agent, sim: RobosuiteSimulation): + """Apply action to agent.""" if hasattr(sim, 'robots') and agent in sim.robots: robot_idx = sim.robots.index(agent) if robot_idx < len(sim.robosuite_env.robots): - action = np.array(self.positions) - sim.pending_robot_action = action + sim.pending_robot_action = np.array(self.positions) + class OSCPositionAction(Action): - """Operational Space Control for end-effector. - - Args: - position_delta: Cartesian position change [x, y, z] - orientation_delta: Orientation change [roll, pitch, yaw] - gripper: Gripper command (-1=open, 1=close) - """ - def __init__(self, position_delta=None, orientation_delta=None, gripper=None): - self.position_delta = position_delta if position_delta else [0, 0, 0] - self.orientation_delta = orientation_delta if orientation_delta else [0, 0, 0] + """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 applyTo(self, agent, sim): - """Apply OSC control to the robot.""" + def applyTo(self, agent, sim: RobosuiteSimulation): + """Apply action to agent.""" if hasattr(sim, 'robots') and agent in sim.robots: robot_idx = sim.robots.index(agent) if robot_idx < len(sim.robosuite_env.robots): - # Build action array based on controller type - if hasattr(sim, 'controller_type') and sim.controller_type == 'JOINT_POSITION': + if sim.controller_type == 'JOINT_POSITION': action = np.zeros(sim.action_dim) action[:3] = self.position_delta else: - # Default OSC action [position(3), orientation(3), gripper(1)] action = np.zeros(7) action[:3] = self.position_delta action[3:6] = self.orientation_delta From 4ea472fdfcd33f61c5c033d5bc22f298c8bd0474 Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Mon, 8 Sep 2025 05:01:58 +0530 Subject: [PATCH 7/8] removed redundant class attributes --- .../robosuite/cube_random_position.scenic | 4 +- .../robosuite/dual_table_workspace.scenic | 17 ++- .../robosuite/four_table_workspace.scenic | 54 ++++---- examples/robosuite/stack_lift.scenic | 15 +-- examples/robosuite/stacked_cubes.scenic | 17 ++- src/scenic/simulators/robosuite/model.scenic | 117 ++++++++---------- src/scenic/simulators/robosuite/simulator.py | 107 +++++++--------- 7 files changed, 137 insertions(+), 194 deletions(-) diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic index 573530c4b..da1c9d57a 100644 --- a/examples/robosuite/cube_random_position.scenic +++ b/examples/robosuite/cube_random_position.scenic @@ -17,10 +17,10 @@ param use_environment = "Lift" # Randomize cube position -cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) +# cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) # Clean approach using object reference -ego = new PandaRobot with behavior PickAndLift(cube, height=1.05) +ego = new Panda with behavior PickAndLift(cube, height=1.05) # other robot options: UR5eRobot diff --git a/examples/robosuite/dual_table_workspace.scenic b/examples/robosuite/dual_table_workspace.scenic index 19ffbc802..7a297c268 100644 --- a/examples/robosuite/dual_table_workspace.scenic +++ b/examples/robosuite/dual_table_workspace.scenic @@ -8,18 +8,15 @@ param use_environment = "Custom" back_table = new Table at (-0.6, 0, 0.8) front_table = new Table at (0.6, 0, 0.8) -# OBJECTS WITH RANDOM PLACEMENT -red_cube = new CustomBox at (-0.6, 0, 0.82), - with envObjectName "cube_back", +# OBJECTS WITH RANDOM PLACEMENT USING RANGE +# Using Range with modern tuple syntax for randomness +red_cube = new Box at (Range(-0.10, -0.15), Range(-0.11, 0.22), 0.83), with color (1, 0, 0, 1), - with width 0.04, with length 0.04, with height 0.04, - with randomPlacement True, with tableIndex 0 + with width 0.04, with length 0.04, with height 0.04 -green_cube = new CustomBox at (0.6, 0, 0.82), - with envObjectName "cube_front", +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, - with randomPlacement True, with tableIndex 1 + with width 0.05, with length 0.05, with height 0.05 # ROBOT -ego = new PandaRobot at (0, 0, 0) \ No newline at end of file +ego = new Panda at (0, 0, 0) \ No newline at end of file diff --git a/examples/robosuite/four_table_workspace.scenic b/examples/robosuite/four_table_workspace.scenic index 16a83bfc6..edb6de9bf 100644 --- a/examples/robosuite/four_table_workspace.scenic +++ b/examples/robosuite/four_table_workspace.scenic @@ -13,50 +13,38 @@ front_table = new Table at (TABLE_DISTANCE, 0, 0.8) right_table = new Table at (0, TABLE_DISTANCE, 0.8) left_table = new Table at (0, -TABLE_DISTANCE, 0.8) -# TABLE 1 (Back): Primitive objects -ball = new CustomBall at (-1.0, 0, 0.82), - with envObjectName "ball", with color (1, 0.5, 0, 1), - with randomPlacement True, with tableIndex 0 +# TABLE 1 (Back): Primitive objects with random positions using Range +ball = new Ball at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), + with color (1, 0.5, 0, 1) -box = new CustomBox at (-1.0, 0, 0.82), - with envObjectName "box", with color (0, 0, 1, 1), - with randomPlacement True, with tableIndex 0 +box = new Box at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), + with color (0, 0, 1, 1) -capsule = new CustomCapsule at (-1.0, 0, 0.82), - with envObjectName "capsule", with color (0.5, 0.5, 0.5, 1), - with randomPlacement True, with tableIndex 0 +capsule = new Capsule at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), + with color (0.5, 0.5, 0.5, 1) -cylinder = new CustomCylinder at (-1.0, 0, 0.82), - with envObjectName "cylinder", with color (0, 1, 1, 1), - with randomPlacement True, with tableIndex 0 +cylinder = new Cylinder at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), + with color (0, 1, 1, 1) -# TABLE 2 (Front): cutsom objects 2 -square_nut = new CustomSquareNut at (1.0, 0, 0.85), - with envObjectName "square_nut", with randomPlacement True, with tableIndex 1 +# TABLE 2 (Front): Nuts and food items with random positions +square_nut = new SquareNut at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) -round_nut = new CustomRoundNut at (1.0, 0, 0.85), - with envObjectName "round_nut", with randomPlacement True, with tableIndex 1 +round_nut = new RoundNut at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) -milk = new CustomMilk at (1.0, 0, 0.85), - with envObjectName "milk", with randomPlacement True, with tableIndex 1 +milk = new Milk at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) -cereal = new CustomCereal at (1.0, 0, 0.85), - with envObjectName "cereal", with randomPlacement True, with tableIndex 1 +cereal = new Cereal at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) -# TABLE 3 (Right): custom objects 1 -can = new CustomCan at (0, 1.0, 0.85), - with envObjectName "can", with randomPlacement True, with tableIndex 2 +# TABLE 3 (Right): Various objects with random positions +can = new Can at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) -bread = new CustomBread at (0, 1.0, 0.85), - with envObjectName "bread", with randomPlacement True, with tableIndex 2 +bread = new Bread at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) -bottle = new CustomBottle at (0, 1.0, 0.85), - with envObjectName "bottle", with randomPlacement True, with tableIndex 2 +bottle = new Bottle at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) -hammer = new CustomHammer at (0, 1.0, 0.85), - with envObjectName "hammer", with randomPlacement True, with tableIndex 2 +hammer = new Hammer at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) -# TABLE 4 (Left): Empty is left empty +# TABLE 4 (Left): Intentionally left empty # ROBOT -ego = new PandaRobot at (0, 0, 0) \ No newline at end of file +ego = new Panda at (0, 0, 0) \ No newline at end of file diff --git a/examples/robosuite/stack_lift.scenic b/examples/robosuite/stack_lift.scenic index 47298a73d..63d29ee92 100644 --- a/examples/robosuite/stack_lift.scenic +++ b/examples/robosuite/stack_lift.scenic @@ -5,28 +5,25 @@ model scenic.simulators.robosuite.model param use_environment = "Custom" # param camera_view = "sideview" -# WORKSPACE: Single rectangular table positioned in front (rotated 90 degrees) +# WORKSPACE: Single rectangular table positioned in front work_table = new Table at (0.6, 0, 0.8), - with width 0.6, # Rotated dimensions + with width 0.6, with length 1.2, with height 0.05 # OBJECTS # Bottom cube (larger, stable base) -bottom_cube = new CustomBox at (0.6, 0, 0.83), - with envObjectName "cube_bottom", +bottom_cube = new Box at (0.6, 0, 0.83), with color (0.2, 0.3, 0.8, 1), with width 0.06, with length 0.06, with height 0.06 # Top cube (smaller, stacked on bottom) -top_cube = new CustomBox at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom - with envObjectName "cube_top", +top_cube = new Box at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom with color (0.8, 0.2, 0.2, 1), with width 0.04, with length 0.04, with height 0.04 # Bottle placed separately on table -bottle = new CustomBottle at (0.6, 0.3, 0.83), - with envObjectName "bottle" +bottle = new Bottle at (0.6, 0.3, 0.83) # Custom lift behavior defined locally behavior CustomLift(): @@ -40,5 +37,5 @@ behavior CustomLift(): wait # ROBOT: Using UR5e with custom behavior -ego = new UR5eRobot at (0, 0, 0), +ego = new UR5e at (0, 0, 0), with behavior CustomLift() \ No newline at end of file diff --git a/examples/robosuite/stacked_cubes.scenic b/examples/robosuite/stacked_cubes.scenic index 46f5572bd..c19d52e0f 100644 --- a/examples/robosuite/stacked_cubes.scenic +++ b/examples/robosuite/stacked_cubes.scenic @@ -5,28 +5,25 @@ model scenic.simulators.robosuite.model param use_environment = "Custom" param camera_view = "sideview" -# WORKSPACE: Single rectangular table positioned in front (rotated 90 degrees) +# WORKSPACE: Single rectangular table positioned in front work_table = new Table at (0.6, 0, 0.8), - with width 0.6, # Rotated dimensions + with width 0.6, with length 1.2, with height 0.05 # OBJECTS # Bottom cube (larger, stable base) -bottom_cube = new CustomBox at (0.6, 0, 0.83), - with envObjectName "cube_bottom", +bottom_cube = new Box at (0.6, 0, 0.83), with color (0.2, 0.3, 0.8, 1), with width 0.06, with length 0.06, with height 0.06 # Top cube (smaller, stacked on bottom) -top_cube = new CustomBox at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom - with envObjectName "cube_top", +top_cube = new Box at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom with color (0.8, 0.2, 0.2, 1), with width 0.04, with length 0.04, with height 0.04 # Bottle placed separately on table -bottle = new CustomBottle at (0.6, 0.3, 0.83), - with envObjectName "bottle" +bottle = new Bottle at (0.6, 0.3, 0.83) -# ROBOT: Using UR5e, positioned at origin -ego = new UR5eRobot at (0, 0, 0) \ No newline at end of file +# ROBOT: Using UR5e with simplified naming +ego = new UR5e at (0, 0, 0) \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index d51264ad8..003875c5f 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,16 +1,18 @@ +# src/scenic/simulators/robosuite/model.scenic + """Scenic world model for RoboSuite.""" from .simulator import RobosuiteSimulator, SetJointPositions, OSCPositionAction -# Global parameters with defaults matching Robosuite's defaults +# Global parameters param use_environment = None param env_config = {} param controller_config = None param camera_view = None param render = True -param real_time = True # Robosuite default +param real_time = True param speed = 1.0 -param lite_physics = None # None = use Robosuite default (True) +param lite_physics = None # Simulator simulator RobosuiteSimulator( @@ -66,123 +68,104 @@ class RoboSuiteObject(Object): shape: BoxShape() allowCollisions: True -# Table class for custom environments +# Table for arena setup class Table(RoboSuiteObject): - """Table in custom environment.""" + """Table in environment.""" isTable: True width: DEFAULTS['table_width'] length: DEFAULTS['table_length'] height: DEFAULTS['table_thickness'] position: (0, 0, DEFAULTS['table_height']) -# Base class for custom environment objects -class CustomObject(RoboSuiteObject): - """Base class for custom environment objects.""" - envObjectName: None - randomPlacement: False - tableIndex: 0 - xRange: (-0.1, 0.1) - yRange: (-0.1, 0.1) +# Base class for manipulable objects +class ManipulationObject(RoboSuiteObject): + """Base class for objects that can be manipulated.""" color: DEFAULTS['default_color'] -# Primitive objects for custom environments -class CustomBox(CustomObject): - """Box object for custom environments.""" +# Primitive shape objects (matching RoboSuite's naming) +class Box(ManipulationObject): + """Box object.""" objectType: "Box" width: DEFAULTS['object_size'] length: DEFAULTS['object_size'] height: DEFAULTS['object_size'] -class CustomBall(CustomObject): - """Ball object for custom environments.""" +class Ball(ManipulationObject): + """Ball/sphere object.""" objectType: "Ball" radius: DEFAULTS['object_size'] width: DEFAULTS['object_size'] * 2 # For compatibility length: DEFAULTS['object_size'] * 2 height: DEFAULTS['object_size'] * 2 -class CustomCylinder(CustomObject): - """Cylinder object for custom environments.""" +class Cylinder(ManipulationObject): + """Cylinder object.""" objectType: "Cylinder" width: DEFAULTS['object_size'] * 2 # Diameter length: DEFAULTS['object_size'] * 2 height: DEFAULTS['object_size'] * 4 # Height -class CustomCapsule(CustomObject): - """Capsule object for custom environments.""" +class Capsule(ManipulationObject): + """Capsule object.""" objectType: "Capsule" width: DEFAULTS['object_size'] * 1.5 length: DEFAULTS['object_size'] * 1.5 height: DEFAULTS['object_size'] * 3 -# Complex objects for custom environments -class CustomMilk(CustomObject): - """Milk carton for custom environments.""" +# Complex objects (matching RoboSuite's naming) +class Milk(ManipulationObject): + """Milk carton object.""" objectType: "Milk" -class CustomCereal(CustomObject): - """Cereal box for custom environments.""" +class Cereal(ManipulationObject): + """Cereal box object.""" objectType: "Cereal" -class CustomCan(CustomObject): - """Can object for custom environments.""" +class Can(ManipulationObject): + """Can object.""" objectType: "Can" -class CustomBread(CustomObject): - """Bread object for custom environments.""" +class Bread(ManipulationObject): + """Bread object.""" objectType: "Bread" -class CustomBottle(CustomObject): - """Bottle object for custom environments.""" +class Bottle(ManipulationObject): + """Bottle object.""" objectType: "Bottle" -class CustomHammer(CustomObject): - """Hammer object for custom environments.""" +class Hammer(ManipulationObject): + """Hammer object.""" objectType: "Hammer" -class CustomSquareNut(CustomObject): - """Square nut for custom environments.""" +class SquareNut(ManipulationObject): + """Square nut object.""" objectType: "SquareNut" -class CustomRoundNut(CustomObject): - """Round nut for custom environments.""" +class RoundNut(ManipulationObject): + """Round nut object.""" objectType: "RoundNut" -# Standard environment objects -class Cube(RoboSuiteObject): - """Cubic object with uniform dimensions.""" - width: DEFAULTS['object_size'] - length: DEFAULTS['object_size'] - height: DEFAULTS['object_size'] - color: DEFAULTS['default_color'] - -class EnvironmentObject(RoboSuiteObject): - """Base class for standard environment objects.""" - envObjectName: None - allowCollisions: True - -class LiftCube(EnvironmentObject): - """Cube in Lift environment.""" - envObjectName: "cube" +# Standard environment-specific objects +# These maintain their specific names as they're tied to particular environments +class LiftCube(RoboSuiteObject): + """Cube specific to Lift environment.""" width: DEFAULTS['object_size'] length: DEFAULTS['object_size'] height: DEFAULTS['object_size'] -class StackCubeA(EnvironmentObject): +class StackCubeA(RoboSuiteObject): """First cube in Stack environment.""" - envObjectName: "cubeA" width: DEFAULTS['object_size'] length: DEFAULTS['object_size'] height: DEFAULTS['object_size'] -class StackCubeB(EnvironmentObject): +class StackCubeB(RoboSuiteObject): """Second cube in Stack environment.""" - envObjectName: "cubeB" width: DEFAULTS['object_size'] length: DEFAULTS['object_size'] height: DEFAULTS['object_size'] -# Robots +# Robots (matching RoboSuite's naming) class Robot(RoboSuiteObject): """Base robot class.""" robot_type: "Panda" @@ -199,31 +182,32 @@ class Robot(RoboSuiteObject): eef_pos[dynamic]: [0, 0, 0] gripper_state[dynamic]: [0, 0] -class PandaRobot(Robot): +class Panda(Robot): """Franka Emika Panda robot.""" robot_type: "Panda" gripper_type: "PandaGripper" -class UR5eRobot(Robot): +class UR5e(Robot): """Universal Robots UR5e.""" robot_type: "UR5e" gripper_type: "Robotiq85Gripper" -class SawyerRobot(Robot): +class Sawyer(Robot): """Rethink Robotics Sawyer.""" robot_type: "Sawyer" gripper_type: "RethinkGripper" -class JacoRobot(Robot): +class Jaco(Robot): """Kinova Jaco robot.""" robot_type: "Jaco" gripper_type: "JacoThreeFingerGripper" -class IIWARobot(Robot): +class IIWA(Robot): """KUKA IIWA robot.""" robot_type: "IIWA" gripper_type: "Robotiq140Gripper" + # Behavior Library behavior OpenGripper(steps=DEFAULTS['gripper_open_steps']): """Open gripper over multiple steps.""" @@ -292,5 +276,4 @@ behavior PickAndLift(target_object, height=1.05): do LiftToHeight(height) if simulation().checkSuccess(): - terminate simulation - + terminate simulation \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/simulator.py b/src/scenic/simulators/robosuite/simulator.py index 273a5b6e5..e4159bef2 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -1,3 +1,5 @@ +# src/scenic/simulators/robosuite/simulator.py + """RoboSuite Simulator interface for Scenic.""" from typing import Dict, List, Any, Optional, Union @@ -100,7 +102,7 @@ def _load_model(self): mujoco_arena = self._create_arena() mujoco_arena.set_origin([0, 0, 0]) - self.mujoco_objects, self.placement_samplers = self._create_objects() + self.mujoco_objects = self._create_objects() self.model = ManipulationTask( mujoco_arena=mujoco_arena, @@ -130,10 +132,9 @@ def _create_arena(self): has_legs=[True] * len(self.scenic_tables) ) - def _create_objects(self) -> tuple: + def _create_objects(self) -> List: """Create Robosuite objects from Scenic configuration.""" mujoco_objects = [] - placement_samplers = [] for obj_config in self.scenic_objects: obj_type = obj_config['type'] @@ -142,31 +143,8 @@ def _create_objects(self) -> tuple: factory = OBJECT_FACTORIES.get(obj_type, OBJECT_FACTORIES['Box']) mj_obj = factory(obj_config) mujoco_objects.append(mj_obj) - - # Create placement sampler if needed - if obj_config.get('random_placement', False): - sampler = self._create_placement_sampler(obj_config, mj_obj) - placement_samplers.append(sampler) - - return mujoco_objects, placement_samplers - - def _create_placement_sampler(self, obj_config: Dict, mj_obj) -> UniformRandomSampler: - """Create placement sampler for randomized object placement.""" - table_idx = obj_config.get('table_index', 0) - ref_pos = (self.scenic_tables[table_idx]['position'] - if table_idx < len(self.scenic_tables) - else [0, 0, 0.8]) - - return UniformRandomSampler( - name=f"{obj_config['name']}_sampler", - mujoco_objects=mj_obj, - x_range=obj_config.get('x_range', [-0.1, 0.1]), - y_range=obj_config.get('y_range', [-0.1, 0.1]), - ensure_object_boundary_in_range=False, - ensure_valid_placement=True, - reference_pos=ref_pos, - z_offset=0.01 - ) + + return mujoco_objects def _setup_references(self): """Setup references to simulation objects.""" @@ -198,18 +176,9 @@ def _reset_internal(self): """Reset environment internals.""" super()._reset_internal() - # Apply placement samplers - for sampler in self.placement_samplers: - placement = sampler.sample() - for obj_pos, obj_quat, obj in placement.values(): - self.sim.data.set_joint_qpos( - obj.joints[0], - np.concatenate([np.array(obj_pos), np.array(obj_quat)]) - ) - - # Set fixed positions + # Set positions for all objects for obj_config, mj_obj in zip(self.scenic_objects, self.mujoco_objects): - if not obj_config.get('random_placement', False) and 'position' in obj_config: + if 'position' in obj_config: pos = obj_config['position'] quat = obj_config.get('quaternion', [1, 0, 0, 0]) self.sim.data.set_joint_qpos( @@ -277,9 +246,9 @@ def __init__(self, scene, render: bool, real_time: bool, speed: float, self.model = None self.data = None - # Object tracking + # Object tracking - maps Scenic object to its name self.body_id_map = {} - self.object_names = {} + self.object_name_map = {} # Maps Scenic object to its name in the sim self.robots = [] self.prev_positions = {} self._current_obs = None @@ -386,26 +355,25 @@ def _add_table_config(self, tables: List, obj): def _add_object_config(self, objects: List, obj): """Add object configuration.""" + # Use the Scenic object's name attribute directly + obj_name = getattr(obj, 'name', f"obj_{id(obj)}") + config = { 'type': obj.objectType, - 'name': getattr(obj, 'envObjectName', f"obj_{id(obj)}"), + 'name': obj_name, # Use Scenic's name directly 'position': [obj.position.x, obj.position.y, obj.position.z], - 'color': getattr(obj, 'color', [1, 0, 0, 1]), - 'random_placement': getattr(obj, 'randomPlacement', False), - 'table_index': getattr(obj, 'tableIndex', 0) + 'color': getattr(obj, 'color', [1, 0, 0, 1]) } + # Store mapping + self.object_name_map[obj] = obj_name + # Add size/radius info if hasattr(obj, 'radius'): config['radius'] = obj.radius elif hasattr(obj, 'width'): config['size'] = [obj.width, obj.length, obj.height] - # Add placement ranges - if config['random_placement']: - config['x_range'] = getattr(obj, 'xRange', [-0.1, 0.1]) - config['y_range'] = getattr(obj, 'yRange', [-0.1, 0.1]) - objects.append(config) def _get_robot_arg(self, robots: List) -> Union[str, List[str]]: @@ -434,7 +402,14 @@ def _setup_standard_environment(self): # Store robot references for i, robot in enumerate(robots[:len(self.robosuite_env.robots)]): self.robots.append(robot) - self.object_names[robot] = f"robot{i}" + robot_name = f"robot{i}" + self.object_name_map[robot] = robot_name + + # Map standard environment objects using their Scenic name + for obj in self.objects: + if hasattr(obj, 'name') and not hasattr(obj, 'robot_type'): + # For standard environments, use the object's Scenic name + self.object_name_map[obj] = obj.name self._apply_initial_positions() self._detect_controller_type([{'type': r.robot_type} for r in robots]) @@ -451,20 +426,24 @@ def _detect_controller_type(self, robot_configs: List): def _setup_body_mapping(self): """Map environment objects to MuJoCo body IDs.""" for obj in self.objects: - if hasattr(obj, 'envObjectName'): + 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 BODY_SUFFIXES: - body_name = f"{obj.envObjectName}{suffix}" + 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.envObjectName] = body_id - self.prev_positions[obj.envObjectName] = self.data.xpos[body_id].copy() + self.body_id_map[obj_name] = body_id + self.prev_positions[obj_name] = self.data.xpos[body_id].copy() break except: continue @@ -472,8 +451,9 @@ def _map_object_body(self, obj): def _apply_initial_positions(self): """Apply initial positions for environment objects.""" for obj in self.objects: - if hasattr(obj, 'envObjectName') and hasattr(obj, 'position'): - self._set_object_position(obj) + obj_name = self.object_name_map.get(obj) + if obj_name and hasattr(obj, 'position'): + self._set_object_position(obj, obj_name) self.robosuite_env.sim.forward() @@ -483,9 +463,9 @@ def _apply_initial_positions(self): self._current_obs = self.robosuite_env._get_observations() - def _set_object_position(self, obj): + def _set_object_position(self, obj, obj_name): """Set position for single object.""" - joint_name = f"{obj.envObjectName}{JOINT_SUFFIX}" + joint_name = f"{obj_name}{JOINT_SUFFIX}" try: qpos = np.concatenate([ [obj.position.x, obj.position.y, obj.position.z], @@ -558,13 +538,14 @@ def getProperties(self, obj, properties: List[str]) -> Dict[str, Any]: def _get_object_position(self, obj) -> Vector: """Get object position.""" - if hasattr(obj, 'envObjectName') and self._current_obs: - obs_key = f"{obj.envObjectName}_pos" + obj_name = self.object_name_map.get(obj) + if obj_name and self._current_obs: + obs_key = f"{obj_name}_pos" if obs_key in self._current_obs: pos = self._current_obs[obs_key] return Vector(pos[0], pos[1], pos[2]) - elif obj.envObjectName in self.body_id_map: - body_id = self.body_id_map[obj.envObjectName] + elif 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 From 70272a40e5da118431954ea1671ea6a43fefa00b Mon Sep 17 00:00:00 2001 From: sahil-tgs Date: Mon, 8 Sep 2025 13:13:45 +0530 Subject: [PATCH 8/8] removed suite.make integration --- .../robosuite/cube_random_position.scenic | 34 ---- .../robosuite/dual_table_workspace.scenic | 12 +- .../robosuite/four_table_workspace.scenic | 11 +- examples/robosuite/stack_lift.scenic | 31 +-- examples/robosuite/stacked_cubes.scenic | 14 +- src/scenic/simulators/robosuite/model.scenic | 46 +---- src/scenic/simulators/robosuite/simulator.py | 185 ++++++------------ 7 files changed, 92 insertions(+), 241 deletions(-) delete mode 100644 examples/robosuite/cube_random_position.scenic diff --git a/examples/robosuite/cube_random_position.scenic b/examples/robosuite/cube_random_position.scenic deleted file mode 100644 index da1c9d57a..000000000 --- a/examples/robosuite/cube_random_position.scenic +++ /dev/null @@ -1,34 +0,0 @@ -# examples/robosuite/cube_random_position.scenic -"""Randomized cube position using behavior library.""" - -model scenic.simulators.robosuite.model - -param use_environment = "Lift" -# param camera_view = "sideview" -# press escape in simulater inrterface to reset to robosuite default frontview,for seeing the cube actually being lifted. -# press ']' or '[' to cycle between different fixed camera_views. (pre defined/set in robosuits's task enviroments) -# other options for camera_view: - # "frontview" - # "birdview" - # "agentview" - # "sideview" - # "robot0_robotview" - # "robot0_eye_in_hand" - - -# Randomize cube position -# cube = new LiftCube at (Range(-0.25, 0.25), Range(-0.25, 0.25), 0.82) - -# Clean approach using object reference -ego = new Panda with behavior PickAndLift(cube, height=1.05) - -# other robot options: UR5eRobot - -# Alternative: Custom composition -# behavior CustomLift(): -# do PickObject(cube) -# do LiftToHeight(1.05) -# if simulation().checkSuccess(): -# terminate simulation - -# ego = new PandaRobot with behavior CustomLift() \ No newline at end of file diff --git a/examples/robosuite/dual_table_workspace.scenic b/examples/robosuite/dual_table_workspace.scenic index 7a297c268..c4824c746 100644 --- a/examples/robosuite/dual_table_workspace.scenic +++ b/examples/robosuite/dual_table_workspace.scenic @@ -1,18 +1,14 @@ # examples/robosuite/dual_table_workspace.scenic model scenic.simulators.robosuite.model -# SCENARIO CONFIGURATION -param use_environment = "Custom" - -# WORKSPACE SETUP +# Arena Setup back_table = new Table at (-0.6, 0, 0.8) front_table = new Table at (0.6, 0, 0.8) -# OBJECTS WITH RANDOM PLACEMENT USING RANGE -# Using Range with modern tuple syntax for randomness -red_cube = new Box at (Range(-0.10, -0.15), Range(-0.11, 0.22), 0.83), +# OBJECTS +red_cube = new Box at (Range(-0.7, -0.5), Range(-0.1, 0.1), 0.83), with color (1, 0, 0, 1), - with width 0.04, with length 0.04, with height 0.04 + green_cube = new Box at (Range(0.5, 0.7), Range(-0.1, 0.1), 0.83), with color (0, 1, 0, 1), diff --git a/examples/robosuite/four_table_workspace.scenic b/examples/robosuite/four_table_workspace.scenic index edb6de9bf..456f42cc0 100644 --- a/examples/robosuite/four_table_workspace.scenic +++ b/examples/robosuite/four_table_workspace.scenic @@ -1,9 +1,8 @@ # examples/robosuite/four_table_workspace.scenic model scenic.simulators.robosuite.model -# SCENARIO CONFIGURATION -param use_environment = "Custom" -param camera_view = "sideview" +# CAMERA CONFIGURATION +# param camera_view = "sideview" TABLE_DISTANCE = 1.0 @@ -13,7 +12,7 @@ front_table = new Table at (TABLE_DISTANCE, 0, 0.8) right_table = new Table at (0, TABLE_DISTANCE, 0.8) left_table = new Table at (0, -TABLE_DISTANCE, 0.8) -# TABLE 1 (Back): Primitive objects with random positions using Range +# TABLE 1 (Back): Primitive objects with random positions ball = new Ball at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), with color (1, 0.5, 0, 1) @@ -26,7 +25,7 @@ capsule = new Capsule at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), cylinder = new Cylinder at (Range(-1.1, -0.9), Range(-0.1, 0.1), 0.83), with color (0, 1, 1, 1) -# TABLE 2 (Front): Nuts and food items with random positions +# TABLE 2 (Front): Nuts and food items square_nut = new SquareNut at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) round_nut = new RoundNut at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) @@ -35,7 +34,7 @@ milk = new Milk at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) cereal = new Cereal at (Range(0.9, 1.1), Range(-0.1, 0.1), 0.85) -# TABLE 3 (Right): Various objects with random positions +# TABLE 3 (Right): Various objects can = new Can at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) bread = new Bread at (Range(-0.1, 0.1), Range(0.9, 1.1), 0.85) diff --git a/examples/robosuite/stack_lift.scenic b/examples/robosuite/stack_lift.scenic index 63d29ee92..488003e02 100644 --- a/examples/robosuite/stack_lift.scenic +++ b/examples/robosuite/stack_lift.scenic @@ -1,41 +1,28 @@ -# examples/robosuite/stack_lift.scenic model scenic.simulators.robosuite.model -# SCENARIO CONFIGURATION -param use_environment = "Custom" -# param camera_view = "sideview" - -# WORKSPACE: Single rectangular table positioned in front work_table = new Table at (0.6, 0, 0.8), with width 0.6, with length 1.2, with height 0.05 -# OBJECTS -# Bottom cube (larger, stable base) bottom_cube = new Box at (0.6, 0, 0.83), with color (0.2, 0.3, 0.8, 1), with width 0.06, with length 0.06, with height 0.06 -# Top cube (smaller, stacked on bottom) -top_cube = new Box at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom +top_cube = new Box at (0.6, 0, 0.89) with color (0.8, 0.2, 0.2, 1), - with width 0.04, with length 0.04, with height 0.04 -# Bottle placed separately on table bottle = new Bottle at (0.6, 0.3, 0.83) -# Custom lift behavior defined locally -behavior CustomLift(): - do PickObject(top_cube) +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) - - # Check if cube is above threshold height - for _ in range(10): # Check for 10 steps - if top_cube.position.z > 1.0: + for _ in range(10): + if pickup_object.position.z > 1.0: terminate simulation wait - -# ROBOT: Using UR5e with custom behavior ego = new UR5e at (0, 0, 0), - with behavior CustomLift() \ No newline at end of file + with behavior StackLift() \ No newline at end of file diff --git a/examples/robosuite/stacked_cubes.scenic b/examples/robosuite/stacked_cubes.scenic index c19d52e0f..7fc0ecd0f 100644 --- a/examples/robosuite/stacked_cubes.scenic +++ b/examples/robosuite/stacked_cubes.scenic @@ -1,29 +1,19 @@ -# examples/robosuite/stacked_cubes.scenic model scenic.simulators.robosuite.model -# SCENARIO CONFIGURATION -param use_environment = "Custom" -param camera_view = "sideview" +# param camera_view = "sideview" -# WORKSPACE: Single rectangular table positioned in front work_table = new Table at (0.6, 0, 0.8), with width 0.6, with length 1.2, with height 0.05 -# OBJECTS -# Bottom cube (larger, stable base) bottom_cube = new Box at (0.6, 0, 0.83), with color (0.2, 0.3, 0.8, 1), with width 0.06, with length 0.06, with height 0.06 -# Top cube (smaller, stacked on bottom) -top_cube = new Box at (0.6, 0, 0.89), # 0.83 + 0.06 height of bottom +top_cube = new Box at (0.6, 0, 0.89), with color (0.8, 0.2, 0.2, 1), - with width 0.04, with length 0.04, with height 0.04 -# Bottle placed separately on table bottle = new Bottle at (0.6, 0.3, 0.83) -# ROBOT: Using UR5e with simplified naming ego = new UR5e at (0, 0, 0) \ No newline at end of file diff --git a/src/scenic/simulators/robosuite/model.scenic b/src/scenic/simulators/robosuite/model.scenic index 003875c5f..0cf1a794e 100644 --- a/src/scenic/simulators/robosuite/model.scenic +++ b/src/scenic/simulators/robosuite/model.scenic @@ -1,25 +1,23 @@ # src/scenic/simulators/robosuite/model.scenic -"""Scenic world model for RoboSuite.""" +"""Scenic world model for RoboSuite - Custom Environments Only.""" from .simulator import RobosuiteSimulator, SetJointPositions, OSCPositionAction -# Global parameters -param use_environment = None +# Global parameters with defaults matching Robosuite's defaults param env_config = {} param controller_config = None param camera_view = None param render = True -param real_time = True +param real_time = True param speed = 1.0 -param lite_physics = None +param lite_physics = None # None = use Robosuite default (True) -# Simulator +# Simulator - no more use_environment parameter simulator RobosuiteSimulator( render=globalParameters.render, real_time=globalParameters.real_time, speed=globalParameters.speed, - use_environment=globalParameters.use_environment, env_config=globalParameters.env_config, controller_config=globalParameters.controller_config, camera_view=globalParameters.camera_view, @@ -29,7 +27,7 @@ simulator RobosuiteSimulator( # Default values dictionary DEFAULTS = { # Object properties - 'object_size': 0.05, + 'object_size': 0.03, 'density': 1000, 'friction': (1.0, 0.005, 0.0001), 'solref': (0.02, 1.0), @@ -94,16 +92,16 @@ class Ball(ManipulationObject): """Ball/sphere object.""" objectType: "Ball" radius: DEFAULTS['object_size'] - width: DEFAULTS['object_size'] * 2 # For compatibility + width: DEFAULTS['object_size'] * 2 length: DEFAULTS['object_size'] * 2 height: DEFAULTS['object_size'] * 2 class Cylinder(ManipulationObject): """Cylinder object.""" objectType: "Cylinder" - width: DEFAULTS['object_size'] * 2 # Diameter + width: DEFAULTS['object_size'] * 2 length: DEFAULTS['object_size'] * 2 - height: DEFAULTS['object_size'] * 4 # Height + height: DEFAULTS['object_size'] * 4 class Capsule(ManipulationObject): """Capsule object.""" @@ -145,26 +143,6 @@ class RoundNut(ManipulationObject): """Round nut object.""" objectType: "RoundNut" -# Standard environment-specific objects -# These maintain their specific names as they're tied to particular environments -class LiftCube(RoboSuiteObject): - """Cube specific to Lift environment.""" - width: DEFAULTS['object_size'] - length: DEFAULTS['object_size'] - height: DEFAULTS['object_size'] - -class StackCubeA(RoboSuiteObject): - """First cube in Stack environment.""" - width: DEFAULTS['object_size'] - length: DEFAULTS['object_size'] - height: DEFAULTS['object_size'] - -class StackCubeB(RoboSuiteObject): - """Second cube in Stack environment.""" - width: DEFAULTS['object_size'] - length: DEFAULTS['object_size'] - height: DEFAULTS['object_size'] - # Robots (matching RoboSuite's naming) class Robot(RoboSuiteObject): """Base robot class.""" @@ -207,7 +185,6 @@ class IIWA(Robot): robot_type: "IIWA" gripper_type: "Robotiq140Gripper" - # Behavior Library behavior OpenGripper(steps=DEFAULTS['gripper_open_steps']): """Open gripper over multiple steps.""" @@ -273,7 +250,4 @@ behavior PickObject(target_object): behavior PickAndLift(target_object, height=1.05): """Complete pick and lift for specific object.""" do PickObject(target_object) - do LiftToHeight(height) - - if simulation().checkSuccess(): - terminate simulation \ No newline at end of file + 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 index e4159bef2..ff9e8436f 100644 --- a/src/scenic/simulators/robosuite/simulator.py +++ b/src/scenic/simulators/robosuite/simulator.py @@ -1,6 +1,6 @@ # src/scenic/simulators/robosuite/simulator.py -"""RoboSuite Simulator interface for Scenic.""" +"""RoboSuite Simulator interface for Scenic - Custom Environment Only.""" from typing import Dict, List, Any, Optional, Union import numpy as np @@ -9,14 +9,13 @@ try: import robosuite as suite from robosuite.environments.manipulation.manipulation_env import ManipulationEnv - from robosuite.models.arenas import TableArena, MultiTableArena + from robosuite.models.arenas import EmptyArena, MultiTableArena from robosuite.models.tasks import ManipulationTask from robosuite.models.objects import ( BallObject, BoxObject, CylinderObject, CapsuleObject, MilkObject, CerealObject, CanObject, BreadObject, HammerObject, SquareNutObject, RoundNutObject, BottleObject ) - from robosuite.utils.placement_samplers import UniformRandomSampler from robosuite.utils.observables import Observable, sensor except ImportError as e: suite = None @@ -48,9 +47,10 @@ # Object type mapping OBJECT_FACTORIES = { + # Primitive objects with customizable size 'Ball': lambda cfg: BallObject( name=cfg['name'], - size=[cfg.get('radius', cfg.get('size', [0.02])[0])], + size=[cfg.get('radius', 0.02)], rgba=cfg.get('color', [1, 0, 0, 1]) ), 'Box': lambda cfg: BoxObject( @@ -60,12 +60,15 @@ ), 'Cylinder': lambda cfg: CylinderObject( name=cfg['name'], - size=_extract_cylinder_size(cfg) + size=_extract_cylinder_size(cfg), + rgba=cfg.get('color', [1, 0, 0, 1]) ), 'Capsule': lambda cfg: CapsuleObject( name=cfg['name'], - size=_extract_cylinder_size(cfg) + size=_extract_cylinder_size(cfg), + rgba=cfg.get('color', [1, 0, 0, 1]) ), + # Complex objects with fixed sizes - no size parameter 'Milk': lambda cfg: MilkObject(name=cfg['name']), 'Cereal': lambda cfg: CerealObject(name=cfg['name']), 'Can': lambda cfg: CanObject(name=cfg['name']), @@ -79,14 +82,20 @@ def _extract_cylinder_size(config: Dict) -> List[float]: """Extract [radius, height] for cylinder-like objects.""" - size = config.get('size', [0.02, 0.04]) + if 'radius' in config and 'height' in config: + return [config['radius'], config['height']] + + size = config.get('size', [0.02, 0.02, 0.04]) if len(size) == 3: # Convert from [width, length, height] - return [size[0] / 2, size[2]] - return size + radius = (size[0] + size[1]) / 4 + return [radius, size[2]] + elif len(size) == 2: + return size + return [0.02, 0.04] # Default -class CustomManipulationEnv(ManipulationEnv): - """Custom manipulation environment for Scenic-defined scenarios.""" +class ScenicManipulationEnv(ManipulationEnv): + """Scenic-driven manipulation environment.""" def __init__(self, scenic_config: Dict, **kwargs): self.scenic_config = scenic_config @@ -121,11 +130,9 @@ def _create_arena(self): """Create arena based on table configuration.""" if not self.scenic_tables: # No tables - use empty arena (just floor) - from robosuite.models.arenas import EmptyArena return EmptyArena() - # Use MultiTableArena for any number of tables (1+) - from robosuite.models.arenas import MultiTableArena + # Use MultiTableArena for any number of tables return MultiTableArena( table_offsets=[t.get('position', [0, 0, 0.8]) for t in self.scenic_tables], table_full_sizes=[t.get('size', (0.8, 0.8, 0.05)) for t in self.scenic_tables], @@ -138,8 +145,6 @@ def _create_objects(self) -> List: for obj_config in self.scenic_objects: obj_type = obj_config['type'] - - # Create object using factory or default to box factory = OBJECT_FACTORIES.get(obj_type, OBJECT_FACTORIES['Box']) mj_obj = factory(obj_config) mujoco_objects.append(mj_obj) @@ -196,12 +201,11 @@ def _check_success(self) -> bool: class RobosuiteSimulator(Simulator): - """Simulator for RoboSuite environments.""" + """Simulator for RoboSuite custom environments.""" def __init__(self, render: bool = True, real_time: bool = True, speed: float = 1.0, - use_environment: Optional[str] = None, env_config: Optional[Dict] = None, - controller_config: Optional[Dict] = None, camera_view: Optional[str] = None, - lite_physics: Optional[bool] = None): + env_config: Optional[Dict] = None, controller_config: Optional[Dict] = None, + camera_view: Optional[str] = None, lite_physics: Optional[bool] = None): super().__init__() if suite is None: raise RuntimeError(f"Unable to import RoboSuite: {_import_error}") @@ -209,7 +213,6 @@ def __init__(self, render: bool = True, real_time: bool = True, speed: float = 1 self.render = render self.real_time = real_time self.speed = speed - self.use_environment = use_environment self.env_config = env_config or {} self.controller_config = controller_config self.camera_view = camera_view @@ -219,23 +222,20 @@ def createSimulation(self, scene, **kwargs): """Create a simulation instance.""" return RobosuiteSimulation( scene, self.render, self.real_time, self.speed, - self.use_environment, self.env_config, - self.controller_config, self.camera_view, - self.lite_physics, **kwargs + self.env_config, self.controller_config, + self.camera_view, self.lite_physics, **kwargs ) class RobosuiteSimulation(Simulation): - """Simulation for RoboSuite environments.""" + """Simulation for RoboSuite custom environments.""" def __init__(self, scene, render: bool, real_time: bool, speed: float, - use_environment: Optional[str], env_config: Optional[Dict], - controller_config: Optional[Dict], camera_view: Optional[str], - lite_physics: Optional[bool], **kwargs): + env_config: Optional[Dict], controller_config: Optional[Dict], + camera_view: Optional[str], lite_physics: Optional[bool], **kwargs): self.render = render self.real_time = real_time self.speed = speed - self.use_environment = use_environment self.env_config = env_config or {} self.controller_config = controller_config self.camera_view = camera_view @@ -246,9 +246,9 @@ def __init__(self, scene, render: bool, real_time: bool, speed: float, self.model = None self.data = None - # Object tracking - maps Scenic object to its name + # Object tracking self.body_id_map = {} - self.object_name_map = {} # Maps Scenic object to its name in the sim + self.object_name_map = {} self.robots = [] self.prev_positions = {} self._current_obs = None @@ -270,14 +270,34 @@ def setup(self): """Initialize the RoboSuite environment.""" super().setup() - if not self.use_environment: - raise ValueError("Environment name must be specified via 'use_environment' parameter") + # Extract configuration from Scenic scene + scenic_config = self._extract_scenic_config() + + # Check for robots - at least one required + if not scenic_config['robots']: + raise ValueError("At least one robot is required in the scene") - if self.use_environment.lower() == "custom": - self._setup_custom_environment() - else: - self._setup_standard_environment() + # Setup environment + robot_arg = self._get_robot_arg(scenic_config['robots']) + env_kwargs = { + 'scenic_config': scenic_config, + '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(scenic_config['robots']) + + # Finalize setup self._finalize_setup() def _finalize_setup(self): @@ -302,28 +322,6 @@ def _set_camera_view(self): if self.robosuite_env.viewer: self.robosuite_env.viewer.set_camera(camera_id=camera_id) - def _setup_custom_environment(self): - """Setup custom manipulation environment.""" - scenic_config = self._extract_scenic_config() - robot_arg = self._get_robot_arg(scenic_config['robots']) - - env_kwargs = { - 'scenic_config': scenic_config, - '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 = CustomManipulationEnv(**env_kwargs) - self._current_obs = self.robosuite_env.reset() - self._detect_controller_type(scenic_config['robots']) - def _extract_scenic_config(self) -> Dict: """Extract configuration from Scenic scene.""" config = {'robots': [], 'tables': [], 'objects': []} @@ -360,7 +358,7 @@ def _add_object_config(self, objects: List, obj): config = { 'type': obj.objectType, - 'name': obj_name, # Use Scenic's name directly + 'name': obj_name, 'position': [obj.position.x, obj.position.y, obj.position.z], 'color': getattr(obj, 'color', [1, 0, 0, 1]) } @@ -368,10 +366,9 @@ def _add_object_config(self, objects: List, obj): # Store mapping self.object_name_map[obj] = obj_name - # Add size/radius info - if hasattr(obj, 'radius'): + if obj.objectType == 'Ball' and hasattr(obj, 'radius'): config['radius'] = obj.radius - elif hasattr(obj, 'width'): + elif obj.objectType in ['Box', 'Cylinder', 'Capsule'] and hasattr(obj, 'width'): config['size'] = [obj.width, obj.length, obj.height] objects.append(config) @@ -379,41 +376,10 @@ def _add_object_config(self, objects: List, obj): def _get_robot_arg(self, robots: List) -> Union[str, List[str]]: """Get robot argument for environment creation.""" if not robots: - return "Panda" + raise ValueError("At least one robot is required") return ([r['type'] for r in robots] if len(robots) > 1 else robots[0]['type']) - def _setup_standard_environment(self): - """Setup standard RoboSuite environment.""" - robots = [obj for obj in self.objects if hasattr(obj, 'robot_type')] - robot_arg = self._get_robot_arg([{'type': r.robot_type} for r in robots]) - - config = { - **self.env_config, - 'has_renderer': self.render, - 'render_camera': self.camera_view, - 'camera_names': ["frontview", "robot0_eye_in_hand"], - 'controller_configs': self.controller_config - } - - self.robosuite_env = suite.make(self.use_environment, robots=robot_arg, **config) - self._current_obs = self.robosuite_env.reset() - - # Store robot references - for i, robot in enumerate(robots[:len(self.robosuite_env.robots)]): - self.robots.append(robot) - robot_name = f"robot{i}" - self.object_name_map[robot] = robot_name - - # Map standard environment objects using their Scenic name - for obj in self.objects: - if hasattr(obj, 'name') and not hasattr(obj, 'robot_type'): - # For standard environments, use the object's Scenic name - self.object_name_map[obj] = obj.name - - self._apply_initial_positions() - self._detect_controller_type([{'type': r.robot_type} for r in robots]) - def _detect_controller_type(self, robot_configs: List): """Detect controller type from first robot.""" if robot_configs and self.robosuite_env.robots: @@ -448,36 +414,9 @@ def _map_object_body(self, obj): except: continue - def _apply_initial_positions(self): - """Apply initial positions for environment objects.""" - for obj in self.objects: - obj_name = self.object_name_map.get(obj) - if obj_name and hasattr(obj, 'position'): - self._set_object_position(obj, obj_name) - - self.robosuite_env.sim.forward() - - # Let physics settle - for _ in range(PHYSICS_SETTLE_STEPS): - self.robosuite_env.step(np.zeros(self.action_dim)) - - self._current_obs = self.robosuite_env._get_observations() - - def _set_object_position(self, obj, obj_name): - """Set position for single object.""" - joint_name = f"{obj_name}{JOINT_SUFFIX}" - try: - qpos = np.concatenate([ - [obj.position.x, obj.position.y, obj.position.z], - [1, 0, 0, 0] - ]) - self.robosuite_env.sim.data.set_joint_qpos(joint_name, qpos) - except: - pass - def createObjectInSimulator(self, obj): """Required by Scenic's Simulator interface.""" - """See _setup_custom_environment() and _setup_standard_environment() for object creation.""" + # Objects are created during setup in ScenicManipulationEnv pass def executeActions(self, allActions: Dict[Any, List]) -> None: