Toolbox
toolbox.py
10 primitives called · 6887 lines
def find_source_tote(refresh: bool = False) -> dict | None:
"""Pose of the source tote, perceived from the wrist camera's depth; None if it has not
been seen yet."""
... # 10 lines
def scanned_objects() -> list[str]:
"""Names of the objects whose barcodes have been read so far in this rollout."""
... # 15 lines
def view_source_tote(arm: str = "left", steps: int | None = None) -> dict:
"""Park the hand where the wrist camera looks into the source tote."""
... # 42 lines
def find_objects(camera_name: str = "left_wrist_cam") -> list[dict]:
"""Every object in the source tote, segmented and measured from the wrist camera.
Shape-agnostic."""
if not self.has_depth(camera_name):
...
return []
pf = self.platform
rgb = np.ascontiguousarray(np.asarray(pf.camera_image(camera_name)))
...
depth = np.asarray(pf.depth_image(camera_name), dtype=np.float32)
intr = pf.camera_intrinsics(camera_name)
pose = self._camera_pose_opencv(camera_name)
...
cavity = self._source_cavity_xy()
floor_z = self._source_floor_z()
finder = self._wrist_finder()
sets, report = finder.object_point_sets(
rgb,
depth,
intr,
pose,
ee,
cavity_xy=cavity,
floor_z=floor_z,
...
)
fits = [
(self._fit_plan_shape(pts), pts, mid)
for pts, mid in zip(sets, report["kept_mask_ids"], strict=True)
]
fits = [f for f in fits if f[0] is not None]
fits.sort(key=lambda f: (0 if f[0]["graspable"] else 1, f[0]["centre_xy"][0]))
out: list[dict] = [f[0] for f in fits]
...
self.last_perception = report
...
return out
def wait(steps: int = 30) -> None:
"""Hold the current command for a number of env steps."""
... # 6 lines
def set_gripper(arm: str, open_: bool, hold_steps: int = 5) -> None:
"""Open or close the gripper on the named arm."""
... # 11 lines
def grasp_object(
arm: str,
obj: dict,
grasp_z: float | None = None,
seat_advance: float = 0.0,
pre_slew: bool = True,
vet: bool = True,
narrow_jaws: bool = False,
) -> dict:
"""Grasp one object from find_objects(); centre, jaw azimuth and grasp height all come
from its measurement."""
self._held_obj = dict(obj)
if self._grasp_attempts >= int(self._MAX_GRASP_ATTEMPTS):
...
return {
"ok": False,
"reason": "attempt_cap",
...
}
self._grasp_attempts += 1
...
top_z = float(obj["top_z"])
psi = float(obj["grasp_azimuth_rad"])
x, y = (float(v) for v in obj["centre_xy"])
psi = self._round_grasp_azimuth(obj, psi)
gz = (
float(grasp_z)
if grasp_z is not None
else max(
self._GRASP_Z_MIN,
min(self._GRASP_Z_MAX, 0.5 * (self._CAVITY_FLOOR_Z + top_z)),
)
)
gz -= float(self._GRASP_Z_BIAS_M)
quat = self.topdown_grasp_quat(psi)
...
pre_z = top_z + self._HAND_BELOW_SITE_M + self._PRE_GRASP_CLEAR_M
...
if vet:
b = self.plan_pose((x, y, gz), arm, target_quat=quat)
a = self.plan_pose((x, y, pre_z), arm, target_quat=quat)
if a["pos_err_m"] > self._HOVER_PLAN_LEAN_M:
...
for lean_deg in self._LEAN_DEGS:
lq = self._lean_toward_robot(self.topdown_grasp_quat(psi_l), lean_deg)
la = self.plan_pose((x, y, pre_z), arm, target_quat=lq)
lb = self.plan_pose((x, y, gz), arm, target_quat=lq)
if (
max(la["pos_err_m"], lb["pos_err_m"]) <= self._VET_POS_TOL_M
and la["pos_err_m"] < a["pos_err_m"]
):
quat, a, b, lean_used, psi = lq, la, lb, lean_deg, psi_l
...
break
...
if b["pos_err_m"] > self._VET_POS_TOL_M:
...
return {
"ok": False,
"reason": "unreachable",
...
}
...
r = self.grasp_topdown(
arm,
(x, y),
psi,
grasp_z=gz,
...
pre_z=pre_z,
pre_iters=strain_iters,
approach_quat=None if lean_used == 0.0 else quat,
object_geometry=obj,
)
if (not r["ok"]) and r["reason"] == "pre_grasp" and lean_used == 0.0:
...
r = self.grasp_topdown(
...
approach_quat=lq,
object_geometry=obj,
)
...
return r
def present_to_scanner(
arm: str,
max_steps: int = 60,
dither_steps: int = 12,
sweep_steps: int = 420,
restore_wrist: bool = True,
) -> bool:
"""Hold the grasped object at the scanner and search its facing until a new read latches."""
already_read = set(self.scanned_objects())
...
quat_entry = self.platform.ee_quat(arm)
tilted = False
flat = self._is_flat(self._held_obj)
...
def _beep() -> bool:
return bool(set(self.scanned_objects()) - already_read)
def _done(value: bool) -> bool:
...
if restore_wrist and (value or tilted):
if tilted:
self.set_gripper(arm, False, hold_steps=2)
self._tilt_wrist(arm, quat_entry)
if value and abs(float(self.platform.joint_pos(arm)[5]) - j6_entry) > 0.02:
self._ramp_joint(arm, 5, j6_entry, self._PRESENT_AZ_RAMP_STEPS)
...
return value
ox, oy, oz = self._READ_ORIGIN
read_point = np.array([ox - self._PRESENT_AXIS_DEPTH, oy, oz])
def _aim_payload(legs: int, iters: int) -> bool:
for _ in range(legs):
if _beep():
return True
pay = np.asarray(self._payload_point(arm), dtype=float)
ee = np.asarray(self.platform.ee_pos(arm), dtype=float)
goal = read_point + (ee - pay)
err = float(np.linalg.norm(goal - ee))
if err < self._PRESENT_AIM_TOL_M:
return False
...
self.move_ee_to(
(float(leg[0]), float(leg[1]), float(leg[2])),
arm,
max_iters=iters,
tol=self._PRESENT_AIM_TOL_M,
)
return _beep()
if _aim_payload(6, max(30, int(max_steps))):
...
return _done(True)
...
def _azimuth_sweep(budget: int) -> bool:
j6_0 = float(self.platform.joint_pos(arm)[5])
...
for deg in self._PRESENT_AZ_STEPS_DEG:
...
want = j6_0 + math.radians(deg)
...
self._ramp_joint(arm, 5, want, self._PRESENT_AZ_RAMP_STEPS)
self.platform.wait(3)
...
if _beep():
...
return True
return False
if flat:
...
else:
...
if _azimuth_sweep(int(sweep_steps) // 2):
...
return _done(True)
...
for tilt in self._PRESENT_TILT_DEG:
rq = self._axis_angle_quat((0.0, 1.0, 0.0), math.radians(tilt))
...
tilted = True
...
t_out = self._tilt_wrist(arm, dq)
...
if _beep():
...
return _done(True)
if t_out["ori_err_rad"] > 0.5:
...
continue
...
if _aim_payload(3, 60):
...
return _done(True)
...
if _azimuth_sweep(int(sweep_steps) // 3):
...
return _done(True)
...
return _done(_beep() or bool(already_read))
def regrasp_rotated(
arm: str,
delta_rad: float = math.pi / 2.0,
settle_steps: int = 40,
) -> dict:
"""Set the held object back down in the source tote, re-perceive it, and grasp it again at
a new azimuth."""
...
src = self._tote_centre("source")
put = None
objs: list[dict] = []
if (
self._is_flat(self._held_obj)
or (self._held_obj or {}).get("long_axis_rad") is not None
):
put, objs = self._flip_until_down(arm, src, settle_steps=int(settle_steps))
if put is not None and not put["ok"]:
...
put = None
if put is None:
put = self.place_in_tote(
arm, tote_xy=src, require_scan=False, return_over_source=False
)
out["put_down"] = put
if not put["ok"]:
out["reason"] = f"put_down_{put['reason']}"
...
return out
if not objs:
self.platform.wait(int(settle_steps))
...
ref = put.get("released_at") or self.platform.ee_pos(arm)
objs = self._look_for_dropped(arm, (ref[0], ref[1]))
out["found"] = len(objs)
if not objs:
out["reason"] = "not_found_after_put_down"
...
return out
ref = put.get("released_at") or self.platform.ee_pos(arm)
obj = min(objs, key=lambda o: math.dist(o["centre_xy"], (ref[0], ref[1])))
obj = dict(obj)
if obj.get("long_axis_rad") is None:
obj["grasp_azimuth_rad"] = (
float(obj["grasp_azimuth_rad"]) + float(delta_rad)
) % math.pi
...
self.set_gripper(arm, True, hold_steps=4)
g = self.grasp_object(arm, obj)
out["grasp"] = g
out["ok"] = bool(g["ok"])
out["reason"] = "held" if g["ok"] else f"grasp_{g['reason']}"
...
return out
def place_in_tote(
arm: str,
tote_xy=None,
require_scan: bool = True,
scanned_before=None,
release_z: float | None = None,
carry_z: float | None = None,
return_over_source: bool = True,
) -> dict:
"""Carry the held object over a tote and release it inside the cavity."""
...
scanned_new = None
if require_scan:
before = set() if scanned_before is None else set(scanned_before)
scanned_new = sorted(set(self.scanned_objects()) - before)
if not scanned_new:
self._log(
"place_in_tote: REFUSING to release — no NEW scan since the grasp "
"(pass require_scan=False only for a stow-without-scan task)"
)
return {
"ok": False,
"reason": "unscanned",
...
}
...
here = self.platform.ee_pos(arm)
inside = (
abs(float(here[0]) - cavity_centre[0]) <= hx
and abs(float(here[1]) - cavity_centre[1]) <= hy
)
if not inside:
...
return {
"ok": False,
"reason": "carry_short",
...
}
...
The ten primitives this CaP calls, with the five that do the work cut to their key lines. Grasps are vetted; the stow refuses unscanned objects.
CaP script
agent_script.py
41 of 79 lines
ARM = "left"
NEEDED = 2
MAX_PASSES = 8
stowed = 0
...
for _pass in range(MAX_PASSES):
if stowed >= NEEDED:
break
objs = tb.find_objects()
if not objs:
tb.view_source_tote(ARM)
tb.wait(5)
objs = tb.find_objects()
if not objs:
break
obj = objs[0]
tb.set_gripper(ARM, True, hold_steps=4)
g = tb.grasp_object(ARM, obj)
if g["reason"] == "attempt_cap":
break
if not g["ok"]:
...
continue
before = set(tb.scanned_objects())
tb.present_to_scanner(ARM)
if not new_reads(before):
r = tb.regrasp_rotated(ARM)
...
tb.present_to_scanner(ARM)
if not new_reads(before):
tb.place_in_tote(ARM, tote_xy=source_xy(), require_scan=False)
continue
p = tb.place_in_tote(ARM, scanned_before=before)
if p["ok"]:
stowed += 1
Find, grasp, present to the scanner; a read counts once the scanned-code set grows. On a miss, regrasp rotated; if it still fails, return it to the source tote.