CaP-X × FurnitureBench v5 — 실물-충실(real-faithful) API로 gpt-6-astra 재평가

[{'role': 'system', 'content': 'You are a helpful assistant that generates Python code to directly solve the task.'}, {'role': 'user', 'content': [{'type': 'text', 'text': '\nYou are controlling a Franka Emika Panda robot to assemble furniture: attach square_table_leg4 to\nsquare_table_top. The leg is threaded and screws into any one of the four holes on the tabletop.\n\nConventions:\n- All positions are XYZ metres in the ROBOT BASE frame. All quaternions are WXYZ.\n- The gripper points straight down at the table when its quaternion is (0, 1, 0, 0).\n- The table surface is at approximately z = 0. The parts start lying flat on it.\n- get_object_pose returns the pose of a part\'s geometric centre; with return_bbox_extent=True it\n also returns the part\'s size along the part\'s own axes.\n- get_holes(part) returns where each hole opens on the part\'s surface and the direction a leg\n screwed into it points.\n\nPhysical facts about the task:\n- The leg is threaded: it does not go in by being pushed. It has to stand along the hole axis with\n its threaded end at the hole and be turned about its own long axis while pressed against the\n hole. It is assembled once it is screwed all the way down, with its axes parallel to the\n tabletop\'s.\n- A part can slip in the fingers when it is moved or turned, so verify where things are with\n get_object_pose instead of assuming.\n- The wrist cannot turn indefinitely. To keep turning a part, release it, turn the empty gripper\n back, and grip it again.\n- Motions are blocking and the episode has a limited step budget.\n\nThe functions (APIs) below are already imported into the environment. If you want to use numpy,\nyou must import it explicitly.\nYou may write python comments for reasoning but write ONLY executable Python code, with no code fences.\n\nAPIs:\n\nget_object_pose(object_name: \'str\', return_bbox_extent: \'bool\' = False)\n Doc:\n Get the pose of a furniture part.\n \n Args:\n object_name: Part name, e.g. "square_table_top" or "square_table_leg4".\n return_bbox_extent: If True, also return the part\'s size.\n \n Returns:\n position: (3,) XYZ in metres, robot base frame -- the centre of the part\'s bounding box.\n quaternion_wxyz: (4,) WXYZ unit quaternion -- the orientation of the part\'s own axes.\n bbox_extent: only when return_bbox_extent is True -- (3,) full side lengths in metres\n along the part\'s OWN x, y, z axes (not the world axes), so a square table leg\n reads as (0.030, 0.0875, 0.030): 8.75 cm long along its own y axis with a 3 cm\n square cross-section. The part spans +/- half of each extent about the returned\n position, along the corresponding axis of the returned orientation.\n\nlist_parts() -> \'list[str]\'\n Doc:\n List the furniture parts present in the scene.\n \n Returns:\n List of part names accepted by get_object_pose / sample_grasp_pose / get_holes.\n\nget_holes(part_name: \'str\') -> \'list[tuple[np.ndarray, np.ndarray]]\'\n Doc:\n Get the holes on a part that other parts screw into.\n \n Design information (from the furniture\'s CAD): where each hole opens on the part\'s surface\n and the direction a part screwed into it extends away from the surface. Nothing about how\n deep the hole is or where a mounted part ends up -- that follows from the mounted part\'s\n own length.\n \n Args:\n part_name: The part with the holes, e.g. "square_table_top".\n \n Returns:\n List of (opening_position, axis): opening_position (3,) XYZ in metres, robot base\n frame, at the centre of the hole on the surface; axis (3,) unit vector in the robot\n base frame pointing out of the hole, i.e. along a seated leg from its threaded end\n toward its free end.\n\nsample_grasp_pose(object_name: \'str\') -> \'tuple[np.ndarray, np.ndarray]\'\n Doc:\n Get a top-down grasp pose for a furniture part.\n \n The gripper points down at the table with its fingers across the part\'s longest axis, at\n the part\'s centre (never below finger clearance above the table). The mirrored grasp --\n the same pose rotated 180 degrees about the gripper\'s approach axis -- is equally valid.\n \n Args:\n object_name: Part name.\n \n Returns:\n position: (3,) XYZ in metres, robot base frame, to pass straight to goto_pose.\n quaternion_wxyz: (4,) WXYZ unit quaternion for the gripper.\n\nget_ee_pose() -> \'tuple[np.ndarray, np.ndarray]\'\n Doc:\n Get the current end-effector pose (the point between the fingertips).\n \n Returns:\n position: (3,) XYZ in metres, robot base frame.\n quaternion_wxyz: (4,) WXYZ unit quaternion.\n\nget_joint_positions() -> \'np.ndarray\'\n Doc:\n Get the arm\'s current joint positions.\n \n Returns:\n (7,) joint angles in radians, joint 1 (base) to joint 7 (wrist). Joint limits are\n +/-2.90 for joints 1, 3, 5, 7; +/-1.76 for joint 2; [-3.07, -0.07] for joint 4;\n [-0.02, 3.75] for joint 6.\n\nis_reachable(position: \'np.ndarray\', quaternion_wxyz: \'np.ndarray\') -> \'bool\'\n Doc:\n Check whether the arm can place the end-effector at a pose.\n \n Solves inverse kinematics and verifies the solution actually reaches the pose within the\n arm\'s joint limits. Useful before committing to a grasp or a placement: a pose the arm\n cannot reach makes goto_pose stop short of it.\n \n Args:\n position: (3,) XYZ in metres, robot base frame.\n quaternion_wxyz: (4,) WXYZ unit quaternion.\n \n Returns:\n True if the pose is reachable.\n\ngoto_pose(position: \'np.ndarray\', quaternion_wxyz: \'np.ndarray\', z_approach: \'float\' = 0.0) -> \'None\'\n Doc:\n Move the end-effector to a pose using inverse kinematics. Blocking.\n \n Args:\n position: (3,) XYZ in metres, robot base frame.\n quaternion_wxyz: (4,) WXYZ unit quaternion.\n z_approach: If non-zero, first move to `z_approach` metres back along the gripper\'s\n approach axis (i.e. above the target for a downward-pointing gripper), then move\n to the target. Useful for grasps and insertions. There is no need to call\n goto_pose a second time with the same target afterwards.\n\nopen_gripper() -> \'None\'\n Doc:\n Open the gripper. Blocking.\n\nclose_gripper() -> \'None\'\n Doc:\n Close the gripper to grasp. Blocking.\n\nhome_pose() -> \'None\'\n Doc:\n Move the arm back to its home joint configuration. Blocking.'}]}]

# Code block 0 import numpy as np LEG = "square_table_leg4" TOP = "square_table_top" def unit(v): v = np.asarray(v, dtype=float) return v / np.linalg.norm(v) def qmat(q): w, x, y, z = unit(q) return np.array([ [1 - 2 * (y*y + z*z), 2 * (x*y - w*z), 2 * (x*z + w*y)], [2 * (x*y + w*z), 1 - 2 * (x*x + z*z), 2 * (y*z - w*x)], [2 * (x*z - w*y), 2 * (y*z + w*x), 1 - 2 * (x*x + y*y)] ]) def matq(R): # Eigenvector conversion remains stable for half-turns. K = np.array([ [R[0, 0] - R[1, 1] - R[2, 2], R[0, 1] + R[1, 0], R[0, 2] + R[2, 0], R[2, 1] - R[1, 2]], [R[0, 1] + R[1, 0], R[1, 1] - R[0, 0] - R[2, 2], R[1, 2] + R[2, 1], R[0, 2] - R[2, 0]], [R[0, 2] + R[2, 0], R[1, 2] + R[2, 1], R[2, 2] - R[0, 0] - R[1, 1], R[1, 0] - R[0, 1]], [R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1], np.trace(R)] ]) / 3.0 _, V = np.linalg.eigh(K) x, y, z, w = V[:, -1] return unit([w, x, y, z]) def rotation(axis, angle): x, y, z = unit(axis) K = np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]) return np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K) def pose(name): p, q = get_object_pose(name) return np.asarray(p, dtype=float), qmat(q) def move_part(center, orientation): # Measure the grasp transform again: the part may have slipped. p, R = pose(LEG) e, eq = get_ee_pose() e = np.asarray(e, dtype=float) E = qmat(eq) local_offset = R.T @ (e - p) relative_rotation = R.T @ E target_e = center + orientation @ local_offset target_q = matq(orientation @ relative_rotation) if not is_reachable(target_e, target_q): return False goto_pose(target_e, target_q) return True def mounted_frame(table_R, axis, long_axis): # Match the tabletop axes, with the leg's positive long axis pointing out. other = (long_axis + 1) % 3 remaining = (long_axis + 2) % 3 v = table_R[:, other] v = v - axis * np.dot(v, axis) if np.linalg.norm(v) < 1e-6: v = table_R[:, remaining] v = v - axis * np.dot(v, axis) R = np.zeros((3, 3)) R[:, long_axis] = axis R[:, other] = unit(v) R[:, remaining] = np.cross(R[:, long_axis], R[:, other]) return R def main(): p0, q0, extent = get_object_pose(LEG, return_bbox_extent=True) p0 = np.asarray(p0, dtype=float) R0 = qmat(q0) extent = np.asarray(extent, dtype=float) long_axis = int(np.argmax(extent)) half_length = extent[long_axis] / 2 _, table_R = pose(TOP) holes = get_holes(TOP) gp, gq = sample_grasp_pose(LEG) gp = np.asarray(gp, dtype=float) G = qmat(gq) candidates = [] for hole_index, (opening, direction) in enumerate(holes): h, a = np.asarray(opening, dtype=float), unit(direction) if a[2] < 0.5: continue target_R = mounted_frame(table_R, a, long_axis) target_center = h + a * (half_length + 0.003) for mirror in (0.0, np.pi): grasp_R = G @ rotation([0, 0, 1], mirror) local_offset = R0.T @ (gp - p0) relative_R = R0.T @ grasp_R ee_target = target_center + target_R @ local_offset ee_R = target_R @ relative_R twist_R = rotation(a, -np.pi / 2) @ target_R twist_e = h + a * half_length + twist_R @ local_offset if (is_reachable(gp, matq(grasp_R)) and is_reachable(ee_target + a * 0.06, matq(ee_R)) and is_reachable(ee_target, matq(ee_R)) and is_reachable(twist_e, matq(twist_R @ relative_R))): score = np.linalg.norm(ee_target - gp) candidates.append((score, hole_index, grasp_R, target_R)) if not candidates: raise RuntimeError("No reachable grasp and upright threading approach found.") _, hole_index, grasp_R, target_R = min(candidates, key=lambda c: c[0]) open_gripper() goto_pose(gp, matq(grasp_R), z_approach=0.07) close_gripper() e, eq = get_ee_pose() goto_pose(np.asarray(e) + np.array([0.0, 0.0, 0.15]), eq) lifted_p, _ = pose(LEG) if lifted_p[2] < p0[2] + 0.04: raise RuntimeError("The leg did not follow the gripper during pickup.") h, a = get_holes(TOP)[hole_index] h, a = np.asarray(h, dtype=float), unit(a) _, table_R = pose(TOP) target_R = mounted_frame(table_R, a, long_axis) if not move_part(h + a * (half_length + 0.065), target_R): raise RuntimeError("Cannot reach the upright pre-insertion pose.") if not move_part(h + a * (half_length + 0.001), target_R): raise RuntimeError("Cannot align the threaded end with the hole.") p, R = pose(LEG) end = p - R[:, long_axis] * half_length lateral = end - h - a * np.dot(end - h, a) if np.linalg.norm(lateral) > 0.004 or np.dot(R[:, long_axis], a) < 0.98: if not move_part(h + a * (half_length + 0.001), target_R): raise RuntimeError("Thread alignment could not be corrected.") # Engage the thread before releasing the sideways pickup grasp. engaged_R = rotation(a, -np.pi / 2) @ target_R if not move_part(h + a * (half_length - 0.0015), engaged_R): raise RuntimeError("The initial threading turn is unreachable.") # Regrip near the free end with the approach along the leg. # Subsequent turns are wrist rotations rather than an orbiting side grasp. open_gripper() e, eq = get_ee_pose() E = qmat(eq) retreat = np.asarray(e) - E[:, 2] * 0.055 if is_reachable(retreat, eq): goto_pose(retreat, eq) p, R = pose(LEG) grip_offset = max(0.0, half_length - 0.022) grip_center = p + a * grip_offset down_R = np.column_stack((target_R[:, (long_axis + 1) % 3], np.zeros(3), -a)) down_R[:, 0] = unit(down_R[:, 0]) down_R[:, 1] = np.cross(down_R[:, 2], down_R[:, 0]) choices = [] _, current_q = get_ee_pose() for yaw in np.arange(8) * np.pi / 4: start_R = rotation(a, yaw) @ down_R finish_R = rotation(a, -np.pi / 2) @ start_R if (is_reachable(grip_center, matq(start_R)) and is_reachable(grip_center - a * 0.002, matq(finish_R)) and is_reachable(grip_center + a * 0.07, matq(start_R))): score = abs(np.dot(matq(start_R), unit(current_q))) choices.append((score, start_R)) if not choices: raise RuntimeError("No reachable axial ratcheting grasp found.") base_R = max(choices, key=lambda c: c[0])[1] goto_pose(grip_center, matq(base_R), z_approach=0.07) close_gripper() total_turn = np.pi / 2 recent_heights = [] verified = False for turn in range(31): h, a = get_holes(TOP)[hole_index] h, a = np.asarray(h, dtype=float), unit(a) p, R = pose(LEG) if np.dot(R[:, long_axis], a) < 0.97: raise RuntimeError("The leg lost axial alignment during threading.") # Correct lateral slip and maintain a small axial preload while turning. height = np.dot(p - h, a) lateral = p - h - height * a desired_center = p - lateral - a * 0.0015 desired_center += a * max( 0.0, half_length - 0.003 - np.dot(desired_center - h, a) ) desired_R = rotation(a, -np.pi / 2) @ R if abs(get_joint_positions()[6]) > 2.72: break if not move_part(desired_center, desired_R): break total_turn += np.pi / 2 p_after, R_after = pose(LEG) h_now, a_now = get_holes(TOP)[hole_index] h_now, a_now = np.asarray(h_now), unit(a_now) axial_height = np.dot(p_after - h_now, a_now) lateral_error = np.linalg.norm( p_after - h_now - axial_height * a_now ) recent_heights.append(axial_height) _, current_table_R = pose(TOP) axes_parallel = np.min( np.abs(np.diag(current_table_R.T @ R_after)) ) > 0.985 settled = (len(recent_heights) >= 4 and np.ptp(recent_heights[-4:]) < 0.0008) if (total_turn >= 4 * np.pi and abs(axial_height - half_length) < 0.003 and lateral_error < 0.003 and axes_parallel and settled): verified = True break # Release before unwinding: never reverse the engaged leg. open_gripper() e, eq = get_ee_pose() clearance = np.asarray(e) + a_now * 0.075 goto_pose(clearance, eq) if not is_reachable(clearance, matq(base_R)): home_pose() else: goto_pose(clearance, matq(base_R)) p, R = pose(LEG) grip_center = p + a_now * grip_offset if not is_reachable(grip_center, matq(base_R)): raise RuntimeError("The leg slipped outside the regrasp workspace.") goto_pose(grip_center, matq(base_R), z_approach=0.055) close_gripper() if verified: open_gripper() e, eq = get_ee_pose() goto_pose(np.asarray(e) + a * 0.08, eq) final_p, final_R = pose(LEG) final_h, final_a = get_holes(TOP)[hole_index] final_a = unit(final_a) final_delta = final_p - np.asarray(final_h) if (np.linalg.norm(final_delta - final_a * half_length) > 0.005 or np.dot(final_R[:, long_axis], final_a) < 0.98): raise RuntimeError("Final verification failed after releasing the leg.") else: raise RuntimeError("Thread seating was not verified within the motion budget.") main()