Paths, lids, and two arms
Plan several waypoints as one move, follow a lid or door along its arc, run both arms at once, and pin a move to one joint configuration branch.
Paths
move_path(arm, steps, *, speed=None, stop=True)Plans a list of waypoints as one move: the whole path is checked before the arm moves, and each waypoint is reached from the one before. Each step is a Step (or a bare pose, which becomes a free step):
Step(target, *, straight_line=False, keep_upright=False, keep_vertical=None, vertical_deg=5.0, tolerance=None,
constraints=(), allow=(), scene=None, speed=None, tcp_speed=None, label="", stop=True)The fields mean what they mean on move_arm. Two are specific to paths:
scene: the joint values of objects predicted at this step, as{"<object>/<joint>": value}or{(object, joint): value}. The planner checks each step against the object as it will be then (a lid half open), and the last step's values are written to the world model when the path ends.stop=Falseon an intermediate step: the arm flows through it instead of arriving.
A straight step is a line from the previous step's target (from the tool for the first step).
Following a lid or a door
An anchor on a moving part moves with it. Evaluating the anchor at a series of joint angles gives the arc the tool must follow. Two helpers build those steps:
sweep_anchor_steps(obj, anchor_name, joint, start_angle, end_angle, steps, tcp_speed, end_dz=0.0, tolerance=None)
interp_anchor_steps(obj, a0, a1, joints, steps, tcp_speed, start_dz=0.0)sweep_anchor_stepsreturnsstepsstraight steps following anchoranchor_nameasjointgoes fromstart_angletoend_angle(rad), each step predicting that angle.end_dzraises the path gradually by that many metres along world z by the last step.tolerance=FREE_YAWleaves the turn about the tool free on every step, for a push on a panel.interp_anchor_stepsreturnsstepsstraight steps from anchora0to anchora1of the same object (with its joints atjoints), for moving the grip from one anchor to the next.start_dzraises the start along world z.
The starter's shaker_lid skill opens and closes a hinged lid this way:
start, end = get_articulation(shaker_id)["lid_joint"], LID_ANGLE[lid]
grip = anchor(shaker_id, "lid_grip") # where the lip is now
set_gripper(ARM, OPEN_M)
move_arm(ARM, grip.preapproach()) # free move to the lip's standoff
with contact(ARM, shaker_id): # the fingers may touch the shaker from here
move_arm(ARM, grip, straight_line=True, tcp_speed=SLOW)
set_gripper(ARM, grip.width)
move_path(ARM, sweep_anchor_steps(shaker_id, "lid_grip", "lid_joint", start, end, ARC_STEPS, SWING))
set_gripper(ARM, OPEN_M)
away = anchor(shaker_id, "lid_grip").preapproach() # the lip where the lid is now
move_arm(ARM, away, straight_line=True, tcp_speed=SLOW, allow=contact(ARM, shaker_id))The world model's lid angle follows the path, so no set_articulation call is needed afterwards.
Two arms at once
results = run_parallel({"left": open_lid, "right": fetch_plate}, timeout=600.0)Runs one zero-argument function per arm at the same time. Moves of the two arms that overlap in time are planned together, each arm checked against the other's planned motion. Returns {arm: return value}. If a leg fails, the first failure is raised once both legs have ended.
- Inside a leg, a function that names the other arm raises
ArmNotInLeg. - On a setup with one arm, two legs raise
DualArmRequired. - Outside
run_parallel, a move of one arm holds the other still.
def open_lid():
move_arm_js("left", LEFT_READY)
def fetch_plate():
move_arm("right", anchor(plate, "grasp_short").preapproach(0.10))
run_parallel({"left": open_lid, "right": fetch_plate})Choosing the joint configuration branch
A pose goal can usually be reached in several arm configurations (elbow up or down, wrist turned one way or the other). By default the planner searches from the arm's current configuration outwards. When a sequence of moves must stay on one branch, pass posture= on the free moves that should resolve on it:
POSTURE = {"q": [-0.218, -0.663, -0.989, -0.031, 1.682, 4.491], # a seed: the starter's right-arm HOME
"prefer": {"joint5": "positive"},
"off_branch": None}
move_arm(ARM, target, posture=POSTURE)qseeds the search with six joint values (rad).preferdefines the branch: per joint, a sign ("negative"or"positive") or a range[lo, hi]in radians (Nonefor an open end).- When no configuration on the branch passes the checks, the move falls back to the default search and logs which rule it broke; a move carrying a held object that would change branch is refused instead.
"off_branch": "log"or"refuse"sets either behaviour explicitly.
posture= applies to free move_arm and move_relative calls. Straight lines keep the branch they start on; keep_upright, via and move_path take no posture. Keep the posture as a constant in the skill and pass it on each move that needs it; there is no project-wide setting.
Updated 1 day ago