[Frozen Code Start]

# run_skeleton_task.py (Fully Generic Skeleton)
#
# Description:
#  - This code is a skeleton that shows how to set up an environment, retrieve
#    object positions, and execute a plan without being tied to a specific skill
#    like pick or place.
#  - Use this as a base for creating your own task logic.
#  - The sections that are task-dependent (e.g., manipulation steps) are marked
#    with TODO or placeholders.

import numpy as np
from pyrep.objects.shape import Shape
from pyrep.objects.proximity_sensor import ProximitySensor

from env import setup_environment, shutdown_environment

from skill_code import *

from video import init_video_writers, recording_step, recording_get_observation

# This module is assumed to provide object positions or other info about the environment
from object_positions import get_object_positions


def run_skeleton_task():
    '''Generic skeleton for running any task in your simulation.'''
    print("===== Starting Skeleton Task =====")
    
    # === Environment Setup ===
    env, task = setup_environment()
    try:
        # Reset the task to its initial state
        descriptions, obs = task.reset()

        # (Optional) Initialize video writers for capturing your simulation
        init_video_writers(obs)

        # Wrap the task steps for recording (if needed)
        original_step = task.step
        task.step = recording_step(original_step)
        original_get_obs = task.get_observation
        task.get_observation = recording_get_observation(original_get_obs)

        # === Retrieve Object Positions ===
        # Example usage: positions = 'object_1': (0,0,0), 'object_2': (1,1,1), ... (dictionary format)
        positions = get_object_positions()

        # === Task-Specific Plan Execution ===

        # Step 1: Rotate the gripper from zero to ninety degrees
        print("[Task] Rotating gripper to ninety degrees")
        zero_quat = positions.get('zero_deg')
        ninety_quat = positions.get('ninety_deg')
        if zero_quat is None or ninety_quat is None:
            raise KeyError("Missing quaternion for zero_deg or ninety_deg in positions")
        obs, reward, done = rotate(env, task, target_quat=ninety_quat)
        if done:
            print("[Task] Finished early during rotation. Reward:", reward)
            return

        # Step 2: Move to the side position of the bottom drawer
        print("[Task] Moving to side position of bottom drawer")
        side_pos = positions.get('bottom_side_pos')
        if side_pos is None:
            raise KeyError("Missing position for bottom_side_pos")
        obs, reward, done = move(env, task, target_pos=side_pos)
        if done:
            print("[Task] Finished early during move to side. Reward:", reward)
            return

        # Step 3: Move to the anchor position of the bottom drawer
        print("[Task] Moving to anchor position of bottom drawer")
        anchor_pos = positions.get('bottom_anchor_pos')
        if anchor_pos is None:
            raise KeyError("Missing position for bottom_anchor_pos")
        obs, reward, done = move(env, task, target_pos=anchor_pos)
        if done:
            print("[Task] Finished early during move to anchor. Reward:", reward)
            return

        # Step 4: Pick the drawer handle (simulate pick-drawer)
        print("[Task] Grasping the drawer handle")
        obs, reward, done = pick(env, task, target_pos=anchor_pos)
        if done:
            print("[Task] Finished early during pick-drawer. Reward:", reward)
            return

        # Step 5: Pull the drawer open
        print("[Task] Pulling the drawer open")
        # Choose a reasonable pull distance along x-axis to open the drawer
        pull_distance = 0.1
        obs, reward, done = pull(env, task, pull_distance=pull_distance, pull_axis='x')
        if done:
            print("[Task] Finished early during pull. Reward:", reward)
            return

        # Steps 6 & 7+: Transfer the two tomatoes onto the plate
        for tomato in ['tomato1', 'tomato2']:
            print(f"[Task] Picking up {tomato}")
            tom_pos = positions.get(tomato)
            if tom_pos is None:
                raise KeyError(f"Missing position for {tomato}")
            obs, reward, done = pick(env, task, target_pos=tom_pos)
            if done:
                print(f"[Task] Finished early during picking {tomato}. Reward:", reward)
                return

            print(f"[Task] Placing {tomato} on the plate")
            plate_pos = positions.get('plate')
            if plate_pos is None:
                raise KeyError("Missing position for plate")
            obs, reward, done = place(env, task, target_pos=plate_pos)
            if done:
                print(f"[Task] Task completed after placing {tomato}. Reward:", reward)
                return

        print("[Task] All tomatoes transferred to the plate. Final Reward:", reward)

    finally:
        # Always ensure the environment is properly shutdown
        shutdown_environment(env)

    print("===== End of Skeleton Task =====")


if __name__ == "__main__":
    run_skeleton_task()

[Frozen Code End]