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=False on 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_steps returns steps straight steps following anchor anchor_name as joint goes from start_angle to end_angle (rad), each step predicting that angle. end_dz raises the path gradually by that many metres along world z by the last step. tolerance=FREE_YAW leaves the turn about the tool free on every step, for a push on a panel.
  • interp_anchor_steps returns steps straight steps from anchor a0 to anchor a1 of the same object (with its joints at joints), for moving the grip from one anchor to the next. start_dz raises 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)
  • q seeds the search with six joint values (rad).
  • prefer defines the branch: per joint, a sign ("negative" or "positive") or a range [lo, hi] in radians (None for 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.


Did this page help you?