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)"""


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

pick_place_demo_prompt = [
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="0.034 0.617 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="0.041 0.889 0.160"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (0.034)
    2. Align with object's y position (0.617)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (0.041, 0.889, 0.160 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.034
    [MOVE_Y] 0.617
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] 0.041 0.889 0.160 grab
""",
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="0.029 0.607 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="0.011 0.882 0.200"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (0.029)
    2. Align with object's y position (0.607)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (0.011, 0.882, 0.200 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.029
    [MOVE_Y] 0.607
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] 0.011 0.882 0.200 grab
""",
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="-0.041 0.629 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="0.035 0.897 0.231"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (-0.041)
    2. Align with object's y position (0.629)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (0.035, 0.897, 0.231 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] -0.041
    [MOVE_Y] 0.629
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] 0.035 0.897 0.231 grab
""",
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="0.028 0.647 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="-0.034 0.894 0.181"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (0.028)
    2. Align with object's y position (0.647)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (-0.034, 0.894, 0.181 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] 0.028
    [MOVE_Y] 0.647
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] -0.034 0.894 0.181 grab
""",
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="-0.032 0.649 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="0.014 0.873 0.090"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (-0.032)
    2. Align with object's y position (0.649)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (0.014, 0.873, 0.090 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] -0.032
    [MOVE_Y] 0.649
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] 0.014 0.873 0.090 grab
""",
    f"""
### Task Instruction: 
    Pick and Place Object to Goal

### Object Kinematic Knowledge:
    <hand pos="0.006 0.600 0.194"/>
    <object pos="-0.002 0.604 0.015" type="cylinder" pos="0 0 0" size="0.02 0.02"/>
    <goal pos="0.029 0.872 0.065"/>

{guidance_prompt}

{action_prompt}

### Abstract Manipulation Sequence:
    1. Align with object's x position (-0.002)
    2. Align with object's y position (0.604)
    3. Descend hand to place object between grippers (0.060)
    4. Grip the object
    5. Move the object toward goal position while grabbing (0.029, 0.872, 0.065 grab)

### 3D Manipulation Waypoints:
    [MOVE_X] -0.002
    [MOVE_Y] 0.604
    [MOVE_Z] 0.060
    [GRIP]
    [PUSH] 0.029 0.872 0.065 grab
""",
]
