[Frozen Code Start]  
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  

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 ===  
        positions = get_object_positions()  

        # TODO: Replace these placeholder keys with your actual objects  
        # object_1_pos = positions['object_1']  
        # object_2_pos = positions['object_2']  

        # === Example Task Plan (Generic) ===  
        #  
        # Here’s where you would define the logical steps of your task. For instance:  
        #  
        # 1) Approach and manipulate object_1  
        # 2) Move or interact with object_1 in some way  
        # 3) Approach and manipulate object_2  
        # 4) Complete the task  
        #  
        # Since this is a skeleton, we don't include any details on "pick" or "place"  
        # or other skills. Instead, you’d add them here if/when needed.  
        #  
        # Example (commented out):  
        #  
        # print("[Task] Manipulating object_1 at:", object_1_pos)  
        # obs, reward, done = pick(  
        #     env,  
        #     task,  
        #     target_pos=object_1_pos,  
        #     approach_distance=0.15,  
        #     max_steps=100,  
        #     threshold=0.01,  
        #     approach_axis='z',  
        #     timeout=10.0  
        # )  
        #  
        # if done:  
        #     print("[Task] Task ended after manipulating object_1!")  
        #     return  
        #  
        # print("[Task] Placing object_1 at:", object_2_pos)  
        # obs, reward, done = place(  
        #     env,  
        #     task,  
        #     target_pos=object_2_pos,  
        #     approach_distance=0.15,  
        #     max_steps=100,  
        #     threshold=0.01,  
        #     approach_axis='z',  
        #     timeout=10.0  
        # )  
        # if done:  
        #     print("[Task] Task completed successfully! Reward:", reward)  
        # else:  
        #     print("[Task] Task not completed yet (done=False).")  
        #  
        # TODO: Continue adding more plan steps as needed...  

    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]  


# --- Plan Implementation ---  
# Insert plan steps here, after the frozen skeleton above.  

import numpy as np  
from env import shutdown_environment  # ensure this import is available  

def run_skeleton_task():  
    print("===== Starting Skeleton Task =====")  
    env, task = setup_environment()  
    try:  
        descriptions, obs = task.reset()  
        init_video_writers(obs)  
        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  
        positions = get_object_positions()  
        try:  
            rubbish_pos = np.array(positions['rubbish'], dtype=np.float32)  
            bin_pos     = np.array(positions['bin'], dtype=np.float32)  
        except KeyError as e:  
            print(f"Error: Missing position for {e}")  
            return  

        # Step 1: Pick up the rubbish from the table  
        print("[Task] Step 1: Picking up 'rubbish' at:", rubbish_pos)  
        obs, reward, done = pick(  
            env,  
            task,  
            target_pos        = rubbish_pos,  
            approach_distance = 0.15,  
            max_steps         = 100,  
            threshold         = 0.01,  
            approach_axis     = 'z',  
            timeout           = 10.0  
        )  
        if done:  
            print("[Task] Task ended during pick. Reward:", reward)  
            return  

        # Step 2: Place the rubbish into the bin  
        print("[Task] Step 2: Placing 'rubbish' into 'bin' at:", bin_pos)  
        obs, reward, done = place(  
            env,  
            task,  
            target_pos        = bin_pos,  
            approach_distance = 0.15,  
            max_steps         = 100,  
            threshold         = 0.01,  
            approach_axis     = 'z',  
            timeout           = 10.0  
        )  
        if done:  
            print("[Task] Task completed successfully! Reward:", reward)  
        else:  
            print("[Task] Task not completed. done flag =", done)  

    finally:  
        shutdown_environment(env)  

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


if __name__ == "__main__":  
    run_skeleton_task()  