# Open-loop trajectory-fit action2code policy generated from the RoboCasa GT EE trajectory
# Task: CloseToasterOvenDoor
# Episode: episode_000000

from primitives import moveJoint, moveLinear, moveC, moveA, moveWholeBodyPath, moveActionSequence, grasp, release, wait

SCENE_ARTIFACT = '../../../artifacts/20260324/CloseToasterOvenDoor/20260324_205259_close-toaster-oven-door_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'CloseToasterOvenDoor',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/CloseToasterOvenDoor/20260324_205259_close-toaster-oven-door_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/CloseToasterOvenDoor/20260324_205259_close-toaster-oven-door_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/CloseToasterOvenDoor/20260324_205259_close-toaster-oven-door_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 140,
    'stable_release_contact_loss_t': 140,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=float('nan'), y=float('nan'), z=float('nan'), qw=float('nan'), qx=float('nan'), qy=float('nan'), qz=float('nan'))
GOAL_RECEPTACLE_POSE = pose(x=float('nan'), y=float('nan'), z=float('nan'), qw=float('nan'), qx=float('nan'), qy=float('nan'), qz=float('nan'))

APPROACH_003_JOINTS = [-0.003074, -0.993536, -0.006137, -2.239994, 0.020381, 1.515249, 0.673605]  # t=3
APPROACH_004_JOINTS = [-0.003089, -1.010390, -0.002806, -2.241075, 0.023192, 1.503504, 0.674699]  # t=4
APPROACH_008_JOINTS = [-0.000294, -1.137320, 0.077144, -2.265349, 0.064592, 1.463661, 0.756177]  # t=8
APPROACH_009_JOINTS = [0.000778, -1.175515, 0.106853, -2.277822, 0.077036, 1.466751, 0.788747]  # t=9
APPROACH_011_JOINTS = [0.003567, -1.249369, 0.171550, -2.306002, 0.098990, 1.488287, 0.855919]  # t=11
APPROACH_012_JOINTS = [0.005332, -1.281403, 0.205221, -2.321503, 0.110023, 1.509646, 0.887642]  # t=12
APPROACH_013_JOINTS = [0.007374, -1.309679, 0.238571, -2.337137, 0.122230, 1.535277, 0.918598]  # t=13
APPROACH_016_JOINTS = [0.016176, -1.380080, 0.335009, -2.377366, 0.164178, 1.626229, 1.005941]  # t=16
APPROACH_019_JOINTS = [0.031440, -1.448749, 0.423979, -2.416575, 0.196423, 1.710664, 1.080635]  # t=19
APPROACH_020_JOINTS = [0.039285, -1.477683, 0.451297, -2.432514, 0.203609, 1.730963, 1.104327]  # t=20
APPROACH_023_JOINTS = [0.071578, -1.585611, 0.536382, -2.490443, 0.215828, 1.788325, 1.184105]  # t=23
APPROACH_024_JOINTS = [0.085977, -1.628701, 0.567984, -2.512097, 0.218051, 1.805008, 1.213968]  # t=24
APPROACH_027_JOINTS = [0.141214, -1.769409, 0.666755, -2.581585, 0.216453, 1.846049, 1.306452]  # t=27
APPROACH_028_JOINTS = [0.162931, -1.765487, 0.698959, -2.581203, 0.197349, 1.859789, 1.348478]  # t=28
APPROACH_031_JOINTS = [0.226862, -1.762909, 0.784022, -2.554412, 0.129847, 1.847839, 1.485764]  # t=31
APPROACH_032_JOINTS = [0.245493, -1.762859, 0.802225, -2.545370, 0.118710, 1.831522, 1.515505]  # t=32
APPROACH_033_JOINTS = [0.262215, -1.762834, 0.817516, -2.539110, 0.114544, 1.813332, 1.536433]  # t=33
APPROACH_036_JOINTS = [0.300210, -1.762813, 0.859808, -2.530442, 0.120046, 1.766334, 1.582044]  # t=36
APPROACH_040_JOINTS = [0.321095, -1.742314, 0.911559, -2.545123, 0.173403, 1.695643, 1.627256]  # t=40
APPROACH_041_JOINTS = [0.321350, -1.734482, 0.929497, -2.551018, 0.185995, 1.689090, 1.645305]  # t=41
APPROACH_043_JOINTS = [0.318830, -1.715793, 0.977717, -2.571646, 0.211945, 1.700080, 1.687742]  # t=43
APPROACH_044_JOINTS = [0.317211, -1.705163, 1.005889, -2.584349, 0.225290, 1.713497, 1.710931]  # t=44
APPROACH_047_JOINTS = [0.313882, -1.666698, 1.097634, -2.627329, 0.266159, 1.769852, 1.786176]  # t=47
APPROACH_048_JOINTS = [0.315168, -1.653407, 1.124388, -2.642114, 0.277304, 1.788307, 1.811531]  # t=48
APPROACH_052_JOINTS = [0.329782, -1.598740, 1.196492, -2.701224, 0.297003, 1.878128, 1.893390]  # t=52
APPROACH_056_JOINTS = [0.344097, -1.528662, 1.260124, -2.770525, 0.312803, 1.989859, 1.969366]  # t=56
APPROACH_060_JOINTS = [0.334956, -1.436715, 1.331907, -2.850779, 0.318226, 2.150744, 2.041412]  # t=60
APPROACH_064_JOINTS = [0.282329, -1.329656, 1.390892, -2.929642, 0.301577, 2.361931, 2.109128]  # t=64
APPROACH_066_JOINTS = [0.238213, -1.276923, 1.409539, -2.950320, 0.279070, 2.469535, 2.139682]  # t=66
APPROACH_067_JOINTS = [0.216208, -1.252624, 1.418122, -2.949546, 0.264177, 2.502523, 2.158349]  # t=67
PRESS_068_JOINTS = [0.199416, -1.236136, 1.420648, -2.939767, 0.241987, 2.499437, 2.177983]  # t=68
PRESS_069_JOINTS = [0.187052, -1.225984, 1.416153, -2.924453, 0.217394, 2.475383, 2.191876]  # t=69
PRESS_070_JOINTS = [0.176907, -1.217519, 1.409461, -2.904355, 0.197430, 2.442225, 2.203233]  # t=70
PRESS_071_JOINTS = [0.167776, -1.208509, 1.405662, -2.878838, 0.182582, 2.408038, 2.214812]  # t=71
PRESS_072_JOINTS = [0.151749, -1.193426, 1.410187, -2.855114, 0.173420, 2.396039, 2.225554]  # t=72
PRESS_073_JOINTS = [0.132197, -1.175567, 1.418787, -2.835161, 0.166451, 2.395717, 2.234815]  # t=73
PRESS_074_JOINTS = [0.112941, -1.158386, 1.426869, -2.816164, 0.160081, 2.394941, 2.242804]  # t=74
PRESS_075_JOINTS = [0.095268, -1.144280, 1.433353, -2.797819, 0.152193, 2.391978, 2.251030]  # t=75
PRESS_077_JOINTS = [0.067717, -1.126179, 1.442426, -2.764750, 0.132692, 2.376917, 2.268615]  # t=77
PRESS_079_JOINTS = [0.047130, -1.113801, 1.451251, -2.741308, 0.118486, 2.362626, 2.282583]  # t=79
PRESS_080_JOINTS = [0.035624, -1.108288, 1.456964, -2.734987, 0.113688, 2.364125, 2.287867]  # t=80
PRESS_081_JOINTS = [0.022577, -1.103125, 1.465030, -2.731607, 0.109798, 2.371998, 2.293097]  # t=81
PRESS_083_JOINTS = [-0.006012, -1.093330, 1.487540, -2.730384, 0.105703, 2.399358, 2.303294]  # t=83
PRESS_085_JOINTS = [-0.035916, -1.086660, 1.512248, -2.732288, 0.103249, 2.432479, 2.312873]  # t=85
PRESS_087_JOINTS = [-0.062528, -1.087431, 1.534423, -2.735760, 0.097093, 2.464659, 2.325181]  # t=87
PRESS_089_JOINTS = [-0.081558, -1.098951, 1.549514, -2.740648, 0.082950, 2.496200, 2.343226]  # t=89
PRESS_091_JOINTS = [-0.093971, -1.118748, 1.564770, -2.749027, 0.064722, 2.536957, 2.373445]  # t=91
PRESS_092_JOINTS = [-0.100069, -1.129243, 1.573277, -2.756043, 0.056779, 2.562585, 2.390835]  # t=92
PRESS_093_JOINTS = [-0.104672, -1.139447, 1.578852, -2.763933, 0.050221, 2.588493, 2.406272]  # t=93
PRESS_095_JOINTS = [-0.109666, -1.159456, 1.581296, -2.777369, 0.038551, 2.634284, 2.428563]  # t=95
PRESS_097_JOINTS = [-0.105469, -1.182556, 1.570946, -2.787971, 0.026562, 2.665029, 2.447283]  # t=97
PRESS_099_JOINTS = [-0.088891, -1.203287, 1.542429, -2.799747, 0.019558, 2.681342, 2.455206]  # t=99
PRESS_100_JOINTS = [-0.080537, -1.211649, 1.523308, -2.806141, 0.017404, 2.690763, 2.453496]  # t=100
PRESS_101_JOINTS = [-0.075350, -1.218288, 1.503012, -2.811364, 0.015719, 2.703296, 2.448699]  # t=101
PRESS_102_JOINTS = [-0.072658, -1.224597, 1.481345, -2.815005, 0.013477, 2.716748, 2.442182]  # t=102
PRESS_103_JOINTS = [-0.072088, -1.230303, 1.459018, -2.817496, 0.010951, 2.730406, 2.434273]  # t=103
PRESS_105_JOINTS = [-0.077068, -1.237404, 1.415101, -2.819264, 0.007493, 2.758314, 2.415251]  # t=105
PRESS_106_JOINTS = [-0.082431, -1.238722, 1.393876, -2.818569, 0.006789, 2.772851, 2.404750]  # t=106
PRESS_107_JOINTS = [-0.089011, -1.238709, 1.373477, -2.817115, 0.006832, 2.787149, 2.394164]  # t=107
PRESS_108_JOINTS = [-0.096600, -1.237290, 1.353826, -2.814967, 0.007616, 2.800924, 2.383400]  # t=108
PRESS_109_JOINTS = [-0.104993, -1.234644, 1.334995, -2.812253, 0.009140, 2.814434, 2.372671]  # t=109
PRESS_111_JOINTS = [-0.122813, -1.226679, 1.298986, -2.806297, 0.014679, 2.842240, 2.352181]  # t=111
PRESS_113_JOINTS = [-0.142101, -1.213678, 1.267154, -2.799629, 0.023568, 2.872057, 2.331608]  # t=113
PRESS_114_JOINTS = [-0.151988, -1.206010, 1.253001, -2.796233, 0.028996, 2.887593, 2.322005]  # t=114
PRESS_115_JOINTS = [-0.162140, -1.197580, 1.239991, -2.792721, 0.034970, 2.903696, 2.312768]  # t=115
PRESS_117_JOINTS = [-0.185076, -1.177449, 1.218260, -2.783353, 0.047679, 2.938319, 2.294519]  # t=117
PRESS_119_JOINTS = [-0.210188, -1.154246, 1.200361, -2.771678, 0.061723, 2.974005, 2.276486]  # t=119
PRESS_120_JOINTS = [-0.222697, -1.141593, 1.192715, -2.765641, 0.069490, 2.992297, 2.267233]  # t=120
PRESS_121_JOINTS = [-0.234405, -1.128742, 1.186184, -2.760012, 0.077740, 3.010564, 2.257863]  # t=121
PRESS_123_JOINTS = [-0.257556, -1.101782, 1.175403, -2.748383, 0.095187, 3.046864, 2.238654]  # t=123
PRESS_125_JOINTS = [-0.280854, -1.072671, 1.167534, -2.735986, 0.112874, 3.082668, 2.217080]  # t=125
PRESS_127_JOINTS = [-0.306914, -1.039608, 1.165418, -2.723208, 0.129138, 3.118181, 2.193071]  # t=127
PRESS_129_JOINTS = [-0.338317, -1.001286, 1.168019, -2.708335, 0.143929, 3.157256, 2.168467]  # t=129
PRESS_131_JOINTS = [-0.368973, -0.960165, 1.172910, -2.695979, 0.159045, 3.202441, 2.143590]  # t=131
PRESS_133_JOINTS = [-0.396908, -0.915913, 1.176074, -2.685969, 0.173462, 3.251013, 2.114487]  # t=133
PRESS_135_JOINTS = [-0.422330, -0.865680, 1.177666, -2.675971, 0.184809, 3.301694, 2.079346]  # t=135
PRESS_137_JOINTS = [-0.444785, -0.809655, 1.178468, -2.666249, 0.191819, 3.353952, 2.040206]  # t=137
PRESS_139_JOINTS = [-0.464456, -0.750157, 1.179068, -2.657044, 0.193836, 3.406540, 1.999077]  # t=139
PRESS_140_JOINTS = [-0.471642, -0.721504, 1.179060, -2.654346, 0.193736, 3.433814, 1.978483]  # t=140
PRESS_141_JOINTS = [-0.472154, -0.699686, 1.177755, -2.656320, 0.195396, 3.461415, 1.959408]  # t=141
PRESS_142_JOINTS = [-0.467939, -0.682992, 1.174665, -2.659319, 0.198490, 3.482485, 1.941983]  # t=142
PRESS_143_JOINTS = [-0.460721, -0.669894, 1.170085, -2.661495, 0.202523, 3.495887, 1.925924]  # t=143
PRESS_145_JOINTS = [-0.441070, -0.649945, 1.157053, -2.663266, 0.211670, 3.504246, 1.897334]  # t=145
PRESS_147_JOINTS = [-0.418793, -0.632252, 1.139764, -2.664337, 0.219328, 3.500111, 1.871944]  # t=147
PRESS_149_JOINTS = [-0.393694, -0.617966, 1.120750, -2.668483, 0.225911, 3.498533, 1.849517]  # t=149
PRESS_151_JOINTS = [-0.371066, -0.603081, 1.102143, -2.671061, 0.231109, 3.497130, 1.828356]  # t=151
PRESS_153_JOINTS = [-0.352435, -0.589141, 1.086020, -2.672183, 0.237133, 3.496302, 1.807902]  # t=153
PRESS_155_JOINTS = [-0.337277, -0.578851, 1.073216, -2.672875, 0.246242, 3.494769, 1.788811]  # t=155
APPROACH_000_JOINTS = [-0.002349, -0.976434, -0.006173, -2.241550, 0.020248, 1.530420, 0.673882]  # t=1

# Phase-level helper functions keep the open-loop policy readable while preserving
# the exact primitive execution order used at runtime.

def move_to_control_panel():
    """Approach the appliance control region."""
    moveJoint(APPROACH_000_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_003_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_004_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_008_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_009_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_011_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_012_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_013_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_016_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_019_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_020_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_023_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_024_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_027_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_028_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_031_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_032_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_033_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_036_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_040_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_041_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_043_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_044_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_047_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_048_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_052_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_056_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_060_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_064_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_066_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_067_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

def press_control():
    """Execute the short interaction path that actuates the control."""
    moveJoint(PRESS_068_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_069_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_070_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_071_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_072_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_073_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_074_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_075_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_077_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_079_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_080_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_081_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_083_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_085_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_087_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_089_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_091_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_092_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_093_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_095_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_097_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_099_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_100_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_101_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_102_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_103_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_105_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_106_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_107_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_108_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_109_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_111_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_113_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_114_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_115_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_117_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_119_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_120_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_121_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_123_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_125_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_127_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_129_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_131_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_133_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_135_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_137_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_139_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_140_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_141_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_142_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_143_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_145_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_147_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_149_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_151_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_153_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PRESS_155_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)

move_to_control_panel()
press_control()

# Fitting strategy:
# - adaptive per-phase waypoint selection on EE position + quaternion error
# - tight tolerances near descend / lift / place, looser tolerances in transport
# - optional moveC / moveA / segmented moveLinear chains / moveActionSequence when a segment is better represented by a smooth or constrained path
# - GT used only during generation; runtime execution uses this saved policy plus scene artifact


# Open-loop trajectory-fit action2code policy generated from the RoboCasa GT EE trajectory
# Task: CoffeeServeMug
# Episode: episode_000000

from primitives import moveJoint, moveLinear, moveC, moveA, moveWholeBodyPath, moveActionSequence, grasp, release, wait

SCENE_ARTIFACT = '../../../artifacts/20260324/CoffeeServeMug/20260324_205300_coffee-serve-mug_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'CoffeeServeMug',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/CoffeeServeMug/20260324_205300_coffee-serve-mug_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/CoffeeServeMug/20260324_205300_coffee-serve-mug_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/CoffeeServeMug/20260324_205300_coffee-serve-mug_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 65,
    'stable_release_contact_loss_t': 113,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=1.405508, y=-0.506516, z=1.030056, qw=0.951114, qx=0.000507, qy=0.000513, qz=0.308839)
GOAL_RECEPTACLE_POSE = pose(x=1.274900, y=-0.501523, z=0.965623, qw=0.954526, qx=0.000560, qy=0.000256, qz=0.298127)

APPROACH_003_JOINTS = [-0.024720, -1.069462, -0.030289, -2.259839, 0.053143, 1.539246, 0.682968]  # t=3
APPROACH_006_JOINTS = [-0.024045, -1.091620, -0.031016, -2.272739, 0.052139, 1.516455, 0.683297]  # t=6
APPROACH_007_JOINTS = [-0.023637, -1.102839, -0.029566, -2.283390, 0.053225, 1.509804, 0.684316]  # t=7
APPROACH_009_JOINTS = [-0.022331, -1.128976, -0.020985, -2.318973, 0.059677, 1.506213, 0.690094]  # t=9
APPROACH_012_JOINTS = [-0.019663, -1.172072, 0.000865, -2.399771, 0.075594, 1.532868, 0.704468]  # t=12
APPROACH_015_JOINTS = [-0.016109, -1.215898, 0.026509, -2.490179, 0.094558, 1.577613, 0.719823]  # t=15
APPROACH_016_JOINTS = [-0.014578, -1.228872, 0.035956, -2.519701, 0.102096, 1.596437, 0.724797]  # t=16
APPROACH_018_JOINTS = [-0.010925, -1.246248, 0.057485, -2.579273, 0.119569, 1.647017, 0.734778]  # t=18
APPROACH_021_JOINTS = [-0.006792, -1.254986, 0.083189, -2.659459, 0.136237, 1.734619, 0.749941]  # t=21
APPROACH_024_JOINTS = [-0.004677, -1.250689, 0.092421, -2.719377, 0.140069, 1.816209, 0.756641]  # t=24
APPROACH_025_JOINTS = [-0.003777, -1.248674, 0.094397, -2.736212, 0.141091, 1.837976, 0.757618]  # t=25
APPROACH_027_JOINTS = [-0.001110, -1.250167, 0.097999, -2.771992, 0.143673, 1.871159, 0.757672]  # t=27
APPROACH_030_JOINTS = [0.004704, -1.260850, 0.105352, -2.845214, 0.150812, 1.921032, 0.756695]  # t=30
APPROACH_033_JOINTS = [0.011658, -1.281232, 0.112855, -2.916227, 0.159009, 1.978305, 0.755520]  # t=33
APPROACH_034_JOINTS = [0.014054, -1.290072, 0.114867, -2.935647, 0.161208, 1.995338, 0.755425]  # t=34
APPROACH_036_JOINTS = [0.019021, -1.308418, 0.117460, -2.967698, 0.165009, 2.021506, 0.752957]  # t=36
APPROACH_038_JOINTS = [0.024161, -1.324292, 0.116684, -2.989543, 0.168527, 2.040611, 0.741243]  # t=38
APPROACH_039_JOINTS = [0.026347, -1.327069, 0.115783, -2.998988, 0.169974, 2.054717, 0.732802]  # t=39
APPROACH_042_JOINTS = [0.032195, -1.310576, 0.114063, -3.020180, 0.170924, 2.098084, 0.694183]  # t=42
APPROACH_045_JOINTS = [0.037420, -1.292471, 0.105694, -3.019245, 0.169048, 2.103290, 0.640673]  # t=45
APPROACH_048_JOINTS = [0.041107, -1.283760, 0.090639, -3.013708, 0.171215, 2.100264, 0.594931]  # t=48
APPROACH_051_JOINTS = [0.042889, -1.276451, 0.081258, -3.011526, 0.172274, 2.103709, 0.570843]  # t=51
APPROACH_053_JOINTS = [0.044531, -1.268340, 0.082078, -3.011083, 0.175466, 2.112893, 0.567777]  # t=53
DESCEND_054_JOINTS = [0.045730, -1.263898, 0.084189, -3.010468, 0.177923, 2.117144, 0.568231]  # t=54
DESCEND_055_JOINTS = [0.047099, -1.259411, 0.087099, -3.009572, 0.180711, 2.121254, 0.569311]  # t=55
DESCEND_056_JOINTS = [0.048764, -1.254009, 0.091068, -3.008847, 0.184274, 2.127243, 0.570798]  # t=56
DESCEND_057_JOINTS = [0.050859, -1.246392, 0.096435, -3.008647, 0.189225, 2.137656, 0.572643]  # t=57
DESCEND_058_JOINTS = [0.053179, -1.236479, 0.102744, -3.008585, 0.194972, 2.151141, 0.574705]  # t=58
DESCEND_059_JOINTS = [0.055325, -1.223493, 0.109340, -3.008648, 0.200737, 2.169358, 0.576655]  # t=59
DESCEND_060_JOINTS = [0.056852, -1.207711, 0.115056, -3.008487, 0.205478, 2.190319, 0.577920]  # t=60
DESCEND_061_JOINTS = [0.057299, -1.190291, 0.118571, -3.007606, 0.208089, 2.211640, 0.578094]  # t=61
DESCEND_062_JOINTS = [0.057021, -1.170893, 0.120155, -3.005973, 0.209490, 2.234182, 0.577147]  # t=62
GRASP_HOLD_065_JOINTS = [0.054927, -1.097832, 0.120373, -2.994496, 0.212718, 2.309447, 0.569823]  # t=65
LIFT_066_JOINTS = [0.054025, -1.077764, 0.119024, -2.986179, 0.212113, 2.319920, 0.567931]  # t=66
LIFT_067_JOINTS = [0.052824, -1.062786, 0.116753, -2.977773, 0.209994, 2.322094, 0.566436]  # t=67
LIFT_068_JOINTS = [0.051655, -1.052469, 0.114112, -2.970695, 0.207362, 2.320401, 0.564899]  # t=68
LIFT_069_JOINTS = [0.050961, -1.046887, 0.111922, -2.965135, 0.205153, 2.314988, 0.563707]  # t=69
LIFT_070_JOINTS = [0.051105, -1.044272, 0.111411, -2.961285, 0.204491, 2.309157, 0.563554]  # t=70
LIFT_071_JOINTS = [0.052631, -1.048784, 0.113045, -2.958193, 0.205552, 2.293949, 0.565326]  # t=71
LIFT_072_JOINTS = [0.055981, -1.064210, 0.117825, -2.954991, 0.208304, 2.264551, 0.570604]  # t=72
LIFT_073_JOINTS = [0.060960, -1.089237, 0.126010, -2.952567, 0.212730, 2.225912, 0.579703]  # t=73
LIFT_074_JOINTS = [0.067786, -1.123620, 0.137894, -2.951469, 0.219563, 2.180976, 0.592027]  # t=74
LIFT_075_JOINTS = [0.076926, -1.164319, 0.154749, -2.952794, 0.230766, 2.136098, 0.607078]  # t=75
TRANSPORT_076_JOINTS = [0.088876, -1.208170, 0.178155, -2.956132, 0.247865, 2.097934, 0.623557]  # t=76
TRANSPORT_077_JOINTS = [0.103369, -1.251436, 0.209177, -2.962017, 0.271088, 2.072632, 0.642218]  # t=77
TRANSPORT_078_JOINTS = [0.119677, -1.290298, 0.247358, -2.970742, 0.299677, 2.061896, 0.663293]  # t=78
TRANSPORT_079_JOINTS = [0.136866, -1.321259, 0.291590, -2.981604, 0.333065, 2.066745, 0.686279]  # t=79
TRANSPORT_080_JOINTS = [0.153998, -1.341986, 0.339513, -2.993535, 0.369778, 2.086154, 0.709439]  # t=80
TRANSPORT_081_JOINTS = [0.170641, -1.353630, 0.388572, -3.003898, 0.407269, 2.111732, 0.731691]  # t=81
TRANSPORT_082_JOINTS = [0.186505, -1.357485, 0.437506, -3.011208, 0.444535, 2.141115, 0.753371]  # t=82
TRANSPORT_083_JOINTS = [0.200876, -1.353554, 0.485501, -3.016223, 0.480594, 2.175564, 0.774343]  # t=83
TRANSPORT_084_JOINTS = [0.212733, -1.341358, 0.531104, -3.020318, 0.514344, 2.215896, 0.793550]  # t=84
TRANSPORT_085_JOINTS = [0.219559, -1.319580, 0.570982, -3.025724, 0.543584, 2.266270, 0.808305]  # t=85
TRANSPORT_086_JOINTS = [0.220577, -1.293951, 0.595946, -3.034281, 0.563323, 2.315883, 0.814318]  # t=86
TRANSPORT_087_JOINTS = [0.218027, -1.267254, 0.606012, -3.042249, 0.576242, 2.356992, 0.811330]  # t=87
TRANSPORT_088_JOINTS = [0.213283, -1.237682, 0.607766, -3.047138, 0.585683, 2.393126, 0.802977]  # t=88
TRANSPORT_089_JOINTS = [0.207124, -1.206210, 0.604390, -3.048446, 0.591867, 2.423871, 0.792113]  # t=89
TRANSPORT_090_JOINTS = [0.199884, -1.173343, 0.597566, -3.046494, 0.595086, 2.449741, 0.780401]  # t=90
TRANSPORT_091_JOINTS = [0.191710, -1.139247, 0.588360, -3.041581, 0.595701, 2.471775, 0.768498]  # t=91
TRANSPORT_092_JOINTS = [0.183248, -1.106141, 0.577372, -3.034118, 0.593635, 2.488665, 0.757102]  # t=92
TRANSPORT_093_JOINTS = [0.175324, -1.076851, 0.565331, -3.024861, 0.588654, 2.499109, 0.747242]  # t=93
TRANSPORT_094_JOINTS = [0.169002, -1.054750, 0.553007, -3.014887, 0.580804, 2.501250, 0.740067]  # t=94
TRANSPORT_095_JOINTS = [0.164781, -1.041135, 0.541635, -3.005356, 0.570661, 2.496874, 0.736470]  # t=95
TRANSPORT_096_JOINTS = [0.162148, -1.032636, 0.532402, -2.997025, 0.560019, 2.490820, 0.736008]  # t=96
TRANSPORT_097_JOINTS = [0.160652, -1.026720, 0.525159, -2.990058, 0.550254, 2.484871, 0.737213]  # t=97
TRANSPORT_098_JOINTS = [0.159958, -1.022373, 0.519427, -2.984383, 0.541728, 2.479689, 0.739299]  # t=98
PLACE_099_JOINTS = [0.159792, -1.018923, 0.514838, -2.979859, 0.534467, 2.475639, 0.741693]  # t=99
PLACE_100_JOINTS = [0.159958, -1.016126, 0.511102, -2.976310, 0.528356, 2.472583, 0.744056]  # t=100
PLACE_101_JOINTS = [0.160352, -1.014831, 0.507421, -2.973181, 0.522809, 2.469449, 0.746257]  # t=101
PLACE_102_JOINTS = [0.160877, -1.017322, 0.502882, -2.969592, 0.516250, 2.465482, 0.748592]  # t=102
PLACE_103_JOINTS = [0.162457, -1.030227, 0.496738, -2.965026, 0.506304, 2.455904, 0.752357]  # t=103
PLACE_104_JOINTS = [0.165847, -1.055886, 0.490049, -2.960257, 0.492839, 2.438909, 0.759067]  # t=104
PLACE_105_JOINTS = [0.171122, -1.094104, 0.483246, -2.955179, 0.476262, 2.415475, 0.769123]  # t=105
PLACE_106_JOINTS = [0.178453, -1.144459, 0.476486, -2.949576, 0.457073, 2.384528, 0.782409]  # t=106
PLACE_107_JOINTS = [0.188144, -1.204837, 0.470403, -2.943901, 0.437314, 2.345709, 0.798376]  # t=107
PLACE_108_JOINTS = [0.199987, -1.274933, 0.464203, -2.939006, 0.416995, 2.299035, 0.816378]  # t=108
PLACE_109_JOINTS = [0.212657, -1.350900, 0.458677, -2.935856, 0.395466, 2.251889, 0.835790]  # t=109
PLACE_110_JOINTS = [0.225505, -1.428486, 0.455443, -2.934184, 0.373495, 2.209819, 0.855217]  # t=110
PLACE_111_JOINTS = [0.238206, -1.506629, 0.453641, -2.933674, 0.350940, 2.171682, 0.873869]  # t=111
PLACE_112_JOINTS = [0.250628, -1.584371, 0.452072, -2.933610, 0.328529, 2.136352, 0.890832]  # t=112
PLACE_113_JOINTS = [0.262655, -1.660402, 0.451184, -2.933258, 0.306624, 2.104549, 0.905282]  # t=113
RELEASE_POSE_113_JOINTS = [0.262655, -1.660402, 0.451184, -2.933258, 0.306624, 2.104549, 0.905282]  # t=113
RETRACT_114_JOINTS = [0.274083, -1.733796, 0.451221, -2.932223, 0.285289, 2.076259, 0.917247]  # t=114
RETRACT_115_JOINTS = [0.284002, -1.771926, 0.450218, -2.917298, 0.259610, 2.056600, 0.929538]  # t=115
RETRACT_116_JOINTS = [0.289603, -1.764929, 0.439372, -2.869791, 0.214297, 2.032440, 0.949980]  # t=116
RETRACT_117_JOINTS = [0.293321, -1.763245, 0.425922, -2.815879, 0.169262, 1.998645, 0.969225]  # t=117
RETRACT_118_JOINTS = [0.295654, -1.763009, 0.413947, -2.762034, 0.131672, 1.965797, 0.983988]  # t=118
RETRACT_119_JOINTS = [0.296196, -1.762997, 0.402140, -2.709026, 0.098715, 1.936886, 0.995887]  # t=119
RETRACT_120_JOINTS = [0.295125, -1.762982, 0.389827, -2.657282, 0.069511, 1.910414, 1.005122]  # t=120
RETRACT_121_JOINTS = [0.292466, -1.762953, 0.378735, -2.607118, 0.044512, 1.886887, 1.011488]  # t=121
RETRACT_122_JOINTS = [0.287543, -1.762880, 0.371098, -2.561008, 0.023850, 1.869746, 1.016510]  # t=122
RETRACT_123_JOINTS = [0.279821, -1.762719, 0.366477, -2.521208, 0.006262, 1.860264, 1.020821]  # t=123
RETRACT_124_JOINTS = [0.269839, -1.762918, 0.361880, -2.484866, -0.010884, 1.855607, 1.025370]  # t=124
RETRACT_125_JOINTS = [0.258560, -1.762882, 0.356313, -2.448259, -0.028540, 1.853081, 1.031322]  # t=125
RETRACT_126_JOINTS = [0.246135, -1.762799, 0.348901, -2.413907, -0.045151, 1.853348, 1.040043]  # t=126
RETRACT_127_JOINTS = [0.232863, -1.762370, 0.342374, -2.382181, -0.059472, 1.858006, 1.047903]  # t=127
RETRACT_128_JOINTS = [0.218936, -1.761234, 0.337104, -2.352066, -0.072461, 1.865730, 1.054864]  # t=128
APPROACH_000_JOINTS = [-0.025185, -1.063129, -0.029761, -2.261026, 0.053602, 1.548416, 0.682920]  # t=1

# Phase-level helper functions keep the open-loop policy readable while preserving
# the exact primitive execution order used at runtime.

def move_to_pregrasp():
    """Coarse approach into the pregrasp corridor."""
    moveJoint(APPROACH_000_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_003_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_006_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_007_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_009_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_012_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_015_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_016_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_018_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_021_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_024_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_025_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_027_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_030_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_033_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_034_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_036_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_038_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_039_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_042_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_045_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_048_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_051_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_053_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)

def execute_grasp_approach():
    """Final Cartesian descent into grasp alignment."""
    moveJoint(DESCEND_054_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_055_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_056_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_057_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_058_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_059_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_060_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_061_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_062_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)

def close_gripper_on_object():
    """Close the gripper and let the grasp settle briefly."""
    grasp(hold_target=GRASP_HOLD_065_JOINTS, input_type='joint', close_steps=1, settle_steps=0, gripper=1.0, require_contact_count=4)
    wait(duration=0.05, hold_target=GRASP_HOLD_065_JOINTS, input_type='joint', gripper=1.0)

def lift_object():
    """Lift the grasped object clear of the counter."""
    moveJoint(LIFT_066_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_067_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_068_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_069_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_070_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_071_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_072_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_073_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_074_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_075_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)

def transport_to_place():
    """Carry the object through the curved transport corridor."""
    moveJoint(TRANSPORT_076_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_077_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_078_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_079_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_080_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_081_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_082_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_083_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_084_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_085_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_086_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_087_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_088_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_089_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_090_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_091_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_092_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_093_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_094_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_095_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_096_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_097_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_098_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

def prepare_release():
    """Set the object into the receptacle and stabilize before release."""
    moveJoint(PLACE_099_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_100_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_101_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_102_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_103_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_104_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_105_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_106_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_107_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_108_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_109_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_110_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_111_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_112_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_113_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    wait(duration=0.05, hold_target=RELEASE_POSE_113_JOINTS, input_type='joint', gripper=-1.0)

def release_object():
    """Open the gripper and hold briefly to avoid disturbing the placement."""
    release(hold_target=RELEASE_POSE_113_JOINTS, input_type='joint', open_steps=1, settle_steps=0, gripper=-1.0)
    wait(duration=0.05, hold_target=RELEASE_POSE_113_JOINTS, input_type='joint', gripper=-1.0)

def retreat_after_release():
    """Retreat away from the placed object without clipping it."""
    moveJoint(RETRACT_114_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_115_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_116_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_117_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_118_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_119_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_120_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_121_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_122_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_123_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_124_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_125_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_126_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_127_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_128_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

move_to_pregrasp()
execute_grasp_approach()
close_gripper_on_object()
lift_object()
transport_to_place()
prepare_release()
release_object()
retreat_after_release()

# Fitting strategy:
# - adaptive per-phase waypoint selection on EE position + quaternion error
# - tight tolerances near descend / lift / place, looser tolerances in transport
# - optional moveC / moveA / segmented moveLinear chains / moveActionSequence when a segment is better represented by a smooth or constrained path
# - GT used only during generation; runtime execution uses this saved policy plus scene artifact

Pick the mug from under the coffee machine dispenser and place it on the counter.


# Open-loop trajectory-fit action2code policy generated from the RoboCasa GT EE trajectory
# Task: CoffeeSetupMug
# Episode: episode_000000

from primitives import moveJoint, moveLinear, moveC, moveA, moveWholeBodyPath, moveActionSequence, grasp, release, wait

SCENE_ARTIFACT = '../../../artifacts/20260324/CoffeeSetupMug/20260324_205301_coffee-setup-mug_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'CoffeeSetupMug',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/CoffeeSetupMug/20260324_205301_coffee-setup-mug_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/CoffeeSetupMug/20260324_205301_coffee-setup-mug_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/CoffeeSetupMug/20260324_205301_coffee-setup-mug_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 103,
    'stable_release_contact_loss_t': 206,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=3.925722, y=-0.567339, z=0.964644, qw=0.886038, qx=-0.000472, qy=0.000443, qz=0.463611)
GOAL_RECEPTACLE_POSE = pose(x=3.997765, y=-0.419605, z=1.016747, qw=0.971317, qx=0.000371, qy=-0.000336, qz=0.237789)

APPROACH_003_JOINTS = [-0.059347, -0.995843, -0.015158, -2.287002, 0.062668, 1.542841, 0.718200]  # t=3
APPROACH_006_JOINTS = [-0.053743, -0.994617, -0.085110, -2.287340, 0.070446, 1.570111, 0.650914]  # t=6
APPROACH_009_JOINTS = [-0.045314, -1.017874, -0.222411, -2.319976, 0.128676, 1.649697, 0.477227]  # t=9
APPROACH_012_JOINTS = [-0.037257, -1.060328, -0.379496, -2.395058, 0.196191, 1.713961, 0.245052]  # t=12
APPROACH_013_JOINTS = [-0.034695, -1.072807, -0.430109, -2.423748, 0.217039, 1.726667, 0.166379]  # t=13
APPROACH_015_JOINTS = [-0.029611, -1.103292, -0.515930, -2.485943, 0.256257, 1.727298, 0.000528]  # t=15
APPROACH_018_JOINTS = [-0.020624, -1.137094, -0.604946, -2.584632, 0.291776, 1.720729, -0.242422]  # t=18
APPROACH_019_JOINTS = [-0.016158, -1.139386, -0.629179, -2.609171, 0.300570, 1.715410, -0.314226]  # t=19
APPROACH_021_JOINTS = [-0.004766, -1.132704, -0.672362, -2.636175, 0.315183, 1.695491, -0.446042]  # t=21
APPROACH_024_JOINTS = [0.015640, -1.100638, -0.722477, -2.652806, 0.317408, 1.652890, -0.610294]  # t=24
APPROACH_027_JOINTS = [0.023820, -1.072313, -0.767203, -2.666487, 0.321276, 1.652577, -0.710537]  # t=27
APPROACH_030_JOINTS = [0.015173, -1.068387, -0.783741, -2.727522, 0.335183, 1.704997, -0.740141]  # t=30
APPROACH_033_JOINTS = [-0.009960, -1.080890, -0.790775, -2.808147, 0.328314, 1.773717, -0.733923]  # t=33
APPROACH_035_JOINTS = [-0.033628, -1.092659, -0.796598, -2.859222, 0.312585, 1.815042, -0.713538]  # t=35
APPROACH_036_JOINTS = [-0.045705, -1.097998, -0.799787, -2.880955, 0.302148, 1.829908, -0.703253]  # t=36
APPROACH_039_JOINTS = [-0.075511, -1.108587, -0.806296, -2.930093, 0.274933, 1.856056, -0.684193]  # t=39
APPROACH_041_JOINTS = [-0.088606, -1.106479, -0.808606, -2.956567, 0.260186, 1.873604, -0.676313]  # t=41
APPROACH_042_JOINTS = [-0.095148, -1.102816, -0.812833, -2.969505, 0.249295, 1.879280, -0.674638]  # t=42
APPROACH_045_JOINTS = [-0.114947, -1.088537, -0.831663, -3.013173, 0.215160, 1.898127, -0.678212]  # t=45
DESCEND_046_JOINTS = [-0.119984, -1.080585, -0.838771, -3.028956, 0.204820, 1.913466, -0.682475]  # t=46
DESCEND_047_JOINTS = [-0.123839, -1.071783, -0.845944, -3.043289, 0.196379, 1.928721, -0.689919]  # t=47
GRASP_050_JOINTS = [-0.126293, -1.045773, -0.863170, -3.052250, 0.187256, 1.946622, -0.733060]  # t=50
GRASP_055_JOINTS = [-0.095485, -0.991317, -0.873232, -3.019140, 0.200195, 1.918846, -0.848055]  # t=55
GRASP_065_JOINTS = [-0.018042, -0.916074, -0.813937, -2.965428, 0.283325, 1.884022, -0.879478]  # t=65
GRASP_073_JOINTS = [-0.014608, -0.902216, -0.803110, -2.942368, 0.288129, 1.845971, -0.886704]  # t=73
GRASP_075_JOINTS = [-0.019293, -0.905609, -0.803667, -2.941166, 0.280324, 1.820884, -0.890095]  # t=75
GRASP_077_JOINTS = [-0.025068, -0.916705, -0.797722, -2.948859, 0.276928, 1.795204, -0.890903]  # t=77
GRASP_081_JOINTS = [-0.031422, -0.944234, -0.767446, -2.986277, 0.281763, 1.782015, -0.896704]  # t=81
GRASP_084_JOINTS = [-0.031553, -0.957965, -0.742273, -3.004480, 0.290249, 1.782944, -0.901067]  # t=84
GRASP_092_JOINTS = [-0.033602, -0.941478, -0.733272, -3.028401, 0.281266, 1.800850, -0.936500]  # t=92
GRASP_093_JOINTS = [-0.032716, -0.938575, -0.732093, -3.028385, 0.282059, 1.800330, -0.941049]  # t=93
GRASP_094_JOINTS = [-0.031586, -0.935610, -0.730536, -3.028645, 0.283101, 1.800171, -0.945046]  # t=94
GRASP_103_JOINTS = [-0.024705, -0.912876, -0.723316, -3.016679, 0.288104, 1.788745, -0.968070]  # t=103
GRASP_HOLD_103_JOINTS = [-0.024705, -0.912876, -0.723316, -3.016679, 0.288104, 1.788745, -0.968070]  # t=103
LIFT_104_JOINTS = [-0.026198, -0.910656, -0.728642, -3.012351, 0.288775, 1.782714, -0.970027]  # t=104
LIFT_105_JOINTS = [-0.029488, -0.908794, -0.740238, -3.003500, 0.292231, 1.769707, -0.971982]  # t=105
LIFT_106_JOINTS = [-0.033680, -0.908222, -0.755496, -2.991664, 0.300495, 1.751860, -0.973664]  # t=106
LIFT_107_JOINTS = [-0.038064, -0.909769, -0.771734, -2.977678, 0.313526, 1.729359, -0.975292]  # t=107
LIFT_108_JOINTS = [-0.042988, -0.912366, -0.788935, -2.962445, 0.328189, 1.705875, -0.975101]  # t=108
LIFT_109_JOINTS = [-0.048751, -0.914914, -0.807959, -2.946941, 0.341914, 1.683920, -0.972946]  # t=109
TRANSPORT_111_JOINTS = [-0.062193, -0.918500, -0.849528, -2.917467, 0.364942, 1.650743, -0.957308]  # t=111
TRANSPORT_113_JOINTS = [-0.073325, -0.919802, -0.885071, -2.895870, 0.387480, 1.652330, -0.933556]  # t=113
TRANSPORT_115_JOINTS = [-0.080551, -0.920846, -0.909356, -2.884409, 0.408441, 1.681917, -0.908431]  # t=115
TRANSPORT_117_JOINTS = [-0.083724, -0.924469, -0.919320, -2.884694, 0.433850, 1.733647, -0.882951]  # t=117
TRANSPORT_119_JOINTS = [-0.080212, -0.931300, -0.911249, -2.897489, 0.471301, 1.797188, -0.860528]  # t=119
TRANSPORT_121_JOINTS = [-0.073923, -0.938344, -0.895139, -2.907524, 0.509729, 1.860261, -0.833388]  # t=121
TRANSPORT_123_JOINTS = [-0.065890, -0.943690, -0.874407, -2.908787, 0.539513, 1.907201, -0.799712]  # t=123
TRANSPORT_125_JOINTS = [-0.055819, -0.943320, -0.854221, -2.910178, 0.557869, 1.942715, -0.767974]  # t=125
TRANSPORT_127_JOINTS = [-0.044568, -0.937437, -0.835195, -2.910984, 0.564165, 1.964479, -0.735508]  # t=127
TRANSPORT_129_JOINTS = [-0.035234, -0.929755, -0.819659, -2.907969, 0.559079, 1.981343, -0.712036]  # t=129
TRANSPORT_131_JOINTS = [-0.025049, -0.918595, -0.806700, -2.909564, 0.556750, 2.010452, -0.701833]  # t=131
TRANSPORT_133_JOINTS = [-0.008028, -0.902481, -0.787083, -2.914541, 0.569053, 2.062916, -0.706413]  # t=133
TRANSPORT_135_JOINTS = [0.013443, -0.881775, -0.760485, -2.918139, 0.586250, 2.127170, -0.714816]  # t=135
TRANSPORT_137_JOINTS = [0.033815, -0.860964, -0.730420, -2.916780, 0.597645, 2.187951, -0.719595]  # t=137
TRANSPORT_138_JOINTS = [0.042860, -0.851342, -0.714599, -2.915156, 0.601138, 2.215992, -0.719412]  # t=138
PLACE_139_JOINTS = [0.051377, -0.842480, -0.698461, -2.912805, 0.604853, 2.241887, -0.717715]  # t=139
PLACE_140_JOINTS = [0.059258, -0.834969, -0.682906, -2.908774, 0.609459, 2.262541, -0.714372]  # t=140
PLACE_142_JOINTS = [0.074384, -0.818959, -0.656214, -2.898974, 0.622122, 2.293351, -0.706039]  # t=142
PLACE_144_JOINTS = [0.089235, -0.800298, -0.639001, -2.889624, 0.644325, 2.317895, -0.703868]  # t=144
PLACE_146_JOINTS = [0.098715, -0.782554, -0.633471, -2.881525, 0.661597, 2.331899, -0.704301]  # t=146
PLACE_148_JOINTS = [0.101553, -0.771309, -0.635027, -2.875550, 0.668265, 2.332812, -0.709368]  # t=148
PLACE_150_JOINTS = [0.101095, -0.763837, -0.636041, -2.870389, 0.667496, 2.326343, -0.711281]  # t=150
PLACE_152_JOINTS = [0.100340, -0.755788, -0.634179, -2.864027, 0.664213, 2.318836, -0.706921]  # t=152
PLACE_154_JOINTS = [0.099484, -0.746248, -0.631189, -2.856235, 0.658544, 2.310194, -0.697786]  # t=154
PLACE_156_JOINTS = [0.097909, -0.738550, -0.629320, -2.848660, 0.652564, 2.299142, -0.691380]  # t=156
PLACE_158_JOINTS = [0.095205, -0.736000, -0.630468, -2.846333, 0.650912, 2.291344, -0.697406]  # t=158
PLACE_160_JOINTS = [0.091928, -0.736619, -0.630493, -2.846758, 0.650420, 2.285718, -0.703741]  # t=160
PLACE_162_JOINTS = [0.088466, -0.738761, -0.629831, -2.848591, 0.649946, 2.282058, -0.709075]  # t=162
PLACE_164_JOINTS = [0.077842, -0.749669, -0.637999, -2.851529, 0.636806, 2.268675, -0.712448]  # t=164
PLACE_166_JOINTS = [0.063728, -0.770034, -0.648730, -2.851847, 0.620504, 2.250389, -0.716944]  # t=166
PLACE_168_JOINTS = [0.047707, -0.797634, -0.660554, -2.846788, 0.603974, 2.228424, -0.717507]  # t=168
PLACE_170_JOINTS = [0.033891, -0.823255, -0.673322, -2.832683, 0.593582, 2.195058, -0.700917]  # t=170
PLACE_172_JOINTS = [0.020191, -0.848450, -0.691790, -2.810832, 0.582242, 2.147446, -0.679111]  # t=172
PLACE_174_JOINTS = [0.003354, -0.873709, -0.723370, -2.785655, 0.564001, 2.087751, -0.656266]  # t=174
PLACE_176_JOINTS = [-0.014842, -0.893697, -0.773182, -2.756822, 0.541833, 2.022244, -0.637222]  # t=176
PLACE_178_JOINTS = [-0.029510, -0.906665, -0.831672, -2.724239, 0.521365, 1.954629, -0.623786]  # t=178
PLACE_180_JOINTS = [-0.039611, -0.913447, -0.899367, -2.686654, 0.506916, 1.883817, -0.613964]  # t=180
PLACE_182_JOINTS = [-0.042969, -0.915506, -0.970463, -2.645388, 0.501635, 1.812902, -0.608826]  # t=182
PLACE_184_JOINTS = [-0.039582, -0.915013, -1.044425, -2.603991, 0.500303, 1.741947, -0.614622]  # t=184
PLACE_186_JOINTS = [-0.027405, -0.911298, -1.111662, -2.569224, 0.496270, 1.682984, -0.629630]  # t=186
PLACE_188_JOINTS = [-0.006750, -0.906086, -1.161250, -2.541053, 0.489765, 1.642729, -0.642458]  # t=188
PLACE_190_JOINTS = [0.017415, -0.903180, -1.188249, -2.520849, 0.479869, 1.622888, -0.646671]  # t=190
PLACE_192_JOINTS = [0.040288, -0.904382, -1.206140, -2.506185, 0.472538, 1.615379, -0.644675]  # t=192
PLACE_194_JOINTS = [0.060168, -0.909106, -1.220715, -2.494746, 0.464615, 1.611101, -0.640062]  # t=194
PLACE_196_JOINTS = [0.077293, -0.916310, -1.232857, -2.485850, 0.456239, 1.609026, -0.633952]  # t=196
PLACE_198_JOINTS = [0.091984, -0.925139, -1.242952, -2.479003, 0.447688, 1.608559, -0.626953]  # t=198
PLACE_200_JOINTS = [0.104556, -0.934849, -1.251346, -2.473648, 0.439188, 1.608853, -0.619480]  # t=200
PLACE_202_JOINTS = [0.115333, -0.944892, -1.258312, -2.469473, 0.430942, 1.609815, -0.611876]  # t=202
PLACE_204_JOINTS = [0.124551, -0.954941, -1.264075, -2.466279, 0.423069, 1.611266, -0.604376]  # t=204
PLACE_206_JOINTS = [0.127491, -0.967917, -1.272631, -2.460421, 0.408043, 1.599186, -0.601025]  # t=206
RELEASE_POSE_206_JOINTS = [0.127491, -0.967917, -1.272631, -2.460421, 0.408043, 1.599186, -0.601025]  # t=206
RETRACT_207_JOINTS = [0.126173, -0.974863, -1.276358, -2.456615, 0.391752, 1.592569, -0.602919]  # t=207
RETRACT_208_JOINTS = [0.123579, -0.981766, -1.276897, -2.452548, 0.369607, 1.588026, -0.606860]  # t=208
RETRACT_209_JOINTS = [0.118840, -0.989278, -1.272662, -2.446457, 0.339786, 1.583282, -0.613993]  # t=209
RETRACT_210_JOINTS = [0.111745, -0.997759, -1.260865, -2.438918, 0.302175, 1.580314, -0.625821]  # t=210
RETRACT_211_JOINTS = [0.102520, -1.007233, -1.243839, -2.432189, 0.262741, 1.580208, -0.638606]  # t=211
RETRACT_212_JOINTS = [0.091085, -1.017958, -1.224735, -2.427046, 0.223794, 1.580688, -0.651755]  # t=212
RETRACT_213_JOINTS = [0.077683, -1.029711, -1.204704, -2.424079, 0.185704, 1.581908, -0.664451]  # t=213
RETRACT_214_JOINTS = [0.062684, -1.042295, -1.183585, -2.423421, 0.147807, 1.584181, -0.676820]  # t=214
RETRACT_215_JOINTS = [0.046217, -1.055714, -1.162553, -2.423655, 0.109913, 1.587432, -0.689245]  # t=215
RETRACT_216_JOINTS = [0.028531, -1.070221, -1.141642, -2.425016, 0.072163, 1.590389, -0.702926]  # t=216
RETRACT_217_JOINTS = [0.010480, -1.085366, -1.120931, -2.428667, 0.036106, 1.596062, -0.716915]  # t=217
RETRACT_218_JOINTS = [-0.007031, -1.100437, -1.100729, -2.435306, 0.002539, 1.605222, -0.729990]  # t=218
RETRACT_219_JOINTS = [-0.023870, -1.115371, -1.081292, -2.444435, -0.028291, 1.616359, -0.741604]  # t=219
RETRACT_220_JOINTS = [-0.040227, -1.130330, -1.062975, -2.455186, -0.056489, 1.627533, -0.751574]  # t=220
RETRACT_221_JOINTS = [-0.055901, -1.145359, -1.046141, -2.467550, -0.080678, 1.638650, -0.759663]  # t=221
APPROACH_000_JOINTS = [-0.062465, -0.994317, -0.008781, -2.287270, 0.069160, 1.545456, 0.718407]  # t=1

# Phase-level helper functions keep the open-loop policy readable while preserving
# the exact primitive execution order used at runtime.

def move_to_pregrasp():
    """Coarse approach into the pregrasp corridor."""
    moveJoint(APPROACH_000_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_003_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_006_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_009_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_012_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_013_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_015_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_018_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_019_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_021_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_024_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_027_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_030_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_033_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_035_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_036_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_039_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_041_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_042_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_045_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)

def execute_grasp_approach():
    """Final Cartesian descent into grasp alignment."""
    moveJoint(DESCEND_046_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_047_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

def close_gripper_on_object():
    """Close the gripper and let the grasp settle briefly."""
    moveJoint(GRASP_050_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_055_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_065_JOINTS, input_type='joint', steps=10, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_073_JOINTS, input_type='joint', steps=8, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_075_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_077_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_081_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_084_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_092_JOINTS, input_type='joint', steps=8, settle_steps=0, gripper=-1.000)
    moveJoint(GRASP_093_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(GRASP_094_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(GRASP_103_JOINTS, input_type='joint', steps=9, settle_steps=0, gripper=1.000)
    wait(duration=0.05, hold_target=GRASP_HOLD_103_JOINTS, input_type='joint', gripper=-0.3)
    grasp(hold_target=GRASP_HOLD_103_JOINTS, input_type='joint', close_steps=1, settle_steps=0, gripper=1.0, require_contact_count=4)
    wait(duration=0.05, hold_target=GRASP_HOLD_103_JOINTS, input_type='joint', gripper=1.0)

def lift_object():
    """Lift the grasped object clear of the counter."""
    moveJoint(LIFT_104_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_105_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_106_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_107_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_108_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_109_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)

def transport_to_place():
    """Carry the object through the curved transport corridor."""
    moveJoint(TRANSPORT_111_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_113_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_115_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_117_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_119_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_121_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_123_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_125_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_127_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_129_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_131_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_133_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_135_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_137_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_138_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)

def prepare_release():
    """Set the object into the receptacle and stabilize before release."""
    moveJoint(PLACE_139_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_140_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_142_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_144_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_146_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_148_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_150_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_152_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_154_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_156_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_158_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_160_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_162_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_164_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_166_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_168_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_170_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_172_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_174_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_176_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_178_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_180_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_182_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_184_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_186_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_188_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_190_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_192_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_194_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_196_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_198_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_200_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_202_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_204_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_206_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    wait(duration=0.05, hold_target=RELEASE_POSE_206_JOINTS, input_type='joint', gripper=-1.0)

def release_object():
    """Open the gripper and hold briefly to avoid disturbing the placement."""
    release(hold_target=RELEASE_POSE_206_JOINTS, input_type='joint', open_steps=1, settle_steps=0, gripper=-1.0)
    wait(duration=0.05, hold_target=RELEASE_POSE_206_JOINTS, input_type='joint', gripper=-1.0)

def retreat_after_release():
    """Retreat away from the placed object without clipping it."""
    moveJoint(RETRACT_207_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_208_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_209_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_210_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_211_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_212_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_213_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_214_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_215_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_216_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_217_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_218_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_219_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_220_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_221_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

move_to_pregrasp()
execute_grasp_approach()
close_gripper_on_object()
lift_object()
transport_to_place()
prepare_release()
release_object()
retreat_after_release()

# Fitting strategy:
# - adaptive per-phase waypoint selection on EE position + quaternion error
# - tight tolerances near descend / lift / place, looser tolerances in transport
# - optional moveC / moveA / segmented moveLinear chains / moveActionSequence when a segment is better represented by a smooth or constrained path
# - GT used only during generation; runtime execution uses this saved policy plus scene artifact

Pick the mug from the counter and place it under the coffee machine dispenser.


kettle

all kettle_non_electric receptacle kettle cookware