File size: 2,110 Bytes
7399b6f | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 | """Task-space path planning: a thin wrapper over the PRM in scripts/yam_prm.py.
Kept separate from the arm controller so a task can swap in a different planner (straight line,
cuRobo, a learned policy) without touching the tracking/grasp code.
"""
from __future__ import annotations
import numpy as np
# planning-only blockers behind the robot: never rendered, only used to keep the planned
# end-effector path from sweeping backwards through the arm's own base
DEFAULT_WALLS = [
(np.array([-0.38, -0.15, 0.70]), np.array([0.02, 0.45, 0.28])),
(np.array([-0.10, -0.45, 0.70]), np.array([0.45, 0.02, 0.28])),
]
def _prm_module():
import importlib, os, sys
scripts = os.path.join(os.path.dirname(os.path.dirname(os.path.dirname(
os.path.dirname(os.path.abspath(__file__))))), "scripts")
if scripts not in sys.path:
sys.path.insert(0, scripts)
return importlib.import_module("yam_prm")
def plan_path(start, goal, arm_root, walls=None, clearance=0.05, samples=300, resample=12):
"""Collision-free end-effector polyline from `start` to `goal`, both in the arm's root frame.
Falls back to a straight line if the roadmap finds nothing, so a task never dies here -- a
blocked path shows up as a tracking error the task can see, not an exception.
"""
yam_prm = _prm_module()
obstacles = [yam_prm.Box(center=(c-np.asarray(arm_root, np.float64)), half=h)
for c, h in (walls if walls is not None else DEFAULT_WALLS)]
prm = yam_prm.PRM(bounds_lo=np.array([-0.05, -0.35, -0.03]),
bounds_hi=np.array([0.55, 0.45, 0.40]),
obstacles=obstacles, clearance=clearance, num_samples=samples, k=12, seed=1)
path = prm.plan(np.asarray(start, np.float64), np.asarray(goal, np.float64))
if path is None:
return [np.asarray(goal, np.float32)]
short = yam_prm.shortcut(path, obstacles, clearance, iters=200, seed=2)
dense = yam_prm.resample_polyline(short, resample).astype(np.float32)
return list(dense[1:]) # drop the start point; the executor begins where it is
|