Skip to content

Python constraints

Seven constraints written in Python, switched on one after another in a live loop.

python_constraints_example, tesseract_robotics 0.35.0.9.dev692

An ABB IRB 2400 rides a rail, with a grinder on its flange. The tool tip rests on a mark on a workpiece. One TrajOpt problem is solved per frame: a single waypoint, warm-started from the frame before, with a squared stay-close cost and whichever rules of trajopt_ifopt.python_constraints are on. Each rule is a few lines of Python; the SQP that satisfies them is TrajOpt's, in C++.

What you are watching

Act Rules on What happens
1 none The rail moves and the tool goes with it.
2 WorldHold The tip stays on the mark while the rail moves. The tool axis wanders.
3 + HoldAxis 0.5° The tool axis locks onto vertical.
4 + WatchTheTool 3° A camera on a mast on the carriage turns to keep the tip in its cone. It stays on from here.
5 PointsDown 20° + ElbowUp The rail becomes the solver's seventh axis. A plane rises under the elbow, and the carriage drives to where the elbow clears it.
6 + StayDexterous Manipulability stays above a rising bound.
7 + Clearance A ball drives in. The forearm keeps 15 cm from it, and the tip stays put.
8 all seven The mark moves, and the tip traces a circle on the workpiece.

In acts 1 to 4 something else moves the rail: its position is measured, and the arm and the camera solve in the carriage's frame. From act 5 the rail is a variable like any joint.

Every bound that moves ramps from the live value, so nothing jumps when a rule comes on.

Reading the scene

  • The panel, on the plane behind the robot, has a ruler per rule. The ball on a ruler is the live value. The bar is the bound, and it is there only while the rule is on.
  • The last ruler is the solve. Its ball is what the frame's solve took, and its bar is the frame the loop has at 30 Hz, 33 ms.
  • The glyphs show each rule in the cell: the mark, the HoldAxis line, the PointsDown cone, the ElbowUp plane, the camera's cone and optical axis, and the shell around the ball that the forearm stays out of.
  • The caption names the act, and says how many rules are on and what a frame's solve took, as the act's median.

The captions, the gauges' markers, the mark and the glyphs are links of the environment on prismatic joints, posed by the same trajectory as the robot. That is how a recording, which can animate link poses and nothing else, shows a bound appear, move and disappear.

The rules of one frame

python_constraints_example.py (rules)
def rules(
    inputs: Inputs, var: ti.Var, group: JointGroup, robot: Robot, base_pose: Isometry3d
) -> list[PyConstraint]:
    """The constraint sets of one frame: the rules `inputs` has on, at its bounds."""
    sets: list[PyConstraint] = []
    if inputs.hold:
        sets.append(WorldHold(var, group, TCP, inputs.mark, base_pose, tol=0.0, coeff=RULE_COEFF))
    if inputs.axis_deg is not None:
        sets.append(HoldAxis(var, group, TCP, DOWN, np.radians(inputs.axis_deg), coeff=RULE_COEFF))
    if inputs.cone_deg is not None:
        sets.append(PointsDown(var, group, TCP, np.radians(inputs.cone_deg), coeff=RULE_COEFF))
    if inputs.elbow_m is not None:
        sets.append(ElbowUp(var, group, ELBOW, WORLD, inputs.elbow_m, coeff=RULE_COEFF))
    if inputs.view_deg is not None:
        sets.append(
            WatchTheTool(var, group, CAMERA, TCP, np.radians(inputs.view_deg), coeff=RULE_COEFF)
        )
    if inputs.dexterity is not None:
        sets.append(StayDexterous(var, group, TCP, inputs.dexterity, coeff=RULE_COEFF))
    if inputs.clearance:
        sets.append(
            Clearance(
                var,
                robot.env,
                [joint_id.name() for joint_id in group.getJointIds()],
                FOREARM,
                CLEARANCE_MIN_M,
                CLEARANCE_MAX_M,
                margin=CLEARANCE_MARGIN_M,
                coeff=RULE_COEFF,
            )
        )
    if inputs.follow_m is not None and inputs.rail_m is None:
        sets.append(CarriageFollows(var, group, TCP, inputs.follow_m, coeff=RULE_COEFF))
    return sets

The loop

LiveLoop.step solves one frame. run plays the acts through it.

python_constraints_example.py (loop)
class LiveLoop:
    """The cell and its state between frames: `step` solves one frame from the last."""

    def __init__(self) -> None:
        self.robot = build_cell()
        env = self.robot.env
        arm_joints = [joint_id.name() for joint_id in env.getGroupJointIds("manipulator")]
        # A joint group keeps the other joints where they are when it is made. With the rail
        # at zero, the first group works in the carriage's frame: the rail is someone else's.
        env.setState({RAIL_JOINT: 0.0, OBSTACLE_JOINT: BALL_PARKED_M})
        self._arm_names = [*arm_joints, *CAMERA_JOINTS]
        self._arm = env.getJointGroup("arm_and_camera", self._arm_names)
        # The second has the rail too: the solver's seventh axis.
        self._cell_names = [RAIL_JOINT, *self._arm_names]
        self._cell = env.getJointGroup("rail_arm_and_camera", self._cell_names)
        self._arm_limits = ti.toBounds(self._arm.getLimits().joint_limits)
        self._cell_limits = ti.toBounds(self._cell.getLimits().joint_limits)
        # Where the tip rests at the start [m]: the mark, until an act moves it.
        start = np.array([*START, 0.0, 0.0])
        self.target = np.array(self._arm.calcFwdKin(start)[TCP].translation)
        self.joint_names = [*self._cell_names, OBSTACLE_JOINT, *INSTRUMENT_JOINTS]

        # Measures the forearm's clearance for the gauge, whether or not the rule is on.
        self._probe_nodes = ti.createNodesVariables(
            "probe", self._cell_names, [np.zeros(len(self._cell_names))], self._cell_limits
        )
        self._probe = Clearance(
            self._probe_nodes.getNodes()[0].getVar("joints"),
            env,
            self._cell_names,
            FOREARM,
            0.0,
            CLEARANCE_MAX_M,
            margin=CLEARANCE_MARGIN_M,
        )
        self.q = np.array([0.0, *start])  # the rail, the arm, the camera's pan and tilt
        self.live = self._measure(self.target, 0.0)

    def _measure(self, mark: np.ndarray, solve_ms: float) -> dict[str, float]:
        measured = measure(self._cell, self.q, mark, float(self._probe.f(self.q)[0]))
        return {**measured, "solve_ms": solve_ms}

    def step(self, inputs: Inputs) -> Frame:
        """Solve one frame: a single waypoint, warm-started from the frame before."""
        env = self.robot.env
        if inputs.rail_m is not None:
            # The rail is measured, not commanded: the arm solves in its own base frame.
            env.setState({RAIL_JOINT: inputs.rail_m, OBSTACLE_JOINT: inputs.ball_m})
            group, names, limits = self._arm, self._arm_names, self._arm_limits
            seed = self.q[1:]
            base_pose = _isometry((0.0, inputs.rail_m, 0.0))
        else:
            env.setState({OBSTACLE_JOINT: inputs.ball_m})
            group, names, limits = self._cell, self._cell_names, self._cell_limits
            seed = self.q
            base_pose = Isometry3d.Identity()

        nodes = ti.createNodesVariables("frame", names, [seed], limits)
        var = nodes.getNodes()[0].getVar("joints")
        problem = tsqp.TrajOptQPProblem(nodes)
        problem.addCostSet(
            ti.JointPosConstraint(seed, var, np.ones(len(seed)), "stay_close"),
            tsqp.CostPenaltyType.SQUARED,
        )
        sets = rules(inputs, var, group, self.robot, base_pose)
        for rule in sets:
            problem.addConstraintSet(rule)
        problem.setup()
        started = time.perf_counter()
        if sets:
            tsqp.TrustRegionSQPSolver(tsqp.OSQPEigenSolver()).solve(problem)
        solve_ms = (time.perf_counter() - started) * MS_PER_S

        solved = np.asarray(var.value())
        self.q = solved.copy() if inputs.rail_m is None else np.array([inputs.rail_m, *solved])
        self.live = self._measure(inputs.mark, solve_ms)
        return Frame(self.q, self.live, solve_ms, _violation(sets))


def run(acts: tuple[Act, ...] = ACTS) -> dict:
    """Play the acts: one solve per frame.

    Returns:
        `robot`, the recorded `joint_names`, an `ActResult` per act in `acts`, the `target` the
        tip holds [m], and `success`: no rule was violated by more than 1e-3 of its unit.
    """
    loop = LiveLoop()
    results = []
    for index, act in enumerate(acts):
        result = ActResult(act)
        began = dict(loop.live)
        frames = int(round(act.seconds * FRAME_HZ))
        for frame in range(frames):
            u = frame / (frames - 1)
            ball = BALL_PARKED_M
            if act.ball_m is not None:
                ball += (act.ball_m - BALL_PARKED_M) * np.sin(np.pi * u) ** 2
            mark = loop.target
            if act.trace_m is not None:
                turn = 2.0 * np.pi * u
                mark = mark + act.trace_m * np.array([np.cos(turn) - 1.0, np.sin(turn), 0.0])
            inputs = Inputs(
                rail_m=None if act.rail is None else float(act.rail(u)),
                ball_m=float(ball),
                mark=mark,
                hold=act.hold,
                axis_deg=act.axis_deg,
                cone_deg=act.cone_deg,
                elbow_m=_ramped(act.elbow_m, began["elbow_m"], u, lower_bound=True),
                view_deg=_ramped(act.view_deg, began["view_deg"], u, lower_bound=False),
                dexterity=_ramped(act.dexterity, began["dexterity"], u, lower_bound=True),
                clearance=act.clearance,
            )
            solved = loop.step(inputs)
            result.solve_ms.append(solved.solve_ms)
            result.live.append(solved.live)
            result.worst_violation = max(result.worst_violation, solved.violation)
            result.rows.append(
                [
                    *solved.q,
                    inputs.ball_m,
                    *instrument_values(
                        inputs, solved.live, loop.target, caption=index, tracing=act.trace_m
                    ),
                    frame / FRAME_HZ,
                ]
            )
        results.append(result)
    return {
        "robot": loop.robot,
        "joint_names": loop.joint_names,
        "acts": results,
        "target": loop.target,
        "success": all(result.worst_violation < VIOLATION_TOLERANCE for result in results),
    }

The acts

python_constraints_example.py (acts)
@dataclass(frozen=True)
class Act:
    """One stretch of the loop: which rules are on, and what moves the rail and the ball."""

    caption: str
    seconds: float
    # The rail position over the act's 0..1 [m] while something else drives it; None once the
    # rail is the solver's seventh axis.
    rail: Callable[[float], float] | None = None
    # The ball's closest approach along its line [m]; it drives in from the far side and back.
    ball_m: float | None = None
    hold: bool = True
    axis_deg: float | None = None
    cone_deg: float | None = None
    # Bounds that ramp from the live value to these targets.
    elbow_m: float | None = None
    view_deg: float | None = None
    dexterity: float | None = None
    clearance: bool = False
    # Radius of the circle the mark moves on [m], once round in the act; None: it stays put.
    trace_m: float | None = None


def _swing(cycles: float, phase: float = 0.0) -> Callable[[float], float]:
    return lambda u: RAIL_SWING_M * np.sin(2.0 * np.pi * cycles * u + phase)


ACTS = (
    Act("No rule\nThe rail moves and the tool goes with it", 3.0, rail=_swing(0.5), hold=False),
    Act(
        "WorldHold\nThe tip stays on the mark; the tool axis wanders",
        4.5,
        rail=_swing(0.75, np.pi),
    ),
    Act(
        "+ HoldAxis 0.5°\nThe tool locks vertical",
        4.0,
        rail=_swing(0.5, 0.5 * np.pi),
        axis_deg=0.5,
    ),
    Act(
        "+ WatchTheTool 3°\nThe camera on the mast turns to keep the tip in its cone",
        5.0,
        rail=_swing(1.0, -0.5 * np.pi),
        axis_deg=0.5,
        view_deg=VIEW_DEG,
    ),
    Act(
        "PointsDown 20° + ElbowUp, and the rail is now the solver's\n"
        "The carriage drives to where the elbow clears the plane",
        4.0,
        cone_deg=20.0,
        view_deg=VIEW_DEG,
        elbow_m=1.25,
    ),
    Act(
        "+ StayDexterous\nManipulability stays above its bound",
        4.0,
        cone_deg=20.0,
        view_deg=VIEW_DEG,
        dexterity=1.25,
    ),
    Act(
        "+ Clearance\nThe forearm keeps 15 cm from the ball",
        6.0,
        cone_deg=20.0,
        view_deg=VIEW_DEG,
        ball_m=-0.10,
        clearance=True,
    ),
    Act(
        "All seven at once\nThe mark moves, and the tip traces the circle",
        7.0,
        ball_m=-0.45,
        axis_deg=20.0,
        cone_deg=20.0,
        elbow_m=1.22,
        view_deg=VIEW_DEG,
        dexterity=0.8,
        clearance=True,
        trace_m=0.12,
    ),
)

The cell

The tool, the rail, the workpiece, the camera and the ball are added to tesseract's abb_irb2400_on_positioner scene. Their meshes carry a MeshMaterial with metallic and roughness values, which the viewer shades with.

python_constraints_example.py (cell)
def build_cell() -> Robot:
    """The IRB 2400 on its rail, with a tool, a workpiece, a forearm camera and a ball."""
    locator = GeneralResourceLocator(paths=[get_tesseract_resource_path()])
    robot = Robot.from_tesseract_support("abb_irb2400_on_positioner", locator)

    # The tool: an adapter plate, then the grinder with its base on the plate and its axis
    # along the flange normal. `tcp` is its tip.
    grinder_resource = locator.locateResource(GRINDER_MESH)
    if grinder_resource is None:
        raise ResourceNotFoundError(f"Could not locate the grinder mesh: {GRINDER_MESH}")
    grinder = createMeshFromResource(grinder_resource, np.full(3, GRINDER_SCALE), True, True)
    _add_link(
        robot,
        "adapter",
        "tool0",
        _isometry((0, 0, ADAPTER_M / 2)),
        (_visual(_cylinder(ADAPTER.radius_m, ADAPTER.height_m, ANODISED)),),
    )
    _add_link(
        robot,
        "grinder",
        "tool0",
        _isometry((0, 0, ADAPTER_M + GRINDER_BASE_M * GRINDER_SCALE)),
        tuple(
            _visual(
                _finished(
                    np.asarray(part.getVertices()),
                    np.asarray(part.getFaces()).reshape(-1, 4)[:, 1:],
                    TOOL_STEEL,
                )
            )
            for part in grinder
        ),
    )
    tool_length = ADAPTER_M + (GRINDER_BASE_M + GRINDER_TIP_M) * GRINDER_SCALE
    _add_link(robot, TCP, "tool0", _isometry((0, 0, tool_length)))

    # The rail and the carriage the robot stands on.
    _add_link(
        robot,
        "rail",
        WORLD,
        _isometry(),
        (
            _visual(
                _box(*RAIL_BEAM_M, RAIL_STEEL), _isometry((RAIL_BEAM_X_M, 0, RAIL_BEAM_M[2] / 2))
            ),
            _visual(
                _box(*RAIL_BEAM_M, RAIL_STEEL), _isometry((-RAIL_BEAM_X_M, 0, RAIL_BEAM_M[2] / 2))
            ),
        ),
    )
    _add_link(
        robot,
        "carriage",
        "positioner_tool0",
        _isometry(),
        (_visual(_box(*CARRIAGE_M, ALUMINIUM), _isometry((0, 0, CARRIAGE_M[2] / 2))),),
    )

    # The workpiece: a plate on a pedestal, its top where the tool tip is at the start.
    tip = robot.env.getJointGroup("manipulator").calcFwdKin(START)[TCP].translation
    top = float(tip[2])
    _add_link(
        robot,
        "workpiece",
        WORLD,
        _isometry((tip[0], tip[1], 0)),
        (
            _visual(
                _box(PEDESTAL_SIDE_M, PEDESTAL_SIDE_M, top - PLATE.height_m, CAST_IRON),
                _isometry((0, 0, (top - PLATE.height_m) / 2)),
            ),
            _visual(
                _cylinder(PLATE.radius_m, PLATE.height_m, POLISHED),
                _isometry((0, 0, top - PLATE.height_m / 2)),
            ),
        ),
    )

    # The camera: a mast on the carriage, a pan joint about the vertical, a tilt joint about
    # the horizontal. The pan frame's x axis heads for the mark and the tilt frame's x axis
    # points at it, so at zero pan and tilt the camera looks at the mark. `camera` is the
    # optical frame: z forward, x right, y down.
    mast_x, mast_y = CAMERA_MAST_XY_M
    to_mark = tip - np.array([mast_x, mast_y, CAMERA_HEIGHT_M])
    heading = np.arctan2(to_mark[1], to_mark[0])
    pitch = np.arctan2(-to_mark[2], np.hypot(to_mark[0], to_mark[1]))
    yaw = np.array(
        [[np.cos(heading), -np.sin(heading), 0], [np.sin(heading), np.cos(heading), 0], [0, 0, 1]]
    )
    nod = np.array(
        [[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]
    )
    optical = np.array(OPTICAL_FROM_TILT)
    _add_link(
        robot,
        "camera_mast",
        "positioner_tool0",
        _isometry((mast_x, mast_y, 0)),
        (
            _visual(
                _cylinder(MAST_RADIUS_M, CAMERA_HEIGHT_M - MAST_HEAD_CLEARANCE_M, RAIL_STEEL),
                _isometry((0, 0, (CAMERA_HEIGHT_M - MAST_HEAD_CLEARANCE_M) / 2)),
            ),
        ),
    )
    _add_link(
        robot,
        "camera_pan",
        "camera_mast",
        _isometry((0, 0, CAMERA_HEIGHT_M), yaw),
        (
            _visual(
                _cylinder(PAN_HOUSING.radius_m, PAN_HOUSING.height_m, ANODISED),
                _isometry((0, 0, -PAN_HOUSING.height_m / 2)),
            ),
        ),
        axis=(0, 0, 1),
        revolute=True,
    )
    _add_link(
        robot,
        CAMERA,
        "camera_pan",
        _isometry(rotation=nod @ optical),
        (
            _visual(_box(*CAMERA_BODY_M, ANODISED)),
            _visual(
                _cylinder(LENS.radius_m, LENS.height_m, TOOL_STEEL), _isometry((0, 0, LENS_Z_M))
            ),
        ),
        # The tilt axis, the pan frame's y, in the optical frame: its -x.
        axis=(-1, 0, 0),
        revolute=True,
    )

    # The ball, on a slide parallel to the rail. The only body with collision geometry
    # besides the robot, so it is what `Clearance` keeps the forearm away from.
    _add_link(
        robot,
        "obstacle",
        WORLD,
        _isometry((BALL_LINE_XZ_M[0], 0, BALL_LINE_XZ_M[1])),
        (_visual(_ball(BALL_RADIUS_M, RED_LACQUER)),),
        collision=Sphere(BALL_RADIUS_M),
        axis=(0, 1, 0),
    )
    _add_instruments(robot)
    return robot

Drive it yourself

Bash
tesseract_python_constraints_example            # the acts play by themselves
tesseract_python_constraints_example --sliders  # you drive

Without the flag the acts play, and the viewer's Playback controls start, stop and scrub them.

With --sliders the loop solves from the viewer's control panel instead:

  • Rail swings by itself starts and stops the rail's own motion; unticked, the Rail slider is yours.
  • Rail is the solver's 7th axis hands the carriage to the solver. It drives to abeam of the tip and stays within the band of Carriage follows the tip within. Widen the band, and the other rules move the carriage instead.
  • The mark has a handle. Drag it: along the rail the carriage follows, and across it the arm reaches.
  • Ball on its slide drives the ball into the arm.
  • Every rule has a checkbox, and a slider where it has a bound. A bound that comes on closes in from the live value, as the acts' bounds ramp.
  • Solve shows the time of the last frame, and says when the rules you ticked conflict.

Either way, Constraint opacity sets how solid the rules' glyphs are drawn.

python_constraints_example.py (sliders)
def drive_with_sliders(viewer, loop: LiveLoop, stop: threading.Event) -> None:
    """Solve a frame per tick from the GUI's state, until `stop` is set.

    The rail swings by itself or follows its slider, or is handed to the solver, which then
    keeps the carriage abeam of the tip; the ball follows its slider; the mark follows the
    handle dragged in the scene; every rule has a checkbox, and a slider where it has a bound.
    A bound that comes on closes in from the live value, as the acts' bounds ramp.
    """
    gui = viewer.server.gui
    handle = viewer.server.scene.add_transform_controls(
        "/mark_handle",
        scale=MARK_HANDLE_M,
        position=tuple(loop.target),
        disable_rotations=True,
        depth_test=False,
    )
    with gui.add_folder("Inputs"):
        swings = gui.add_checkbox("Rail swings by itself", True)
        rail = _slider(gui, "Rail [m]", SLIDER_RAIL)
        ours = gui.add_checkbox("Rail is the solver's 7th axis", False)
        follow = _slider(gui, "Carriage follows the tip within [m]", SLIDER_FOLLOW)
        gui.add_markdown(
            "Ticked, the carriage drives to abeam of the tip and stays there: drag the mark by "
            "its handle. Widen the band for the other rules to move the carriage."
        )
        ball = _slider(gui, "Ball on its slide [m]", SLIDER_BALL)
    with gui.add_folder("Rules"):
        hold = gui.add_checkbox("WorldHold", True)
        axis_on = gui.add_checkbox("HoldAxis", False)
        axis = _slider(gui, "HoldAxis tolerance [°]", SLIDER_AXIS_DEG)
        cone_on = gui.add_checkbox("PointsDown", False)
        cone = _slider(gui, "PointsDown cone [°]", SLIDER_CONE_DEG)
        elbow_on = gui.add_checkbox("ElbowUp", False)
        elbow = _slider(gui, "Elbow at least [m]", SLIDER_ELBOW_M)
        view_on = gui.add_checkbox("WatchTheTool", True)
        view = _slider(gui, "Tip off the camera axis at most [°]", SLIDER_VIEW_DEG)
        dexterous_on = gui.add_checkbox("StayDexterous", False)
        dexterous = _slider(gui, "Manipulability at least", SLIDER_DEXTERITY)
        clearance = gui.add_checkbox("Clearance", False)
    status = gui.add_text("Solve", initial_value="", disabled=True)

    began = time.perf_counter()
    recent: list[float] = []
    while not stop.is_set():
        tick = time.perf_counter()
        if swings.value and not ours.value:
            rail.value = float(RAIL_SWING_M * np.sin(2.0 * np.pi * (tick - began) / RAIL_PERIOD_S))
        live = loop.live
        inputs = Inputs(
            rail_m=None if ours.value else float(rail.value),
            ball_m=float(ball.value),
            mark=np.array(handle.position, dtype=np.float64),
            hold=hold.value,
            axis_deg=_closing("tilt_deg", axis.value, live) if axis_on.value else None,
            cone_deg=_closing("tilt_deg", cone.value, live) if cone_on.value else None,
            elbow_m=_closing("elbow_m", elbow.value, live) if elbow_on.value else None,
            view_deg=_closing("view_deg", view.value, live) if view_on.value else None,
            dexterity=_closing("dexterity", dexterous.value, live) if dexterous_on.value else None,
            clearance=clearance.value,
            follow_m=_closing("lead_m", follow.value, live) if ours.value else None,
        )
        frame = loop.step(inputs)
        if ours.value:
            # Show where the solver put the carriage; the slider takes over from there.
            rail.value = float(np.clip(frame.q[0], -RAIL_SWING_M, RAIL_SWING_M))
        viewer.update_joint_positions(
            loop.joint_names,
            [
                *frame.q,
                inputs.ball_m,
                *instrument_values(inputs, frame.live, loop.target, caption=None),
            ],
        )
        feasible = frame.violation < VIOLATION_TOLERANCE
        status.value = f"{frame.solve_ms:.1f} ms" + ("" if feasible else "  ·  rules conflict")
        recent.append(frame.solve_ms)
        if len(recent) == READOUT_FRAMES:
            # Where an act's caption would be: what the solves of the last frames took.
            on = rule_count(inputs) + (inputs.follow_m is not None)
            _add_text(
                viewer.server.scene,
                "/readout",
                solve_line(on, float(np.median(recent))),
                caption_anchor(),
                panel_frame()[1],
                CAPTION_LINE_M,
                valign="top",
            )
            recent.clear()
        stop.wait(max(0.0, 1.0 / FRAME_HZ - (time.perf_counter() - tick)))

A rule of your own

The carriage follows the tip because of an eighth rule, written in the example. With the rail handed over and no rule asking, the solver leaves the carriage where it is: the only cost is staying close to the frame before. CarriageFollows asks.

python_constraints_example.py (own_rule)
class CarriageFollows(PyConstraint):
    """A rule of this example's own: the carriage stays abeam of the tip, within `band` [m].

    Along the rail, the tip may lead or trail the carriage by `band` at most: the carriage does
    the travelling and the arm the reaching. The rail is the first joint of `q`, and runs along
    the world's y.
    """

    def __init__(self, var: ti.Var, cell: JointGroup, tcp: str, band: float, coeff: float = 1.0):
        super().__init__("carriage_follows", var, [-band], [band], coeff=coeff)
        self._cell, self._tcp = cell, tcp

    def f(self, q: np.ndarray) -> np.ndarray:
        return np.array([self._cell.calcFwdKin(q)[self._tcp].translation[1] - q[0]])

Solve times

Each act's caption carries the median solve of its frames, and the last ruler of the panel shows every frame's solve against the 33 ms a 30 Hz loop has. The numbers are those of the machine the recording was made on, a shared 2-vCPU cloud VM (Intel Xeon, 2.1 GHz). Run the example for local numbers: it prints the median and the slowest frame per act.

StayDexterous, WatchTheTool, ElbowUp and Clearance use the default forward-difference Jacobian; Clearance queries the contact manager for each column.

See also