#define system
{{{system_prompt_panda}}}

{{{bridge_setup_code}}}

{{{skill_preface_rai}}}

{{{bridge_skills_sr}}}

{{{cap_role}}}

#define user
initial=BridgeState(frames=[Frame(name="block_red", x_pos=0.0, y_pos=0.0, z_pos=0.71, x_rot=-0.0, y_rot=0.0, z_rot=-0.0, size=[0.04, 0.04, 0.12, 0.0], color="[255, 0, 0]"), Frame(name="block_green", x_pos=0.15, y_pos=0.0, z_pos=0.71, x_rot=-0.0, y_rot=0.0, z_rot=-0.0, size=[0.04, 0.04, 0.12, 0.0], color="[0, 255, 0]"), Frame(name="block_blue", x_pos=0.3, y_pos=0.0, z_pos=0.71, x_rot=-0.0, y_rot=0.0, z_rot=-0.0, size=[0.04, 0.04, 0.12, 0.0], color="[0, 0, 255]"), Frame(name="l_gripper", x_pos=0.0, y_pos=0.28, z_pos=1.27, x_rot=0.5, y_rot=-0.0, z_rot=1.29, size=[0.03], color="[229, 229, 229]"), Frame(name="table", x_pos=0.0, y_pos=0.0, z_pos=0.6, x_rot=-0.0, y_rot=0.0, z_rot=-0.0, size=[1.0, 1.0, 0.1, 0.02], color="[76, 76, 76]")])
Goal: Build a bridge. A bridge is defined as two vertical blocks next to each other and one horizontal block on top of them.


#define assistant
```python
def gen_plan(state: BridgeState):

    import numpy as np

    center = [.0, .4]
    yaw = 0.
    
    # Build the bridge
    actions = []

    block_size_z = state.getFrame("block_red").size[2]
    
    # Red #
    pos_x = np.cos(yaw) * block_size_z * .5 + center[0]
    pos_y = -np.sin(yaw) * block_size_z * .5 + center[1]
    actions.append(Action("pick", ["block_red", None]))
    actions.append(Action("place_sr", [pos_x, pos_y, None, None, None]))

    # Green #
    pos_x = -np.cos(yaw) * block_size_z * .5 + center[0]
    pos_y = np.sin(yaw) * block_size_z * .5 + center[1]
    actions.append(Action("pick", ["block_green", None]))
    actions.append(Action("place_sr", [pos_x, pos_y, None, None, None]))

    # Blue #
    pos_z = block_size_z + .02
    actions.append(Action("pick", ["block_blue", None]))
    actions.append(Action("place_sr", [center[0], center[1], pos_z, True, yaw]))
    
    return actions
```


#define user
initial=BridgeState(frames=[Frame(name="big_red_block", x_pos=-0.2, y_pos=0.3, z_pos=0.7, x_rot=-0.0, y_rot=-0.0, z_rot=1.57, size=[0.1, 0.2, 0.1, 0.0], color="[204, 51, 63]"), Frame(name="target_pose", x_pos=0.4, y_pos=0.3, z_pos=0.7, x_rot=-0.0, y_rot=0.0, z_rot=-2.51, size=[0.1, 0.2, 0.1, 0.0], color="[0, 255, 0]")])
Goal: Push the red block to the target pose.


#define assistant
```python
def gen_plan(state: BridgeState):
    
    import numpy as np

    # Build the block towards the target
    actions = []
    red_box = state.getFrame("big_red_block")
    target = state.getFrame("target_pose")
    dir = np.array([target.x_pos, target.y_pos]) - np.array([red_box.x_pos, red_box.y_pos])
    dir_mag = np.linalg.norm(dir)
    if dir_mag != 0;
        dir_normed = dir / dir_mag
    else:
        dir_normed = np.zeros(2)

    # First push start
    offset_mag = max(red_box.size[:2]) * 3
    a_start_x = red_box.x_pos - dir_normed[0]*offset_mag
    a_start_y = red_box.y_pos - dir_normed[1]*offset_mag
    
    # First push end
    a_end_x = target.x_pos
    a_end_y = target.x_pos
    
    # Second push start
    b_start_x = target.x_pos - dir_normed[0]*.2
    b_start_y = target.y_pos - dir_normed[1]*.2
    
    # Second push end
    b_end_x = target.x_pos
    b_end_y = target.x_pos

    # First Push #
    actions.append(Action("push_motion", [a_start_x, a_start_y, a_end_x, a_end_y]))

    # Second Push (For adjusting position) #
    actions.append(Action("push_motion", [b_start_x, b_start_y, b_end_x, b_end_y]))

    return actions
```
