guidance_prompt = """
### Guidance: 
    First generate abstract sequence, further convert into waypoints"""

action_prompt = """
### Actions for execution:
    [MOVE_X] target_x (grab)
    [MOVE_Y] target_y (grab)
    [MOVE_Z] target_z (grab)
    [GRIP] 
    [PUSH] target_x target_y target_z (grab)
    [PULL] target_x target_y target_z (grab)"""


####################################################################################################

peg_insert_side_demo_prompt = [
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.190 0.583 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.520 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.190)
    2. Align with peg's y position (0.583)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.520 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.190
    [MOVE_Y] 0.583
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.520 grab 
    [MOVE_X] -0.350 grab
""",
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.000 0.644 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.623 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.000)
    2. Align with peg's y position (0.644)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.623 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.000
    [MOVE_Y] 0.644
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.623 grab 
    [MOVE_X] -0.350 grab
""",
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.093 0.619 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.453 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.093)
    2. Align with peg's y position (0.619)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.453 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.093
    [MOVE_Y] 0.619
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.453 grab 
    [MOVE_X] -0.350 grab
""",
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.017 0.614 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.612 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.017)
    2. Align with peg's y position (0.614)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.612 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.017
    [MOVE_Y] 0.614
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.612 grab 
    [MOVE_X] -0.350 grab
""",
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.146 0.533 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.466 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.146)
    2. Align with peg's y position (0.533)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.466 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.146
    [MOVE_Y] 0.533
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.466 grab 
    [MOVE_X] -0.350 grab
""",
    f"""
### Task Instruction: 
    Insert Peg to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <peg pos="0.066 0.518 0.020" type="box" euler="0 1.57 0" size="0.015 0.015 0.12"/>
    <goal pos="-0.350 0.548 0.170"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with peg's x position (0.066)
    2. Align with peg's y position (0.518)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Align with goal's z position while grabbing (0.170 grab)
    6. Align with goal's y position while grabbing (0.548 grab)
    7. Move object toward goal position in x axis while grabbing (-0.350 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.066
    [MOVE_Y] 0.518
    [MOVE_Z] 0.060
    [GRIP]
    [MOVE_Z] 0.170 grab
    [MOVE_Y] 0.548 grab 
    [MOVE_X] -0.350 grab
""",
]
