Kinematics Examples¶

Kinematics below the high-level Robot API: solver plugins, Jacobians and manipulability, and state solvers. Each example is a port of an upstream tesseract test or takes its parameters from upstream source, cited in the module docstring. Every value printed is also asserted in tests/examples.

Bash

# Installed console script
tesseract_kinematics_plugins_example

# Or via Python module invocation
pixi run python -m tesseract_robotics.examples.kinematics_plugins_example

Kinematics Plugins¶

KUKA LBR IIWA 14 R820 forward and inverse kinematics built from the plugin YAML the SRDF names (lbr_iiwa_14_r820_plugins.yaml): one forward solver, KDLFwdKinChain, and two inverse solvers, KDLInvKinChainLMA (the default) and KDLInvKinChainNR.

Load the factory from the YAML file and list its solvers:

kinematics_plugins_example.py

    locator = GeneralResourceLocator()
    factory = KinematicsPluginFactory(locate(locator, PLUGINS_URL), locator)

    fwd_kin_plugins = plugin_names(factory.getFwdKinPlugins())
    inv_kin_plugins = plugin_names(factory.getInvKinPlugins())
    default_inv_kin_plugin = factory.getDefaultInvKinPlugin(GROUP)

Switch the default inverse solver and solve upstream's runInvKinIIWATest problem with each: the target is tool0 at q = 0, (0, 0, 1.306) m with the identity rotation, seeded at ±0.785398 rad.

kinematics_plugins_example.py

    fwd_kin = factory.createFwdKin(
        GROUP, factory.getDefaultFwdKinPlugin(GROUP), scene_graph, scene_state
    )
    target = fwd_kin.calcFwdKin(np.zeros(len(JOINT_NAMES)))[TIP_LINK]

    ik = {}
    for solver_name in ["KDLInvKinChainLMA", "KDLInvKinChainNR"]:
        factory.setDefaultInvKinPlugin(GROUP, solver_name)
        inv_kin = factory.createInvKin(
            GROUP, factory.getDefaultInvKinPlugin(GROUP), scene_graph, scene_state
        )
        solutions = inv_kin.calcInvKin({TIP_LINK: target}, SEED)
        ik[solver_name] = {
            "solver_name": inv_kin.getSolverName(),
            **check_solutions(solver_name, solutions, fwd_kin, target),
        }

Each solution is checked with the solver's own stopping rule rather than a guessed tolerance, evaluated on the error twist KDL uses (diff(FK(q), target) in the base frame):

solver	stopping rule (orocos_kdl 1.5.3)	bound
`KDLInvKinChainLMA`	task-weighted twist norm, weights (1, 1, 1, 0.1, 0.1, 0.1)	`eps` = 1e-5
`KDLInvKinChainNR`	every twist component	`pos_eps` = 1e-6

NR tests the iterate before its final Newton update, so the returned q is one step further on; the example checks the returned q against the same bound.

Solvers can also be created from a PluginInfo directly, without a group entry in the factory:

kinematics_plugins_example.py

    fwd_info = factory.getFwdKinPlugins()[GROUP].plugins["KDLFwdKinChain"]
    inv_info = factory.getInvKinPlugins()[GROUP].plugins["KDLInvKinChainNR"]
    fwd_kin_from_info = factory.createFwdKin("KDLFwdKinChain", fwd_info, scene_graph, scene_state)
    inv_kin_from_info = factory.createInvKin("KDLInvKinChainNR", inv_info, scene_graph, scene_state)
    created_by_plugin_info = (fwd_kin_from_info.getSolverName(), inv_kin_from_info.getSolverName())

Add a solver, make it the default and remove it again, as upstream's PluginFactorAPIUnit does. When the default is removed, the first remaining solver by name becomes the default. Removing a group's last solver raises KinematicsPluginRemovalError.

kinematics_plugins_example.py

    fwd_default = PluginInfo()
    fwd_default.class_name = fwd_info.class_name
    fwd_default.config = fwd_info.config
    factory.addFwdKinPlugin(GROUP, "default", fwd_default)
    fwd_after_add = list(factory.getFwdKinPlugins()[GROUP].plugins)
    factory.setDefaultFwdKinPlugin(GROUP, "default")
    fwd_default_after_set = factory.getDefaultFwdKinPlugin(GROUP)
    factory.removeFwdKinPlugin(GROUP, "default")

    inv_default = PluginInfo()
    inv_default.class_name = inv_info.class_name
    inv_default.config = inv_info.config
    factory.addInvKinPlugin(GROUP, "default", inv_default)
    inv_after_add = list(factory.getInvKinPlugins()[GROUP].plugins)
    factory.setDefaultInvKinPlugin(GROUP, "default")
    inv_default_after_set = factory.getDefaultInvKinPlugin(GROUP)
    factory.removeInvKinPlugin(GROUP, "default")

saveConfig writes the factory's current configuration, and a factory built from that file reports the same getConfig():

kinematics_plugins_example.py

    with tempfile.TemporaryDirectory() as tmp:
        saved = Path(tmp) / "kinematics_plugins.yaml"
        factory.saveConfig(saved)
        config_round_trip_equal = (
            KinematicsPluginFactory(saved, locator).getConfig() == factory.getConfig()
        )

A KinematicGroup needs no Environment, only an inverse solver, a scene graph and a scene state. calcInvKin takes a Python list of KinGroupIKInput; a list never converts to the opaque KinGroupIKInputs, so it always selects the list overload.

kinematics_plugins_example.py

    group = KinematicGroup(GROUP, JOINT_NAMES, inv_kin_from_info, scene_graph, scene_state)
    solutions = group.calcInvKin([KinGroupIKInput(target, BASE_LINK, TIP_LINK)], SEED)

Kinematics Analysis¶

Bash

tesseract_kinematics_analysis_example

A port of upstream's runJacobianTest on the IIWA, followed by the analysis functions of tesseract/kinematics/utils.h and the pose-error helpers of tesseract/common/utils.h, each on upstream's own unit-test values.

Jacobians against finite differences¶

The ForwardKinematics Jacobian, re-based with jacobianChangeBase and jacobianChangeRefPoint, against numericalJacobian, at upstream's q = (−0.785398, 0.785398, …) with link points e_k and a change_base of Rz(90°) plus a unit translation:

kinematics_analysis_example.py

    # runJacobianIIWATest: tool0 at the origin and at e_k, with and without change_base
    cases = [(np.zeros(3), Isometry3d.Identity())]
    cases += [(unit(k), Isometry3d.Identity()) for k in range(3)]
    cases += [(np.zeros(3), isometry(RZ90, unit(k))) for k in range(3)]
    cases += [(unit(k), isometry(RZ90, unit(k))) for k in range(3)]

    tip_pose = fwd_kin.calcFwdKin(Q)[TIP_LINK]
    errors = []
    for link_point, change_base in cases:
        jacobian = np.asfortranarray(fwd_kin.calcJacobian(Q, TIP_LINK))
        jacobianChangeBase(jacobian, change_base)
        jacobianChangeRefPoint(jacobian, (change_base * tip_pose).linear @ link_point)
        numerical = numericalJacobian(change_base, fwd_kin, Q, TIP_LINK, link_point)
        errors.append(np.abs(numerical - jacobian).max())

Every JointGroup.calcJacobian overload on a KinematicGroup, for every link, against the matching numericalJacobian overload: a static base link becomes a change_base, an active one the base-link form of numericalJacobian.

kinematics_analysis_example.py

    # runKinGroupJacobianIIWATest: every link, every static and active base link
    poses = group.calcFwdKin(Q)
    relative_errors = []
    for k in range(3):
        link_point = unit(k)
        link_offset = Isometry3d(Translation3d(link_point))
        for link in LINK_NAMES:
            identity = Isometry3d.Identity()
            errors.append(
                np.abs(
                    group.calcJacobian(Q, link, link_point)
                    - numericalJacobian(identity, group, Q, link, link_point)
                ).max()
            )
            errors.append(
                np.abs(
                    group.calcJacobian(Q, link)
                    - numericalJacobian(identity, group, Q, link, np.zeros(3))
                ).max()
            )
            for base in group.getStaticLinkNames():
                change_base = poses[base].inverse()
                errors.append(
                    np.abs(
                        group.calcJacobian(Q, base, link, link_point)
                        - numericalJacobian(change_base, group, Q, link, link_point)
                    ).max()
                )
                errors.append(
                    np.abs(
                        group.calcJacobian(Q, base, link)
                        - numericalJacobian(change_base, group, Q, link, np.zeros(3))
                    ).max()
                )
            for base in group.getActiveLinkNames():
                relative_errors.append(
                    np.abs(
                        group.calcJacobian(Q, base, link, link_point)
                        - numericalJacobian(group, Q, base, identity, link, link_offset)
                    ).max()
                )
                relative_errors.append(
                    np.abs(
                        group.calcJacobian(Q, base, link)
                        - numericalJacobian(group, Q, base, identity, link, identity)
                    ).max()
                )

numericalJacobian is a forward difference with δ = 1e-8, so its error is truncation plus rounding, and the tolerance adds the two:

term	value	source
truncation, δ/2 · max point norm	1.7e-8	δ: `kinematics/core/src/utils.cpp:44, 80`; norm ≤ 1.306 + 1 + 1 m
rounding, 2 · (8 transforms · 3ε) · 3.306 m / δ	3.5e-6	FK chain `joint_a1`…`joint_a7-tool0`
`JACOBIAN_TOL`	3.5e-6	measured maximum 6.6e-8; upstream's ceiling is 1e-3

The form relative to an active base link differences two numerical Jacobians, so its bound is twice that.

Twists¶

Changing the base and reference point of a twist J·q̇ equals changing them on J and then multiplying by q̇; the residual is rounding only.

kinematics_analysis_example.py

    jacobian = np.asfortranarray(fwd_kin.calcJacobian(Q, TIP_LINK))
    q_dot = np.arange(1.0, 8.0) / 10.0
    change_base, ref_point = isometry(RZ90, unit(0)), unit(2)

    twist = jacobian @ q_dot
    twistChangeBase(twist, change_base)
    twistChangeRefPoint(twist, ref_point)

    changed = jacobian.copy(order="F")
    jacobianChangeBase(changed, change_base)
    jacobianChangeRefPoint(changed, ref_point)

Manipulability and singularity¶

At q = 0 the IIWA is singular: joint_a1, joint_a5 and joint_a7 all turn about the base z-axis (every axis 0 0 1, and the x offsets ∓0.00043624 of joint_a2 and joint_a4 cancel), so their Jacobian columns are identical and the rank drops to 5. The example checks the columns, the rank and σ_min against isNearSingularity's default threshold 0.01 (utils.h:129), not only the flag. At a singular configuration the ellipsoid's measure and condition are sys.float_info.max and its volume is 0, as upstream.

kinematics_analysis_example.py

    singularity = {}
    manipulability = {}
    for name, q in [("upstream", Q), ("zero", np.zeros(len(JOINT_NAMES)))]:
        jacobian = fwd_kin.calcJacobian(q, TIP_LINK)
        singular_values = np.linalg.svd(jacobian, compute_uv=False)
        singularity[name] = {
            "sigma_min": float(singular_values.min()),
            "rank": int(np.linalg.matrix_rank(jacobian)),
            "near_singularity": isNearSingularity(jacobian),
            # joint_a1, a5, a7 columns (indices 0, 4, 6)
            "identical_columns": bool(
                np.array_equal(jacobian[:, 0], jacobian[:, 4])
                and np.array_equal(jacobian[:, 0], jacobian[:, 6])
            ),
        }
        manip = calcManipulability(jacobian)
        manipulability[name] = (
            manipulability_fields(manip) if name == "upstream" else {"m": ellipsoid_fields(manip.m)}
        )
        if name == "upstream":
            manipulability["repr"] = repr(manip)

Harmonizing joint angles¶

Shifting every redundancy-capable joint by 2π and harmonizing gives q back to within 2.7e-15 rad, not bit-exactly: q + 2π and the harmonizer's + π each round once (fmod itself is exact).

kinematics_analysis_example.py

    redundant = group.getRedundancyCapableJointIndices()
    position_limits = group.getLimits().joint_limits
    shifted = Q.copy()
    shifted[redundant] += 2 * np.pi

    toward_zero = shifted.copy()
    harmonizeTowardZero(toward_zero, redundant)
    toward_median = shifted.copy()
    harmonizeTowardMedian(toward_median, redundant, position_limits)

Validity and limits¶

kinematics_analysis_example.py

    six = Q[:6].tolist()
    is_valid = (isValid(six), isValid([*six[:5], float("nan")]))
    check_kinematics = checkKinematics(group)

KinematicLimits.resize sizes all four limit arrays; isWithinLimits has no tolerance, and enforceLimits returns a clamped copy and leaves its input alone.

kinematics_analysis_example.py

    group_limits = group.getLimits()
    limits = KinematicLimits()
    limits.resize(len(JOINT_NAMES))
    shapes_after_resize = [
        limits.joint_limits.shape,
        limits.velocity_limits.shape,
        limits.acceleration_limits.shape,
        limits.jerk_limits.shape,
    ]
    limits.joint_limits = group_limits.joint_limits
    limits.velocity_limits = group_limits.velocity_limits
    limits.acceleration_limits = group_limits.acceleration_limits
    limits.jerk_limits = group_limits.jerk_limits

    within = (isWithinLimits(Q, limits.joint_limits), isWithinLimits(shifted, limits.joint_limits))
    shifted_before = shifted.copy()
    enforced = enforceLimits(shifted, limits.joint_limits)

Pose errors and tolerance bands¶

kinematics_analysis_example.py

    identity = Isometry3d.Identity()
    rot_x = Isometry3d(AngleAxisd(np.pi / 2, UNIT_X))
    translated = Isometry3d(Translation3d(1.0, 2.0, 3.0))
    rotation_error = calcTransformError(identity, rot_x)
    translation_error = calcTransformError(identity, translated)

applyTolerances zeroes the part of an error inside a band and shifts the rest by the band edge. calcJacobianTransformErrorDiff applies it to both the error and the perturbed error, so a band around both gives a zero row, a band below both leaves the raw difference, and a band edge between them gives the clamped difference 0.15 (upstream's calcJacobianTransformErrorDiff_Toleranced):

kinematics_analysis_example.py

    v = np.array([-2.0, -0.25, 0.25, 2.0])
    applyTolerances(v, np.array([-1.0, -0.5, -0.5, -1.0]), np.array([1.0, 0.5, 0.5, 1.0]))

    target = Isometry3d(Translation3d(1.0, 2.0, 3.0))
    target_perturbed = Isometry3d(AngleAxisd(-ERROR_DIFF_EPS, UNIT_X)) * target
    source = Isometry3d(AngleAxisd(ERROR_DIFF_BASE_ANGLE, UNIT_X))
    source_perturbed = Isometry3d(AngleAxisd(ERROR_DIFF_BASE_ANGLE + ERROR_DIFF_EPS, UNIT_X))
    source_big = Isometry3d(AngleAxisd(ERROR_DIFF_BASE_ANGLE + ERROR_DIFF_BIG_STEP, UNIT_X))

    # Band around both errors on the x-rotation row: clamped to zero
    inside_lower = np.array([-5.0, -5.0, -5.0, 0.0, -0.5, -0.5])
    inside_upper = np.array([5.0, 5.0, 5.0, 1.0, 0.5, 0.5])
    # Band below both errors: the same offset on both, the raw difference remains
    band_top = ERROR_DIFF_BASE_ANGLE - 0.1
    below_lower = np.array([-5.0, -5.0, -5.0, band_top - 0.01, -0.5, -0.5])
    below_upper = np.array([5.0, 5.0, 5.0, band_top, 0.5, 0.5])
    # Band edge between the two errors: 0.5 inside [0, 0.55], 0.7 above it -> 0.15
    edge_upper = np.array([5.0, 5.0, 5.0, 0.55, 0.5, 0.5])

    raw = calcJacobianTransformErrorDiff(target, source, source_perturbed)
    raw_dynamic = calcJacobianTransformErrorDiff(target, target_perturbed, source, source_perturbed)
    error_diff = {
        "raw": raw,
        "inside_band": np.abs(
            calcJacobianTransformErrorDiff(
                target, source, source_perturbed, inside_lower, inside_upper
            )
        ).max(),
        "above_band_vs_raw": np.abs(
            calcJacobianTransformErrorDiff(
                target, source, source_perturbed, below_lower, below_upper
            )
            - raw
        ).max(),
        "band_edge": calcJacobianTransformErrorDiff(
            target, source, source_big, inside_lower, edge_upper
        )[3],
        "dynamic_inside_band": np.abs(
            calcJacobianTransformErrorDiff(
                target, target_perturbed, source, source_perturbed, inside_lower, inside_upper
            )
        ).max(),
        "dynamic_above_band_vs_raw": abs(
            calcJacobianTransformErrorDiff(
                target, target_perturbed, source, source_perturbed, below_lower, below_upper
            )[3]
            - raw_dynamic[3]
        ),
        "dynamic_band_edge": calcJacobianTransformErrorDiff(
            target, target_perturbed, source, source_big, inside_lower, edge_upper
        )[3],
    }

State Solver¶

Bash

tesseract_state_solver_example

A port of upstream's floating-joint and insert-scene-graph state-solver tests (state_solver_test_suite.h, runSetFloatingJointStateTest and runAddSceneGraphTest). After every change, a KDLStateSolver rebuilt from the same scene graph is the oracle: KDL treats a FLOATING joint as fixed at its origin (kdl_parser.cpp:133–160), so moving the origin with changeJointOrigin and rebuilding gives an independent computation of the same link transforms. Link transforms are compared with upstream's isApprox(1e-6).

replaceJoint turns joint_a1 into a FLOATING joint at x = 1.25 m. It is no longer an active joint, so joint_a2…joint_a7 remain:

state_solver_example.py

    origin = translation(ORIGIN_X)
    joint = Joint(FLOATING_JOINT)
    joint.parent_to_joint_origin_transform = origin
    joint.parent_link_name = "base_link"
    joint.child_link_name = "link_1"
    joint.type = JointType.FLOATING

    edits = [
        scene_graph.removeJoint(FLOATING_JOINT),
        scene_graph.addJoint(joint),
        solver.replaceJoint(joint),
    ]

Move the floating joint with floating values only, then with joints and floating values together (upstream's y = 1.5 m and z = 1.5 m steps). The second move is upstream's: read state.floating_joints, edit it, pass it back. It is a dict copy, so editing it changes nothing until it is passed to setState:

state_solver_example.py

    # Floating value only: the active joints keep their values
    origin = translation(ORIGIN_X, ORIGIN_Y)
    solver.setState({FLOATING_JOINT: origin})
    edits.append(scene_graph.changeJointOrigin(FLOATING_JOINT, origin))
    steps.append(check_step(solver, scene_graph, origin))

    # Joints and floating values together: read the state back, edit it, pass it in
    origin = translation(ORIGIN_X, ORIGIN_Y, ORIGIN_Z)
    state = solver.getState()
    floating_joints = state.floating_joints  # a copy: edit it, then pass it back
    floating_joints[FLOATING_JOINT] = origin
    solver.setState(state.joints, floating_joints)
    edits.append(scene_graph.changeJointOrigin(FLOATING_JOINT, origin))
    steps.append(check_step(solver, scene_graph, origin))

Every native setState, getState and getLinkTransforms overload, each with floating_joint_values. Each setState starts from a different state, so a call that changed nothing would disagree with the oracle:

state_solver_example.py

    # Each setState starts from another state (zero joints, the step-1 floating
    # origin), so a call that changed nothing would disagree with the oracle
    other_joints, other_floating = np.zeros(len(names)), {FLOATING_JOINT: translation(ORIGIN_X)}
    overloads = {}
    solver.setState(other_joints, other_floating)
    solver.setState(JOINT_VALUES, floating)
    overloads["setState(values, floating)"] = agree(solver.getState().link_transforms, oracle)
    solver.setState(other_joints, other_floating)
    solver.setState(joints, floating)
    overloads["setState(dict, floating)"] = agree(solver.getState().link_transforms, oracle)
    solver.setState(other_joints, other_floating)
    solver.setState(names, JOINT_VALUES, floating)
    overloads["setState(names, values, floating)"] = agree(
        solver.getState().link_transforms, oracle
    )
    solver.setState(JOINT_VALUES, other_floating)
    solver.setState(floating)  # floating values only: the joints keep JOINT_VALUES
    overloads["setState(floating)"] = agree(solver.getState().link_transforms, oracle)

    # getState computes a state without changing the solver's; it holds other_floating
    solver.setState(JOINT_VALUES, other_floating)
    overloads["getState(values, floating)"] = agree(
        solver.getState(JOINT_VALUES, floating).link_transforms, oracle
    )
    overloads["getState(dict, floating)"] = agree(
        solver.getState(joints, floating).link_transforms, oracle
    )
    overloads["getState(names, values, floating)"] = agree(
        solver.getState(names, JOINT_VALUES, floating).link_transforms, oracle
    )
    overloads["getState(floating)"] = agree(solver.getState(floating).link_transforms, oracle)
    overloads["getLinkTransforms(names, values, floating)"] = agree(
        solver.getLinkTransforms(names, JOINT_VALUES, floating), oracle
    )

    # getLinkTransforms() lists the current state in getLinkNames() order
    solver.setState(JOINT_VALUES, floating)
    overloads["getLinkTransforms()"] = agree(
        dict(zip(solver.getLinkNames(), solver.getLinkTransforms())), oracle
    )

The three getJacobian overloads give the same matrix, which agrees with KDL's; the floating joint adds no column.

state_solver_example.py

    jacobians = [
        solver.getJacobian(JOINT_VALUES, TIP_LINK, floating),
        solver.getJacobian(joints, TIP_LINK, floating),
        solver.getJacobian(names, JOINT_VALUES, TIP_LINK, floating),
    ]
    kdl_jacobian = KDLStateSolver(scene_graph).getJacobian(names, JOINT_VALUES, TIP_LINK)

insertSceneGraph attaches upstream's two-link sub-graph (a unit box link and a link 1.25 m along x, joined by a FIXED joint), first through a FIXED joint, then again under the prefix prefix_ through a FLOATING joint. The inserted root sits at parent ∘ joint origin, its child 1.25 m along x from it, and a floating value for the attach joint moves the whole sub-graph.

state_solver_example.py

    subgraph = sub_scene_graph()
    root = scene_graph.getRoot()

    attach = Joint("attach_subgraph_joint")
    attach.parent_link_name = root
    attach.child_link_name = subgraph.getRoot()
    attach.type = JointType.FIXED

    prefix_attach = Joint(PREFIX + "attach_subgraph_joint")
    prefix_attach.parent_link_name = root
    prefix_attach.child_link_name = PREFIX + subgraph.getRoot()
    prefix_attach.type = JointType.FLOATING

    inserted = {}
    for attach_joint, prefix in [(attach, ""), (prefix_attach, PREFIX)]:
        edits.append(scene_graph.insertSceneGraph(subgraph, attach_joint, prefix))
        inserted[attach_joint.getName()] = solver.insertSceneGraph(subgraph, attach_joint, prefix)