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

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

SCENE_ARTIFACT = '../../../artifacts/20260324/PickPlaceCabinetToCounter/20260324_205317_pick-place-cabinet-to-counter_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'PickPlaceCabinetToCounter',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/PickPlaceCabinetToCounter/20260324_205317_pick-place-cabinet-to-counter_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/PickPlaceCabinetToCounter/20260324_205317_pick-place-cabinet-to-counter_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/PickPlaceCabinetToCounter/20260324_205317_pick-place-cabinet-to-counter_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 106,
    'stable_release_contact_loss_t': 170,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=0.292534, y=-0.583483, z=1.423870, qw=0.433973, qx=-0.027999, qy=-0.001050, qz=0.900490)
GOAL_RECEPTACLE_POSE = pose(x=0.202578, y=-0.589669, z=1.464330, qw=0.683935, qx=0.001432, qy=0.002423, qz=0.729538)

APPROACH_005_JOINTS = [-0.013366, -1.054603, -0.059660, -2.290586, 0.051710, 1.509351, 0.710185]  # t=5
APPROACH_010_JOINTS = [-0.038409, -1.131126, -0.086543, -2.300407, -0.027067, 1.578086, 0.844423]  # t=10
APPROACH_011_JOINTS = [-0.045429, -1.145800, -0.088512, -2.313202, -0.053546, 1.603280, 0.887711]  # t=11
APPROACH_015_JOINTS = [-0.068971, -1.159818, -0.075622, -2.388780, -0.168473, 1.687651, 1.042418]  # t=15
APPROACH_017_JOINTS = [-0.074807, -1.160035, -0.050165, -2.422805, -0.213733, 1.722783, 1.087970]  # t=17
APPROACH_020_JOINTS = [-0.084129, -1.147700, -0.051419, -2.451915, -0.262599, 1.767963, 1.147416]  # t=20
APPROACH_022_JOINTS = [-0.091920, -1.132000, -0.067360, -2.447709, -0.297029, 1.784769, 1.181660]  # t=22
APPROACH_025_JOINTS = [-0.101343, -1.101693, -0.104466, -2.391441, -0.340911, 1.787597, 1.188952]  # t=25
APPROACH_030_JOINTS = [-0.102715, -1.028075, -0.187236, -2.222998, -0.350055, 1.794915, 1.123834]  # t=30
APPROACH_034_JOINTS = [-0.095166, -0.966121, -0.247856, -2.087922, -0.315240, 1.806590, 1.081942]  # t=34
APPROACH_035_JOINTS = [-0.092479, -0.951223, -0.265209, -2.061851, -0.303472, 1.814974, 1.073173]  # t=35
APPROACH_040_JOINTS = [-0.080588, -0.900506, -0.320863, -1.966502, -0.265469, 1.843098, 1.031101]  # t=40
APPROACH_043_JOINTS = [-0.075984, -0.890213, -0.330384, -1.941337, -0.258143, 1.870495, 1.018295]  # t=43
APPROACH_045_JOINTS = [-0.073234, -0.886230, -0.332030, -1.947693, -0.258495, 1.918863, 1.016186]  # t=45
APPROACH_048_JOINTS = [-0.068770, -0.864249, -0.340420, -1.970519, -0.274777, 2.036650, 1.010305]  # t=48
APPROACH_050_JOINTS = [-0.067049, -0.831502, -0.355010, -1.978530, -0.310005, 2.128140, 0.990691]  # t=50
APPROACH_054_JOINTS = [-0.060532, -0.737530, -0.388149, -1.941812, -0.360767, 2.265510, 0.952246]  # t=54
APPROACH_055_JOINTS = [-0.058061, -0.720253, -0.390084, -1.919944, -0.356217, 2.269175, 0.946897]  # t=55
APPROACH_058_JOINTS = [-0.052482, -0.672814, -0.386919, -1.845344, -0.343529, 2.223833, 0.926600]  # t=58
APPROACH_060_JOINTS = [-0.048248, -0.632852, -0.384961, -1.797957, -0.337128, 2.182743, 0.913505]  # t=60
APPROACH_062_JOINTS = [-0.043466, -0.589920, -0.384439, -1.756496, -0.328844, 2.155931, 0.907404]  # t=62
APPROACH_064_JOINTS = [-0.039447, -0.549153, -0.385427, -1.725159, -0.321449, 2.137771, 0.904479]  # t=64
APPROACH_065_JOINTS = [-0.037815, -0.529141, -0.387134, -1.712454, -0.318661, 2.131485, 0.902463]  # t=65
APPROACH_070_JOINTS = [-0.031513, -0.423396, -0.407497, -1.655949, -0.301298, 2.123554, 0.883510]  # t=70
APPROACH_075_JOINTS = [-0.029334, -0.305665, -0.458451, -1.580870, -0.252051, 2.133038, 0.834358]  # t=75
APPROACH_080_JOINTS = [-0.031339, -0.185749, -0.510370, -1.492546, -0.164280, 2.139407, 0.774334]  # t=80
APPROACH_085_JOINTS = [-0.043337, -0.067592, -0.531182, -1.401443, -0.092758, 2.127701, 0.736610]  # t=85
APPROACH_086_JOINTS = [-0.047817, -0.047832, -0.532757, -1.383124, -0.081850, 2.120167, 0.730542]  # t=86
DESCEND_088_JOINTS = [-0.058908, -0.012507, -0.535253, -1.347746, -0.058405, 2.107224, 0.719310]  # t=88
DESCEND_090_JOINTS = [-0.072420, 0.022908, -0.537014, -1.314399, -0.033309, 2.097751, 0.707238]  # t=90
DESCEND_092_JOINTS = [-0.088259, 0.059009, -0.538429, -1.280272, -0.006018, 2.091319, 0.695043]  # t=92
DESCEND_094_JOINTS = [-0.105892, 0.095256, -0.538324, -1.245456, 0.023377, 2.085343, 0.682513]  # t=94
DESCEND_095_JOINTS = [-0.114962, 0.111820, -0.536925, -1.226478, 0.038146, 2.080997, 0.677026]  # t=95
DESCEND_096_JOINTS = [-0.124090, 0.127627, -0.534532, -1.207406, 0.051669, 2.075177, 0.672021]  # t=96
DESCEND_097_JOINTS = [-0.133274, 0.142967, -0.531275, -1.188434, 0.063800, 2.068814, 0.667806]  # t=97
DESCEND_098_JOINTS = [-0.142568, 0.157793, -0.527481, -1.169316, 0.074427, 2.061475, 0.664108]  # t=98
DESCEND_099_JOINTS = [-0.151913, 0.172447, -0.523051, -1.149990, 0.083988, 2.053787, 0.661049]  # t=99
DESCEND_100_JOINTS = [-0.161210, 0.187178, -0.517743, -1.130741, 0.093330, 2.046579, 0.659331]  # t=100
DESCEND_102_JOINTS = [-0.179623, 0.217315, -0.505122, -1.091231, 0.109530, 2.033586, 0.658581]  # t=102
DESCEND_104_JOINTS = [-0.197905, 0.247473, -0.491179, -1.050066, 0.121296, 2.019545, 0.659191]  # t=104
DESCEND_105_JOINTS = [-0.207182, 0.262498, -0.484313, -1.028889, 0.125758, 2.011730, 0.659515]  # t=105
GRASP_HOLD_106_JOINTS = [-0.216643, 0.277735, -0.477650, -1.007043, 0.129776, 2.003954, 0.659767]  # t=106
LIFT_107_JOINTS = [-0.226379, 0.293053, -0.471352, -0.984556, 0.133634, 1.996030, 0.659793]  # t=107
LIFT_108_JOINTS = [-0.236442, 0.308661, -0.465502, -0.961357, 0.137020, 1.987750, 0.659274]  # t=108
LIFT_109_JOINTS = [-0.246822, 0.324577, -0.459890, -0.937223, 0.140555, 1.978929, 0.658366]  # t=109
LIFT_110_JOINTS = [-0.257445, 0.340813, -0.454285, -0.912236, 0.144442, 1.969816, 0.657307]  # t=110
LIFT_111_JOINTS = [-0.268284, 0.357241, -0.448671, -0.886598, 0.148430, 1.960294, 0.656216]  # t=111
LIFT_112_JOINTS = [-0.279477, 0.373269, -0.443139, -0.860834, 0.153431, 1.949726, 0.655782]  # t=112
LIFT_113_JOINTS = [-0.290918, 0.386587, -0.437705, -0.836092, 0.159658, 1.932764, 0.654942]  # t=113
LIFT_114_JOINTS = [-0.301968, 0.395960, -0.431856, -0.812986, 0.165431, 1.907847, 0.651138]  # t=114
LIFT_115_JOINTS = [-0.312879, 0.397310, -0.427397, -0.794715, 0.168650, 1.869162, 0.642764]  # t=115
LIFT_116_JOINTS = [-0.323165, 0.389994, -0.423046, -0.780839, 0.172913, 1.818660, 0.629766]  # t=116
LIFT_117_JOINTS = [-0.333099, 0.378128, -0.419125, -0.771385, 0.178520, 1.768453, 0.613637]  # t=117
LIFT_118_JOINTS = [-0.342633, 0.363875, -0.415921, -0.766486, 0.185027, 1.721894, 0.595852]  # t=118
LIFT_119_JOINTS = [-0.351642, 0.347616, -0.413356, -0.765694, 0.193504, 1.680476, 0.576930]  # t=119
LIFT_120_JOINTS = [-0.360107, 0.329479, -0.411762, -0.769216, 0.203961, 1.643132, 0.556848]  # t=120
LIFT_121_JOINTS = [-0.367413, 0.308695, -0.410393, -0.776688, 0.217109, 1.608006, 0.536202]  # t=121
PLACE_123_JOINTS = [-0.376658, 0.258219, -0.408094, -0.805364, 0.244391, 1.542207, 0.493313]  # t=123
PLACE_125_JOINTS = [-0.376239, 0.195705, -0.405579, -0.857049, 0.261080, 1.483644, 0.448004]  # t=125
PLACE_127_JOINTS = [-0.366328, 0.120819, -0.403682, -0.932639, 0.265699, 1.442668, 0.403462]  # t=127
PLACE_129_JOINTS = [-0.347718, 0.031449, -0.401740, -1.028299, 0.261983, 1.423178, 0.359711]  # t=129
PLACE_131_JOINTS = [-0.321851, -0.071975, -0.398336, -1.141435, 0.247819, 1.423123, 0.317557]  # t=131
PLACE_133_JOINTS = [-0.291497, -0.186652, -0.393558, -1.268273, 0.224485, 1.440295, 0.278215]  # t=133
PLACE_135_JOINTS = [-0.259656, -0.309480, -0.387910, -1.408921, 0.193929, 1.466983, 0.239958]  # t=135
PLACE_137_JOINTS = [-0.228898, -0.436501, -0.380608, -1.563263, 0.154296, 1.488804, 0.204207]  # t=137
PLACE_139_JOINTS = [-0.201294, -0.566202, -0.374985, -1.724316, 0.117325, 1.508487, 0.172781]  # t=139
PLACE_141_JOINTS = [-0.178217, -0.692161, -0.371859, -1.877702, 0.075254, 1.516712, 0.143175]  # t=141
PLACE_143_JOINTS = [-0.159081, -0.804679, -0.373351, -2.014370, 0.031109, 1.514902, 0.114466]  # t=143
PLACE_145_JOINTS = [-0.141112, -0.890057, -0.378591, -2.132909, -0.006855, 1.512604, 0.088722]  # t=145
PLACE_147_JOINTS = [-0.121892, -0.937579, -0.387399, -2.232130, -0.034652, 1.515184, 0.070688]  # t=147
PLACE_149_JOINTS = [-0.101635, -0.951266, -0.396591, -2.309747, -0.058342, 1.515202, 0.058591]  # t=149
PLACE_151_JOINTS = [-0.079102, -0.931197, -0.406395, -2.369824, -0.077107, 1.527682, 0.046599]  # t=151
PLACE_153_JOINTS = [-0.053218, -0.874619, -0.418235, -2.413351, -0.090253, 1.556528, 0.032352]  # t=153
PLACE_155_JOINTS = [-0.025745, -0.791620, -0.432385, -2.436535, -0.094677, 1.590701, 0.013514]  # t=155
PLACE_157_JOINTS = [0.000416, -0.696381, -0.446753, -2.443591, -0.092529, 1.621439, -0.010828]  # t=157
PLACE_159_JOINTS = [0.022296, -0.604018, -0.463098, -2.442835, -0.079948, 1.644866, -0.046723]  # t=159
PLACE_161_JOINTS = [0.037572, -0.520304, -0.484621, -2.442300, -0.061290, 1.665602, -0.094555]  # t=161
PLACE_163_JOINTS = [0.045844, -0.448980, -0.510577, -2.447329, -0.035888, 1.691154, -0.151646]  # t=163
PLACE_164_JOINTS = [0.048006, -0.417025, -0.523175, -2.453112, -0.022148, 1.710218, -0.182127]  # t=164
PLACE_165_JOINTS = [0.048972, -0.388075, -0.534352, -2.461928, -0.009107, 1.729022, -0.211657]  # t=165
PLACE_167_JOINTS = [0.049028, -0.343389, -0.545782, -2.484295, 0.020692, 1.766953, -0.261698]  # t=167
PLACE_169_JOINTS = [0.041633, -0.327668, -0.557814, -2.507054, 0.042376, 1.798673, -0.312542]  # t=169
PLACE_170_JOINTS = [0.036397, -0.323721, -0.565031, -2.523577, 0.051173, 1.820655, -0.339157]  # t=170
RELEASE_POSE_170_JOINTS = [0.036397, -0.323721, -0.565031, -2.523577, 0.051173, 1.820655, -0.339157]  # t=170
RETRACT_172_JOINTS = [0.026369, -0.318309, -0.575986, -2.562083, 0.071147, 1.878208, -0.391732]  # t=172
RETRACT_173_JOINTS = [0.021222, -0.318874, -0.580942, -2.576580, 0.082095, 1.908551, -0.418362]  # t=173
RETRACT_175_JOINTS = [0.010382, -0.328438, -0.590411, -2.593420, 0.102478, 1.951468, -0.466730]  # t=175
RETRACT_176_JOINTS = [0.005210, -0.337133, -0.594429, -2.603242, 0.113762, 1.968488, -0.489771]  # t=176
RETRACT_179_JOINTS = [-0.008781, -0.377116, -0.605297, -2.638021, 0.149563, 2.023474, -0.558458]  # t=179
RETRACT_180_JOINTS = [-0.013649, -0.395481, -0.610921, -2.650444, 0.160262, 2.043328, -0.583560]  # t=180
RETRACT_182_JOINTS = [-0.029405, -0.458175, -0.628284, -2.668794, 0.165363, 2.048568, -0.628877]  # t=182
RETRACT_185_JOINTS = [-0.071741, -0.622578, -0.643669, -2.678980, 0.099437, 1.961004, -0.628216]  # t=185
RETRACT_186_JOINTS = [-0.087087, -0.686391, -0.634929, -2.683969, 0.061225, 1.928563, -0.604315]  # t=186
RETRACT_188_JOINTS = [-0.112931, -0.818688, -0.598856, -2.701005, -0.000323, 1.871734, -0.545260]  # t=188
RETRACT_189_JOINTS = [-0.121667, -0.882658, -0.572820, -2.713291, -0.017641, 1.850537, -0.512990]  # t=189
RETRACT_191_JOINTS = [-0.127858, -0.996322, -0.508450, -2.747042, -0.020599, 1.830529, -0.446896]  # t=191
RETRACT_194_JOINTS = [-0.124530, -1.136584, -0.424254, -2.803239, 0.012682, 1.832552, -0.369788]  # t=194
RETRACT_195_JOINTS = [-0.123843, -1.179709, -0.406332, -2.818219, 0.024026, 1.834540, -0.354517]  # t=195
RETRACT_197_JOINTS = [-0.122953, -1.261988, -0.378983, -2.840957, 0.045006, 1.837060, -0.332321]  # t=197
RETRACT_200_JOINTS = [-0.114772, -1.362155, -0.332800, -2.859222, 0.084271, 1.845671, -0.296921]  # t=200
RETRACT_203_JOINTS = [-0.098273, -1.425409, -0.277340, -2.841668, 0.124582, 1.837572, -0.249782]  # t=203
RETRACT_205_JOINTS = [-0.084044, -1.442534, -0.240915, -2.812875, 0.146108, 1.826019, -0.214675]  # t=205
RETRACT_206_JOINTS = [-0.076783, -1.444431, -0.226000, -2.794917, 0.153722, 1.820234, -0.198926]  # t=206
RETRACT_209_JOINTS = [-0.056583, -1.429737, -0.195843, -2.726378, 0.162400, 1.792407, -0.154159]  # t=209
RETRACT_212_JOINTS = [-0.039000, -1.388439, -0.177712, -2.640689, 0.156073, 1.734262, -0.102339]  # t=212
RETRACT_215_JOINTS = [-0.025183, -1.346184, -0.167539, -2.556639, 0.145221, 1.669597, -0.054477]  # t=215
RETRACT_218_JOINTS = [-0.015462, -1.319436, -0.162572, -2.493377, 0.135560, 1.619030, -0.018680]  # t=218
RETRACT_219_JOINTS = [-0.013282, -1.316872, -0.161613, -2.474925, 0.132523, 1.599873, -0.008301]  # t=219
APPROACH_000_JOINTS = [-0.010520, -1.025454, -0.058702, -2.297071, 0.053644, 1.544523, 0.706240]  # 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_005_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_010_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_011_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_015_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_017_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_020_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_022_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_025_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_030_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_034_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_035_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_040_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_043_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_045_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_048_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_050_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_054_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_055_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_058_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_060_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_062_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_064_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_065_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_070_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_075_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_080_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_085_JOINTS, input_type='joint', steps=5, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_086_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

def execute_grasp_approach():
    """Final Cartesian descent into grasp alignment."""
    moveJoint(DESCEND_088_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_090_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_092_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_094_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_095_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_096_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_097_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_098_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_099_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_100_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_102_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_104_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_105_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_106_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_106_JOINTS, input_type='joint', gripper=1.0)

def lift_object():
    """Lift the grasped object clear of the counter."""
    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)
    moveJoint(LIFT_110_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_111_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_112_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_113_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_114_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_115_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_116_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_117_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_118_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_119_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_120_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_121_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_123_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_125_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_127_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_129_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_131_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_133_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_135_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_137_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_139_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_141_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_143_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_145_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_147_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_149_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_151_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_153_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_155_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_157_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_159_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_161_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_163_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_164_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_165_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_167_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_169_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_170_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    wait(duration=0.05, hold_target=RELEASE_POSE_170_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_170_JOINTS, input_type='joint', open_steps=1, settle_steps=0, gripper=-1.0)
    wait(duration=0.05, hold_target=RELEASE_POSE_170_JOINTS, input_type='joint', gripper=-1.0)

def retreat_after_release():
    """Retreat away from the placed object without clipping it."""
    moveJoint(RETRACT_172_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_173_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_175_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_176_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_179_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_180_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_182_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_185_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_186_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_188_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_189_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_191_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_194_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_195_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_197_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_200_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_203_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_205_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_206_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_209_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_212_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_215_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_218_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_219_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)

move_to_pregrasp()
execute_grasp_approach()
close_gripper_on_object()
lift_object()
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 ice cube from the cabinet and place it on the counter.


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

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

SCENE_ARTIFACT = '../../../artifacts/20260324/PickPlaceCounterToBlender/20260324_205318_pick-place-counter-to-blender_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'PickPlaceCounterToBlender',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/PickPlaceCounterToBlender/20260324_205318_pick-place-counter-to-blender_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/PickPlaceCounterToBlender/20260324_205318_pick-place-counter-to-blender_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/PickPlaceCounterToBlender/20260324_205318_pick-place-counter-to-blender_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 106,
    'stable_release_contact_loss_t': 239,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=3.707522, y=-2.777146, z=0.949463, qw=0.988361, qx=0.032758, qy=-0.028550, qz=0.145789)
GOAL_RECEPTACLE_POSE = pose(x=3.318372, y=-2.718032, z=0.995802, qw=0.933261, qx=-0.000052, qy=-0.000092, qz=-0.359199)

APPROACH_004_JOINTS = [-0.004513, -0.975402, -0.077698, -2.265469, 0.036446, 1.507853, 0.677439]  # t=4
APPROACH_008_JOINTS = [-0.029325, -1.020080, -0.161287, -2.289899, -0.045215, 1.478458, 0.712154]  # t=8
APPROACH_009_JOINTS = [-0.035408, -1.031687, -0.178755, -2.300794, -0.065054, 1.481180, 0.724926]  # t=9
APPROACH_012_JOINTS = [-0.050922, -1.045022, -0.233137, -2.340244, -0.126135, 1.512850, 0.750284]  # t=12
APPROACH_016_JOINTS = [-0.065083, -1.031710, -0.329651, -2.367206, -0.204112, 1.580631, 0.713938]  # t=16
APPROACH_020_JOINTS = [-0.068422, -0.978979, -0.419488, -2.349825, -0.264963, 1.624175, 0.647711]  # t=20
APPROACH_024_JOINTS = [-0.058039, -0.883668, -0.482363, -2.311338, -0.298088, 1.655040, 0.600849]  # t=24
APPROACH_028_JOINTS = [-0.040694, -0.755678, -0.557520, -2.265541, -0.334384, 1.690155, 0.550666]  # t=28
APPROACH_032_JOINTS = [-0.017861, -0.614016, -0.634389, -2.206449, -0.347746, 1.725950, 0.494810]  # t=32
APPROACH_036_JOINTS = [0.007392, -0.466072, -0.703855, -2.137802, -0.331738, 1.767585, 0.437588]  # t=36
APPROACH_039_JOINTS = [0.022765, -0.355429, -0.749111, -2.076491, -0.299406, 1.787387, 0.389070]  # t=39
APPROACH_040_JOINTS = [0.026530, -0.322779, -0.759231, -2.056245, -0.281052, 1.788756, 0.373595]  # t=40
APPROACH_044_JOINTS = [0.030217, -0.209049, -0.780202, -1.999595, -0.203095, 1.798628, 0.325751]  # t=44
APPROACH_048_JOINTS = [0.015513, -0.114840, -0.775760, -1.971489, -0.133306, 1.823450, 0.293118]  # t=48
APPROACH_052_JOINTS = [-0.014102, -0.034748, -0.749682, -1.948361, -0.072006, 1.850553, 0.269391]  # t=52
APPROACH_056_JOINTS = [-0.054415, 0.038675, -0.710776, -1.916947, -0.017497, 1.873280, 0.250175]  # t=56
APPROACH_060_JOINTS = [-0.101692, 0.109448, -0.669072, -1.881155, 0.039974, 1.884343, 0.224479]  # t=60
APPROACH_063_JOINTS = [-0.139238, 0.163528, -0.637393, -1.854043, 0.075441, 1.896252, 0.206615]  # t=63
APPROACH_064_JOINTS = [-0.152225, 0.179212, -0.628482, -1.845157, 0.084579, 1.897970, 0.199890]  # t=64
APPROACH_068_JOINTS = [-0.199686, 0.222983, -0.591017, -1.824498, 0.106362, 1.889292, 0.181713]  # t=68
DESCEND_070_JOINTS = [-0.218945, 0.237318, -0.570229, -1.821045, 0.108114, 1.878609, 0.181292]  # t=70
DESCEND_072_JOINTS = [-0.235353, 0.243820, -0.551484, -1.820253, 0.108413, 1.872933, 0.182594]  # t=72
DESCEND_074_JOINTS = [-0.248541, 0.244451, -0.534871, -1.823299, 0.106907, 1.871317, 0.185426]  # t=74
DESCEND_075_JOINTS = [-0.254327, 0.243829, -0.528440, -1.825763, 0.105171, 1.871656, 0.186644]  # t=75
DESCEND_076_JOINTS = [-0.259748, 0.242425, -0.523682, -1.828718, 0.103008, 1.871846, 0.186879]  # t=76
DESCEND_078_JOINTS = [-0.269061, 0.238576, -0.516920, -1.836291, 0.099023, 1.873138, 0.185925]  # t=78
DESCEND_079_JOINTS = [-0.273011, 0.236052, -0.514917, -1.841524, 0.096875, 1.873814, 0.184855]  # t=79
DESCEND_080_JOINTS = [-0.276645, 0.232928, -0.514194, -1.848269, 0.094539, 1.874635, 0.183683]  # t=80
DESCEND_081_JOINTS = [-0.279958, 0.229024, -0.514493, -1.856498, 0.092136, 1.875838, 0.182444]  # t=81
DESCEND_082_JOINTS = [-0.282893, 0.224363, -0.515420, -1.865756, 0.089538, 1.877772, 0.180743]  # t=82
DESCEND_084_JOINTS = [-0.287206, 0.214241, -0.517201, -1.886166, 0.084479, 1.884829, 0.176067]  # t=84
DESCEND_086_JOINTS = [-0.289064, 0.204870, -0.517150, -1.907112, 0.080522, 1.896112, 0.174026]  # t=86
DESCEND_088_JOINTS = [-0.289002, 0.201615, -0.515225, -1.924898, 0.078008, 1.913484, 0.175186]  # t=88
DESCEND_090_JOINTS = [-0.288565, 0.203365, -0.513772, -1.940216, 0.077993, 1.932643, 0.175758]  # t=90
DESCEND_092_JOINTS = [-0.287807, 0.208907, -0.511405, -1.951538, 0.081093, 1.953196, 0.175841]  # t=92
DESCEND_094_JOINTS = [-0.287061, 0.215797, -0.508696, -1.957555, 0.085392, 1.969396, 0.176144]  # t=94
DESCEND_096_JOINTS = [-0.286690, 0.224644, -0.505764, -1.960687, 0.090243, 1.983188, 0.176286]  # t=96
DESCEND_098_JOINTS = [-0.286767, 0.235549, -0.502319, -1.961816, 0.096029, 1.996000, 0.176290]  # t=98
DESCEND_100_JOINTS = [-0.287467, 0.245647, -0.499685, -1.962353, 0.101024, 2.005285, 0.176033]  # t=100
DESCEND_101_JOINTS = [-0.288079, 0.250185, -0.498671, -1.962887, 0.103153, 2.009162, 0.175582]  # t=101
DESCEND_102_JOINTS = [-0.288776, 0.254152, -0.497641, -1.963783, 0.105126, 2.012502, 0.174993]  # t=102
DESCEND_103_JOINTS = [-0.289451, 0.257524, -0.496340, -1.965106, 0.106949, 2.015489, 0.174598]  # t=103
DESCEND_104_JOINTS = [-0.290076, 0.260419, -0.494748, -1.966875, 0.108555, 2.018459, 0.174531]  # t=104
DESCEND_105_JOINTS = [-0.290521, 0.262489, -0.492625, -1.968726, 0.110126, 2.020945, 0.174733]  # t=105
GRASP_HOLD_106_JOINTS = [-0.290769, 0.263359, -0.490261, -1.970053, 0.111119, 2.022247, 0.175562]  # t=106
LIFT_107_JOINTS = [-0.291014, 0.263508, -0.488267, -1.970979, 0.111321, 2.022768, 0.176688]  # t=107
LIFT_108_JOINTS = [-0.291266, 0.262862, -0.486765, -1.971352, 0.111090, 2.022390, 0.177774]  # t=108
LIFT_109_JOINTS = [-0.291403, 0.262515, -0.485620, -1.970817, 0.108602, 2.020859, 0.179975]  # t=109
LIFT_110_JOINTS = [-0.291362, 0.262975, -0.484539, -1.969034, 0.103224, 2.017146, 0.183993]  # t=110
LIFT_111_JOINTS = [-0.291560, 0.261346, -0.484575, -1.967448, 0.098502, 2.010543, 0.186907]  # t=111
LIFT_112_JOINTS = [-0.291930, 0.256502, -0.485690, -1.966100, 0.095018, 2.002607, 0.187691]  # t=112
LIFT_113_JOINTS = [-0.291963, 0.246766, -0.487646, -1.962630, 0.089578, 1.991973, 0.188828]  # t=113
LIFT_114_JOINTS = [-0.291344, 0.232841, -0.489436, -1.956970, 0.082021, 1.977156, 0.191483]  # t=114
LIFT_115_JOINTS = [-0.290145, 0.215597, -0.490853, -1.948852, 0.074440, 1.957241, 0.194977]  # t=115
LIFT_116_JOINTS = [-0.288487, 0.196406, -0.492234, -1.939259, 0.066535, 1.933358, 0.199110]  # t=116
LIFT_117_JOINTS = [-0.286471, 0.177616, -0.493682, -1.928464, 0.057799, 1.908867, 0.203975]  # t=117
TRANSPORT_120_JOINTS = [-0.280624, 0.114420, -0.505175, -1.898126, 0.045785, 1.838444, 0.206296]  # t=120
TRANSPORT_122_JOINTS = [-0.274022, 0.063469, -0.511190, -1.886090, 0.051096, 1.799195, 0.203504]  # t=122
TRANSPORT_123_JOINTS = [-0.268930, 0.035830, -0.511215, -1.882127, 0.054852, 1.781142, 0.205399]  # t=123
TRANSPORT_126_JOINTS = [-0.247833, -0.059748, -0.503722, -1.880203, 0.054437, 1.733519, 0.214862]  # t=126
TRANSPORT_129_JOINTS = [-0.219780, -0.175098, -0.483595, -1.894747, 0.029732, 1.709202, 0.235327]  # t=129
TRANSPORT_132_JOINTS = [-0.188078, -0.297704, -0.454480, -1.923570, 0.013634, 1.728760, 0.265517]  # t=132
TRANSPORT_134_JOINTS = [-0.166036, -0.379888, -0.430629, -1.948044, 0.013893, 1.754932, 0.280150]  # t=134
TRANSPORT_135_JOINTS = [-0.154869, -0.421262, -0.416798, -1.961748, 0.017360, 1.768379, 0.285342]  # t=135
TRANSPORT_138_JOINTS = [-0.121122, -0.533249, -0.376862, -1.999403, 0.045461, 1.800508, 0.285129]  # t=138
TRANSPORT_141_JOINTS = [-0.087919, -0.621685, -0.339038, -2.017846, 0.089861, 1.832016, 0.270579]  # t=141
TRANSPORT_144_JOINTS = [-0.056160, -0.688777, -0.296530, -2.021288, 0.142372, 1.865964, 0.257786]  # t=144
TRANSPORT_147_JOINTS = [-0.025594, -0.725299, -0.251663, -2.018762, 0.197842, 1.905258, 0.243955]  # t=147
TRANSPORT_150_JOINTS = [0.001807, -0.738807, -0.201902, -1.996298, 0.247728, 1.932035, 0.238851]  # t=150
TRANSPORT_153_JOINTS = [0.024098, -0.735540, -0.156220, -1.960829, 0.292652, 1.920343, 0.251538]  # t=153
TRANSPORT_156_JOINTS = [0.040959, -0.725199, -0.116250, -1.939655, 0.329371, 1.896967, 0.257157]  # t=156
TRANSPORT_157_JOINTS = [0.045220, -0.720866, -0.099962, -1.935304, 0.336683, 1.888892, 0.261964]  # t=157
TRANSPORT_159_JOINTS = [0.051395, -0.710122, -0.062173, -1.925462, 0.340927, 1.873150, 0.280557]  # t=159
TRANSPORT_162_JOINTS = [0.055668, -0.684157, 0.003424, -1.904640, 0.333929, 1.846515, 0.340068]  # t=162
TRANSPORT_164_JOINTS = [0.055908, -0.656762, 0.046388, -1.884675, 0.328320, 1.839514, 0.388271]  # t=164
TRANSPORT_165_JOINTS = [0.054874, -0.641271, 0.061411, -1.875687, 0.323397, 1.837248, 0.406625]  # t=165
TRANSPORT_166_JOINTS = [0.053188, -0.626836, 0.072040, -1.866978, 0.316848, 1.837439, 0.418864]  # t=166
TRANSPORT_168_JOINTS = [0.048692, -0.605785, 0.084471, -1.851553, 0.296312, 1.828870, 0.427096]  # t=168
TRANSPORT_171_JOINTS = [0.042069, -0.588831, 0.103844, -1.838500, 0.256557, 1.815302, 0.424532]  # t=171
TRANSPORT_174_JOINTS = [0.036189, -0.581557, 0.127515, -1.834294, 0.218548, 1.807738, 0.427920]  # t=174
TRANSPORT_177_JOINTS = [0.030879, -0.577598, 0.143830, -1.837812, 0.192985, 1.806440, 0.429586]  # t=177
TRANSPORT_180_JOINTS = [0.026123, -0.573232, 0.154946, -1.844431, 0.172879, 1.800964, 0.429375]  # t=180
TRANSPORT_183_JOINTS = [0.022499, -0.569694, 0.168483, -1.847480, 0.154331, 1.800403, 0.430261]  # t=183
TRANSPORT_186_JOINTS = [0.019589, -0.568690, 0.179687, -1.849137, 0.143257, 1.800512, 0.434826]  # t=186
TRANSPORT_189_JOINTS = [0.017291, -0.568755, 0.182256, -1.849039, 0.143218, 1.800511, 0.435625]  # t=189
TRANSPORT_191_JOINTS = [0.016439, -0.569071, 0.184096, -1.849008, 0.143997, 1.799962, 0.434529]  # t=191
TRANSPORT_192_JOINTS = [0.016230, -0.570660, 0.185157, -1.849104, 0.144220, 1.797570, 0.434124]  # t=192
TRANSPORT_195_JOINTS = [0.015193, -0.598647, 0.180334, -1.854568, 0.141310, 1.765143, 0.441153]  # t=195
TRANSPORT_198_JOINTS = [0.016119, -0.652166, 0.176938, -1.873006, 0.141455, 1.751153, 0.439126]  # t=198
TRANSPORT_200_JOINTS = [0.016662, -0.681989, 0.177671, -1.895354, 0.142634, 1.762390, 0.438200]  # t=200
TRANSPORT_201_JOINTS = [0.016856, -0.693963, 0.178061, -1.907342, 0.143901, 1.767804, 0.437995]  # t=201
TRANSPORT_204_JOINTS = [0.017436, -0.724315, 0.178475, -1.935745, 0.148669, 1.773939, 0.437769]  # t=204
TRANSPORT_206_JOINTS = [0.018131, -0.743472, 0.180593, -1.948579, 0.152390, 1.771797, 0.439437]  # t=206
TRANSPORT_207_JOINTS = [0.018684, -0.752771, 0.183586, -1.953899, 0.154681, 1.769352, 0.441466]  # t=207
TRANSPORT_210_JOINTS = [0.020776, -0.781517, 0.201125, -1.967053, 0.159967, 1.757589, 0.455015]  # t=210
TRANSPORT_212_JOINTS = [0.021582, -0.794643, 0.217738, -1.974013, 0.161699, 1.756236, 0.467892]  # t=212
TRANSPORT_213_JOINTS = [0.021607, -0.798258, 0.225972, -1.977802, 0.162298, 1.757946, 0.474533]  # t=213
TRANSPORT_216_JOINTS = [0.021078, -0.799595, 0.244456, -1.994822, 0.167458, 1.756181, 0.490226]  # t=216
TRANSPORT_219_JOINTS = [0.020153, -0.795697, 0.247801, -2.006464, 0.176333, 1.736125, 0.495250]  # t=219
TRANSPORT_222_JOINTS = [0.018946, -0.786748, 0.247573, -2.010338, 0.185072, 1.710164, 0.495527]  # t=222
TRANSPORT_225_JOINTS = [0.017866, -0.776441, 0.247540, -2.014771, 0.193200, 1.684290, 0.495810]  # t=225
TRANSPORT_228_JOINTS = [0.016984, -0.764599, 0.247446, -2.020062, 0.201782, 1.657017, 0.496498]  # t=228
TRANSPORT_231_JOINTS = [0.016266, -0.751176, 0.247141, -2.025713, 0.210743, 1.629404, 0.497471]  # t=231
TRANSPORT_234_JOINTS = [0.015475, -0.742137, 0.246065, -2.016738, 0.213376, 1.615197, 0.497745]  # t=234
TRANSPORT_237_JOINTS = [0.014926, -0.737648, 0.246055, -2.010546, 0.212990, 1.610049, 0.497755]  # t=237
TRANSPORT_239_JOINTS = [0.014919, -0.740573, 0.245931, -2.008561, 0.212244, 1.601325, 0.502281]  # t=239
RELEASE_POSE_239_JOINTS = [0.014919, -0.740573, 0.245931, -2.008561, 0.212244, 1.601325, 0.502281]  # t=239
RETRACT_241_JOINTS = [0.015051, -0.754608, 0.248159, -2.000964, 0.207306, 1.584159, 0.521612]  # t=241
RETRACT_243_JOINTS = [0.014914, -0.770842, 0.256362, -1.984471, 0.192206, 1.569199, 0.543310]  # t=243
RETRACT_245_JOINTS = [0.015003, -0.793036, 0.262548, -1.956788, 0.175826, 1.552781, 0.564085]  # t=245
RETRACT_247_JOINTS = [0.015563, -0.823848, 0.266044, -1.917965, 0.158803, 1.532784, 0.580994]  # t=247
RETRACT_249_JOINTS = [0.016359, -0.857583, 0.268550, -1.873207, 0.144226, 1.513945, 0.592321]  # t=249
RETRACT_251_JOINTS = [0.016893, -0.890040, 0.269548, -1.829886, 0.133160, 1.501468, 0.597343]  # t=251
RETRACT_253_JOINTS = [0.017016, -0.922824, 0.271308, -1.795339, 0.122848, 1.493055, 0.600320]  # t=253
RETRACT_254_JOINTS = [0.017421, -0.942154, 0.271240, -1.784988, 0.120858, 1.489875, 0.603615]  # t=254
RETRACT_255_JOINTS = [0.018541, -0.967814, 0.268516, -1.781873, 0.119662, 1.486570, 0.607340]  # t=255
RETRACT_257_JOINTS = [0.022947, -1.040814, 0.251505, -1.795549, 0.114346, 1.501836, 0.610932]  # t=257
RETRACT_259_JOINTS = [0.029713, -1.132410, 0.232119, -1.837376, 0.107575, 1.538672, 0.605043]  # t=259
RETRACT_260_JOINTS = [0.033332, -1.180994, 0.224703, -1.863793, 0.103660, 1.560443, 0.600548]  # t=260
RETRACT_261_JOINTS = [0.036980, -1.230362, 0.218140, -1.891653, 0.100207, 1.583603, 0.596630]  # t=261
RETRACT_263_JOINTS = [0.044109, -1.328874, 0.208523, -1.948672, 0.094236, 1.627653, 0.589389]  # t=263
RETRACT_265_JOINTS = [0.050945, -1.425314, 0.203851, -2.004003, 0.090592, 1.661871, 0.584086]  # t=265
RETRACT_266_JOINTS = [0.054397, -1.471632, 0.201748, -2.030222, 0.091119, 1.671089, 0.583635]  # t=266
APPROACH_000_JOINTS = [0.003835, -0.973922, -0.033819, -2.265038, 0.057100, 1.510168, 0.688931]  # 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_004_JOINTS, input_type='joint', steps=3, 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_012_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_016_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_020_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_024_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_028_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_032_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_036_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_039_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_040_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_044_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_048_JOINTS, input_type='joint', steps=4, 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_063_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_064_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_068_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)

def execute_grasp_approach():
    """Final Cartesian descent into grasp alignment."""
    moveJoint(DESCEND_070_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_072_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_074_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_075_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_076_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_078_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_079_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_080_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_081_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_082_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_084_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_086_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_088_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_090_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_092_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_094_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_096_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_098_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_100_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_101_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_102_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_103_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_104_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_105_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_106_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_106_JOINTS, input_type='joint', gripper=1.0)

def lift_object():
    """Lift the grasped object clear of the counter."""
    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)
    moveJoint(LIFT_110_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_111_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_112_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_113_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_114_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_115_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_116_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_117_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_120_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_122_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_123_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_126_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_129_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_132_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_134_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_135_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_138_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_141_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_144_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_147_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_150_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_153_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_156_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_157_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_159_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_162_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_164_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_165_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_166_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_168_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_171_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_174_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_177_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_180_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_183_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_186_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_189_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_191_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_192_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_195_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_198_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_200_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_201_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_204_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_206_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_207_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_210_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_212_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_213_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_216_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_219_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_222_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_225_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_228_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_231_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_234_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_237_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(TRANSPORT_239_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)

def prepare_release():
    """Set the object into the receptacle and stabilize before release."""
    wait(duration=0.05, hold_target=RELEASE_POSE_239_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_239_JOINTS, input_type='joint', open_steps=1, settle_steps=0, gripper=-1.0)
    wait(duration=0.05, hold_target=RELEASE_POSE_239_JOINTS, input_type='joint', gripper=-1.0)

def retreat_after_release():
    """Retreat away from the placed object without clipping it."""
    moveJoint(RETRACT_241_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_243_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_245_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_247_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_249_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_251_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_253_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_254_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_255_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_257_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_259_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_260_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_261_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_263_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_265_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_266_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 orange from the counter and place it in the blender.

obj

all orange fruit food in_container food_set1


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

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

SCENE_ARTIFACT = '../../../artifacts/20260324/PickPlaceCounterToCabinet/20260324_205319_pick-place-counter-to-cabinet_episode_000000_action2code-open-loop-joint_initial-scene.json'
POLICY_METADATA = {
    'policy_kind': 'open_loop_joint',
    'policy_variant': 'open-loop-joint',
    'task_name': 'PickPlaceCounterToCabinet',
    'episode_id': 'episode_000000',
    'scene_artifact_path': '../../../artifacts/20260324/PickPlaceCounterToCabinet/20260324_205319_pick-place-counter-to-cabinet_episode_000000_action2code-open-loop-joint_initial-scene.json',
    'fitting_summary_path': '../../../artifacts/20260324/PickPlaceCounterToCabinet/20260324_205319_pick-place-counter-to-cabinet_episode_000000_action2code-open-loop-joint_fitting-summary.json',
    'extracted_pose_metadata_path': '../../../artifacts/20260324/PickPlaceCounterToCabinet/20260324_205319_pick-place-counter-to-cabinet_episode_000000_action2code-open-loop-joint_extracted-pose-metadata.json',
    'stable_grasp_contact_t': 91,
    'stable_release_contact_loss_t': 195,
    'uses_moveC': False,
    'controller_mode': 'whole_body_ik',
}

OBJ_POSE_T0 = pose(x=0.468258, y=-1.823974, z=0.992418, qw=0.747907, qx=0.000000, qy=0.000000, qz=0.663803)
GOAL_RECEPTACLE_POSE = pose(x=0.141651, y=-2.414321, z=1.435520, qw=0.652670, qx=0.000070, qy=-0.000314, qz=0.757642)

APPROACH_004_JOINTS = [-0.031540, -1.105440, -0.025354, -2.287643, 0.010668, 1.511874, 0.691151]  # t=4
APPROACH_006_JOINTS = [-0.032965, -1.144083, -0.047622, -2.292414, -0.009527, 1.470265, 0.678572]  # t=6
APPROACH_008_JOINTS = [-0.036075, -1.198781, -0.079405, -2.298075, -0.039432, 1.427257, 0.659960]  # t=8
APPROACH_012_JOINTS = [-0.047757, -1.338622, -0.161595, -2.312310, -0.115935, 1.373242, 0.614218]  # t=12
APPROACH_016_JOINTS = [-0.063244, -1.466508, -0.257879, -2.345251, -0.208643, 1.360355, 0.543038]  # t=16
APPROACH_019_JOINTS = [-0.077602, -1.536601, -0.338727, -2.370777, -0.281844, 1.387118, 0.499525]  # t=19
APPROACH_020_JOINTS = [-0.082341, -1.552429, -0.366502, -2.380266, -0.306128, 1.403769, 0.487938]  # t=20
APPROACH_024_JOINTS = [-0.097615, -1.577044, -0.477429, -2.411363, -0.389586, 1.477086, 0.427368]  # t=24
APPROACH_028_JOINTS = [-0.112510, -1.567817, -0.553160, -2.414790, -0.434595, 1.527183, 0.369286]  # t=28
APPROACH_030_JOINTS = [-0.120474, -1.557258, -0.579742, -2.415722, -0.448163, 1.547782, 0.350132]  # t=30
APPROACH_032_JOINTS = [-0.126312, -1.539578, -0.604616, -2.425846, -0.461839, 1.583743, 0.342280]  # t=32
APPROACH_036_JOINTS = [-0.128551, -1.478546, -0.661784, -2.459797, -0.496765, 1.683702, 0.331994]  # t=36
APPROACH_040_JOINTS = [-0.126149, -1.404449, -0.710818, -2.485058, -0.519101, 1.780240, 0.323616]  # t=40
APPROACH_044_JOINTS = [-0.116675, -1.316790, -0.754273, -2.509899, -0.534608, 1.883273, 0.320179]  # t=44
APPROACH_046_JOINTS = [-0.109885, -1.270624, -0.770036, -2.521292, -0.537172, 1.930795, 0.320129]  # t=46
APPROACH_048_JOINTS = [-0.103691, -1.226845, -0.779371, -2.532019, -0.535865, 1.970457, 0.322080]  # t=48
APPROACH_052_JOINTS = [-0.093833, -1.148566, -0.789519, -2.550526, -0.528411, 2.039710, 0.327971]  # t=52
APPROACH_056_JOINTS = [-0.079815, -1.063371, -0.809203, -2.562885, -0.527077, 2.117237, 0.330378]  # t=56
APPROACH_060_JOINTS = [-0.060171, -0.972502, -0.834603, -2.565130, -0.520831, 2.194487, 0.327877]  # t=60
APPROACH_064_JOINTS = [-0.038549, -0.882951, -0.859733, -2.563361, -0.508150, 2.265161, 0.321503]  # t=64
APPROACH_068_JOINTS = [-0.016664, -0.795942, -0.883786, -2.559870, -0.489598, 2.330910, 0.312120]  # t=68
APPROACH_072_JOINTS = [0.006855, -0.709277, -0.909010, -2.553053, -0.461261, 2.398042, 0.295587]  # t=72
DESCEND_073_JOINTS = [0.013041, -0.687362, -0.915720, -2.550336, -0.452298, 2.414590, 0.289744]  # t=73
DESCEND_074_JOINTS = [0.019185, -0.665773, -0.921723, -2.547517, -0.442595, 2.429590, 0.283574]  # t=74
DESCEND_075_JOINTS = [0.024950, -0.645305, -0.926227, -2.544788, -0.432254, 2.442280, 0.277658]  # t=75
DESCEND_076_JOINTS = [0.030063, -0.626468, -0.929191, -2.542698, -0.421552, 2.452374, 0.271634]  # t=76
DESCEND_077_JOINTS = [0.034648, -0.608705, -0.931472, -2.540774, -0.410839, 2.461522, 0.264703]  # t=77
DESCEND_078_JOINTS = [0.038499, -0.592358, -0.933224, -2.535635, -0.400028, 2.468466, 0.256012]  # t=78
DESCEND_079_JOINTS = [0.041968, -0.576998, -0.934796, -2.527888, -0.388462, 2.471896, 0.244819]  # t=79
DESCEND_080_JOINTS = [0.045163, -0.562893, -0.936103, -2.518529, -0.375750, 2.471718, 0.230869]  # t=80
DESCEND_081_JOINTS = [0.047506, -0.551729, -0.935910, -2.509125, -0.362232, 2.466272, 0.215301]  # t=81
DESCEND_082_JOINTS = [0.048833, -0.543536, -0.933867, -2.500739, -0.349030, 2.456977, 0.199484]  # t=82
DESCEND_083_JOINTS = [0.049765, -0.536434, -0.930135, -2.493729, -0.337193, 2.446157, 0.183729]  # t=83
DESCEND_084_JOINTS = [0.050874, -0.528580, -0.924786, -2.487210, -0.327387, 2.434483, 0.168542]  # t=84
DESCEND_085_JOINTS = [0.052641, -0.519557, -0.917504, -2.480261, -0.318522, 2.420807, 0.153645]  # t=85
DESCEND_086_JOINTS = [0.055459, -0.509181, -0.909405, -2.471996, -0.308023, 2.404766, 0.138097]  # t=86
DESCEND_087_JOINTS = [0.058650, -0.500202, -0.902705, -2.461853, -0.292279, 2.383900, 0.118788]  # t=87
GRASP_HOLD_091_JOINTS = [0.073073, -0.474081, -0.901983, -2.406476, -0.183114, 2.289464, 0.002119]  # t=91
LIFT_092_JOINTS = [0.076166, -0.467396, -0.906191, -2.391128, -0.154420, 2.267242, -0.029474]  # t=92
LIFT_093_JOINTS = [0.078550, -0.460447, -0.909847, -2.377099, -0.129611, 2.245781, -0.056713]  # t=93
LIFT_094_JOINTS = [0.078740, -0.455933, -0.912818, -2.364725, -0.110467, 2.221003, -0.076366]  # t=94
LIFT_095_JOINTS = [0.075422, -0.457467, -0.915474, -2.353265, -0.097917, 2.190496, -0.090069]  # t=95
LIFT_096_JOINTS = [0.068916, -0.466149, -0.918563, -2.343778, -0.089519, 2.157471, -0.102097]  # t=96
LIFT_097_JOINTS = [0.059434, -0.482141, -0.923222, -2.337293, -0.084028, 2.123133, -0.114936]  # t=97
LIFT_098_JOINTS = [0.047437, -0.504737, -0.929540, -2.334247, -0.081032, 2.089104, -0.129082]  # t=98
LIFT_099_JOINTS = [0.034031, -0.532003, -0.937404, -2.334895, -0.079101, 2.059014, -0.144439]  # t=99
LIFT_100_JOINTS = [0.019795, -0.563137, -0.946734, -2.338507, -0.077367, 2.036618, -0.161714]  # t=100
LIFT_101_JOINTS = [0.003992, -0.599160, -0.956653, -2.344035, -0.076018, 2.022743, -0.177344]  # t=101
TRANSPORT_104_JOINTS = [-0.055328, -0.738918, -0.968052, -2.371095, -0.083836, 1.981762, -0.209918]  # t=104
TRANSPORT_105_JOINTS = [-0.076691, -0.791024, -0.961044, -2.382390, -0.088844, 1.970663, -0.212506]  # t=105
TRANSPORT_107_JOINTS = [-0.120081, -0.897713, -0.934661, -2.404905, -0.097383, 1.953087, -0.205485]  # t=107
TRANSPORT_109_JOINTS = [-0.163995, -1.005795, -0.900615, -2.427400, -0.104616, 1.938366, -0.196126]  # t=109
TRANSPORT_110_JOINTS = [-0.186845, -1.062199, -0.882262, -2.438775, -0.103412, 1.922639, -0.189781]  # t=110
TRANSPORT_113_JOINTS = [-0.248995, -1.226384, -0.823019, -2.470510, -0.079105, 1.874532, -0.159535]  # t=113
TRANSPORT_116_JOINTS = [-0.291067, -1.367720, -0.772369, -2.496516, -0.030418, 1.862909, -0.133904]  # t=116
TRANSPORT_117_JOINTS = [-0.300711, -1.408926, -0.757102, -2.502645, -0.011745, 1.864578, -0.126370]  # t=117
TRANSPORT_119_JOINTS = [-0.311830, -1.477744, -0.725199, -2.515034, 0.027659, 1.880874, -0.107087]  # t=119
TRANSPORT_121_JOINTS = [-0.311297, -1.527474, -0.689985, -2.524976, 0.071263, 1.915627, -0.085744]  # t=121
TRANSPORT_122_JOINTS = [-0.307254, -1.546848, -0.672740, -2.528787, 0.095442, 1.939271, -0.078030]  # t=122
TRANSPORT_125_JOINTS = [-0.284895, -1.584043, -0.622310, -2.532304, 0.163011, 2.021073, -0.060591]  # t=125
TRANSPORT_128_JOINTS = [-0.249536, -1.594784, -0.574661, -2.519843, 0.229235, 2.100614, -0.052036]  # t=128
TRANSPORT_129_JOINTS = [-0.233861, -1.588164, -0.558639, -2.513480, 0.250280, 2.128931, -0.051132]  # t=129
TRANSPORT_131_JOINTS = [-0.192072, -1.547969, -0.524062, -2.496514, 0.294496, 2.192367, -0.053823]  # t=131
TRANSPORT_133_JOINTS = [-0.141478, -1.486289, -0.493042, -2.463913, 0.344846, 2.245071, -0.059257]  # t=133
TRANSPORT_134_JOINTS = [-0.115267, -1.451665, -0.480698, -2.440628, 0.370109, 2.264129, -0.061831]  # t=134
TRANSPORT_137_JOINTS = [-0.037653, -1.339787, -0.454109, -2.348984, 0.443827, 2.295256, -0.066616]  # t=137
TRANSPORT_140_JOINTS = [0.028879, -1.233475, -0.434414, -2.236887, 0.501725, 2.289808, -0.069204]  # t=140
TRANSPORT_141_JOINTS = [0.045973, -1.204533, -0.429051, -2.197407, 0.513720, 2.278900, -0.067308]  # t=141
TRANSPORT_143_JOINTS = [0.074679, -1.147442, -0.418656, -2.116330, 0.530113, 2.256985, -0.055765]  # t=143
TRANSPORT_145_JOINTS = [0.101527, -1.076486, -0.412475, -2.037979, 0.543136, 2.250338, -0.044523]  # t=145
TRANSPORT_146_JOINTS = [0.114082, -1.036402, -0.412566, -2.004215, 0.545843, 2.250237, -0.043574]  # t=146
TRANSPORT_149_JOINTS = [0.144494, -0.919440, -0.417665, -1.916740, 0.536549, 2.240607, -0.057530]  # t=149
TRANSPORT_152_JOINTS = [0.161662, -0.829107, -0.421331, -1.845275, 0.509625, 2.210428, -0.086529]  # t=152
TRANSPORT_153_JOINTS = [0.163688, -0.807510, -0.419547, -1.824884, 0.496170, 2.197870, -0.094146]  # t=153
TRANSPORT_155_JOINTS = [0.163976, -0.771550, -0.410619, -1.789590, 0.465234, 2.174781, -0.103998]  # t=155
TRANSPORT_157_JOINTS = [0.162090, -0.735626, -0.397522, -1.758908, 0.432757, 2.159521, -0.107446]  # t=157
TRANSPORT_158_JOINTS = [0.160739, -0.717048, -0.390399, -1.744234, 0.416965, 2.153815, -0.107434]  # t=158
TRANSPORT_161_JOINTS = [0.154632, -0.667183, -0.368499, -1.702652, 0.374761, 2.136451, -0.102140]  # t=161
TRANSPORT_164_JOINTS = [0.143968, -0.636020, -0.347046, -1.672898, 0.337937, 2.106547, -0.081114]  # t=164
TRANSPORT_165_JOINTS = [0.139565, -0.629462, -0.340042, -1.667182, 0.326268, 2.093340, -0.070570]  # t=165
TRANSPORT_167_JOINTS = [0.130528, -0.619439, -0.326807, -1.660721, 0.306382, 2.063695, -0.045833]  # t=167
TRANSPORT_168_JOINTS = [0.125980, -0.616030, -0.320816, -1.659543, 0.297967, 2.046225, -0.032420]  # t=168
PLACE_170_JOINTS = [0.117166, -0.611776, -0.309391, -1.658625, 0.283165, 2.008533, -0.007749]  # t=170
PLACE_172_JOINTS = [0.109156, -0.607655, -0.298451, -1.657935, 0.270840, 1.970701, 0.014225]  # t=172
PLACE_174_JOINTS = [0.101949, -0.602376, -0.288315, -1.655563, 0.260157, 1.934354, 0.034508]  # t=174
PLACE_176_JOINTS = [0.095737, -0.594631, -0.279649, -1.647630, 0.251778, 1.904980, 0.051092]  # t=176
PLACE_178_JOINTS = [0.090724, -0.581360, -0.273835, -1.630392, 0.245260, 1.881690, 0.062391]  # t=178
PLACE_179_JOINTS = [0.088644, -0.572889, -0.272146, -1.619265, 0.242996, 1.870043, 0.066535]  # t=179
PLACE_180_JOINTS = [0.086801, -0.563615, -0.271086, -1.606822, 0.241424, 1.857864, 0.069979]  # t=180
PLACE_182_JOINTS = [0.083718, -0.542513, -0.270303, -1.578815, 0.239811, 1.832477, 0.075628]  # t=182
PLACE_184_JOINTS = [0.081166, -0.518287, -0.270648, -1.547242, 0.239307, 1.805117, 0.080511]  # t=184
PLACE_185_JOINTS = [0.080023, -0.505630, -0.271073, -1.530659, 0.239459, 1.791243, 0.082533]  # t=185
PLACE_186_JOINTS = [0.078988, -0.492816, -0.271430, -1.513888, 0.240097, 1.778448, 0.084013]  # t=186
PLACE_188_JOINTS = [0.077197, -0.467150, -0.271307, -1.482323, 0.243006, 1.758462, 0.085056]  # t=188
PLACE_190_JOINTS = [0.075496, -0.444519, -0.269413, -1.454881, 0.248002, 1.744511, 0.085615]  # t=190
PLACE_192_JOINTS = [0.072937, -0.431212, -0.266865, -1.431030, 0.251713, 1.725895, 0.088380]  # t=192
PLACE_194_JOINTS = [0.069706, -0.421935, -0.266788, -1.412957, 0.254599, 1.710229, 0.089216]  # t=194
PLACE_195_JOINTS = [0.067947, -0.419799, -0.267045, -1.405711, 0.256208, 1.702372, 0.088959]  # t=195
RELEASE_POSE_195_JOINTS = [0.067947, -0.419799, -0.267045, -1.405711, 0.256208, 1.702372, 0.088959]  # t=195
RETRACT_197_JOINTS = [0.063975, -0.423560, -0.267689, -1.395361, 0.258525, 1.687009, 0.088194]  # t=197
RETRACT_199_JOINTS = [0.059504, -0.437233, -0.267532, -1.391850, 0.258138, 1.676524, 0.088911]  # t=199
RETRACT_201_JOINTS = [0.054971, -0.458060, -0.265837, -1.395112, 0.254706, 1.670551, 0.092140]  # t=201
RETRACT_203_JOINTS = [0.050521, -0.487151, -0.262868, -1.404477, 0.248625, 1.665496, 0.097004]  # t=203
RETRACT_205_JOINTS = [0.046318, -0.524797, -0.258975, -1.418642, 0.241317, 1.662437, 0.102033]  # t=205
RETRACT_206_JOINTS = [0.044327, -0.546244, -0.256849, -1.428071, 0.237119, 1.661251, 0.104517]  # t=206
RETRACT_207_JOINTS = [0.042378, -0.569176, -0.254661, -1.439513, 0.232285, 1.659577, 0.107181]  # t=207
RETRACT_209_JOINTS = [0.038343, -0.620052, -0.250103, -1.467600, 0.219790, 1.652986, 0.114213]  # t=209
RETRACT_211_JOINTS = [0.033998, -0.675000, -0.244443, -1.499245, 0.202986, 1.643975, 0.126186]  # t=211
RETRACT_213_JOINTS = [0.029035, -0.734903, -0.236130, -1.535942, 0.182895, 1.624454, 0.146859]  # t=213
RETRACT_215_JOINTS = [0.023400, -0.800707, -0.223943, -1.576477, 0.164541, 1.600502, 0.178941]  # t=215
RETRACT_217_JOINTS = [0.017195, -0.869450, -0.209449, -1.619492, 0.149156, 1.579177, 0.220455]  # t=217
RETRACT_219_JOINTS = [0.010683, -0.939176, -0.191842, -1.661904, 0.135698, 1.558941, 0.265152]  # t=219
RETRACT_221_JOINTS = [0.004338, -1.007488, -0.173325, -1.700482, 0.125850, 1.535247, 0.312146]  # t=221
RETRACT_223_JOINTS = [-0.001818, -1.073378, -0.155413, -1.733419, 0.119053, 1.507436, 0.361768]  # t=223
RETRACT_225_JOINTS = [-0.007857, -1.136586, -0.137611, -1.760400, 0.113914, 1.474717, 0.413296]  # t=225
RETRACT_227_JOINTS = [-0.013842, -1.198185, -0.119748, -1.782903, 0.109478, 1.437671, 0.465432]  # t=227
RETRACT_229_JOINTS = [-0.019596, -1.259800, -0.102015, -1.803030, 0.106139, 1.398007, 0.515858]  # t=229
RETRACT_231_JOINTS = [-0.024901, -1.322707, -0.086016, -1.822166, 0.104744, 1.358957, 0.564220]  # t=231
APPROACH_000_JOINTS = [-0.031285, -1.079367, -0.012311, -2.286808, 0.023426, 1.552287, 0.694803]  # 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_004_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_006_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_008_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_012_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_016_JOINTS, input_type='joint', steps=4, 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_024_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_028_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_030_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_032_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_036_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_040_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_044_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_046_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_048_JOINTS, input_type='joint', steps=2, 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_068_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)
    moveJoint(APPROACH_072_JOINTS, input_type='joint', steps=4, settle_steps=0, gripper=-1.000)

def execute_grasp_approach():
    """Final Cartesian descent into grasp alignment."""
    moveJoint(DESCEND_073_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_074_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_075_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_076_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_077_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_078_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_079_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_080_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_081_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(DESCEND_082_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_083_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_084_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_085_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_086_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(DESCEND_087_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_091_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_091_JOINTS, input_type='joint', gripper=1.0)

def lift_object():
    """Lift the grasped object clear of the counter."""
    moveJoint(LIFT_092_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_093_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_094_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_095_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_096_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_097_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_098_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_099_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_100_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(LIFT_101_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_104_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_105_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_107_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_109_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_110_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_113_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_116_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_117_JOINTS, input_type='joint', steps=1, 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_122_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_125_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_128_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_129_JOINTS, input_type='joint', steps=1, 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_134_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_137_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_140_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_141_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_143_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_145_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_146_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_149_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_152_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_153_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_155_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_157_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_158_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_161_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_164_JOINTS, input_type='joint', steps=3, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_165_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_167_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=1.000)
    moveJoint(TRANSPORT_168_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_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_179_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=1.000)
    moveJoint(PLACE_180_JOINTS, input_type='joint', steps=1, 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_185_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(PLACE_186_JOINTS, input_type='joint', steps=1, 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_195_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    wait(duration=0.05, hold_target=RELEASE_POSE_195_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_195_JOINTS, input_type='joint', open_steps=1, settle_steps=0, gripper=-1.0)
    wait(duration=0.05, hold_target=RELEASE_POSE_195_JOINTS, input_type='joint', gripper=-1.0)

def retreat_after_release():
    """Retreat away from the placed object without clipping it."""
    moveJoint(RETRACT_197_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_199_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_201_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_203_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_205_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_206_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_207_JOINTS, input_type='joint', steps=1, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_209_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_211_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_213_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_215_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_217_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_219_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_221_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_223_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_225_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_227_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_229_JOINTS, input_type='joint', steps=2, settle_steps=0, gripper=-1.000)
    moveJoint(RETRACT_231_JOINTS, input_type='joint', steps=2, 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 cereal from the counter and place it in the cabinet.

all cheese dairy food in_container food_set1