Skip to content

Kinematic Model ​

SUMMARY

Kinematic Model is the Pinocchio-backed model behind every Synapse manipulator, available as robot.kinematic_model. It answers forward-kinematics, joint-limit, frame and self-collision queries at a joint configuration, and it is the model that Planning Scene, Collision Checker, IK solvers and the Cartesian Interpolator take as input.

Use it for fast, low-level queries. Most programs can use the robot-level equivalents, which take degrees.

UNITS

Every joint input and output of the model is in radians, in model order. Poses are 4x4 homogeneous matrices in meters.

The robot-level methods, such as robot.forward_kinematics, use degrees and [x, y, z, rx, ry, rz] poses with Euler XYZ degrees. Convert with np.deg2rad and np.rad2deg when you mix the two.

The Skill ​

python
model = robot.kinematic_model
world_T_tcp = model.forward_kinematics(q_rad, robot.default_tcp)

The Code ​

python
"""
Queries a UR10e's kinematic model: forward kinematics, frames, joint limits and self-collision,
visualized in Rerun.

Supports Universal Robots (UR), Epson, and virtual. Runs offline, no hardware needed.

Usage:
    python kinematic_model.py
    python kinematic_model.py --save-rrd kinematic_model.rrd
"""

import argparse

import numpy as np
import rerun as rr
from loguru import logger

from telekinesis.synapse.robots.manipulators import universal_robots

Q_DEG = [0.0, -90.0, 90.0, 0.0, 90.0, 0.0]


def init_rerun(app_id: str, save_rrd: str | None) -> "rr.RecordingStream":
    """Start a Rerun recording, live in the viewer or saved to an .rrd file."""
    rr.init(app_id, spawn=save_rrd is None)
    recording = rr.get_global_data_recording()
    if save_rrd:
        recording.save(save_rrd)
    return recording


def log_frame(recording: "rr.RecordingStream", path: str, pose: np.ndarray) -> None:
    """Draw an XYZ axis triad at a 4 x 4 frame pose, in meters."""
    recording.log(path, rr.Transform3D(translation=pose[:3, 3], mat3x3=pose[:3, :3]), static=True)
    recording.log(
        f"{path}/axes",
        rr.Arrows3D(
            origins=np.zeros((3, 3)),
            vectors=np.eye(3) * 0.03,
            colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]],
        ),
        static=True,
    )


def main(save_rrd: str | None) -> None:
    """Read the model's structure and evaluate it at one joint configuration."""

    #===================== Create Robot ==========================================
    robot = universal_robots.UniversalRobotsUR10E(name='UR10e')
    model = robot.kinematic_model
    q_rad = np.deg2rad(Q_DEG)

    # ==================== Visualization ========================================
    # Move the robot to the configuration, then draw it in the same recording.
    recording = init_rerun("telekinesis_synapse_kinematic_model", save_rrd)
    recording.log("/", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
    robot.set_joint_positions(Q_DEG)
    robot.visualize_rerun(recording_stream=recording, live=False)

    # ==================== Run Skill ============================================
    # Structure. These are methods on the model, so call them.
    logger.info(f"nq={model.nq()}, ndof={model.ndof()}")
    logger.info(f"Joint names: {model.joint_names()}")
    logger.info(f"Frames: {len(model.frame_names())}")

    # Forward kinematics: a 4x4 pose of one frame, or of every frame in one call.
    world_T_tcp = model.forward_kinematics(q_rad, robot.default_tcp)
    logger.info(f"TCP position (m): {np.round(world_T_tcp[:3, 3], 4).tolist()}")
    transforms = model.all_frame_transforms(q_rad)
    logger.info(f"Frame transforms returned: {len(transforms)}")

    # Every frame of the model at this configuration, as an axis triad.
    for frame_name, world_T_frame in transforms.items():
        log_frame(recording, f"/kinematic_model/frames/{frame_name}", np.asarray(world_T_frame))

    # Joint limits, in radians.
    lower_deg = np.rad2deg(model.lower_position_limits())
    upper_deg = np.rad2deg(model.upper_position_limits())
    logger.info(f"Lower limits (deg): {np.round(lower_deg, 1).tolist()}")
    logger.info(f"Upper limits (deg): {np.round(upper_deg, 1).tolist()}")
    logger.info(f"Strictly within limits: {model.in_joint_limits(q_rad)}")

    # Self-collision and the neutral configuration.
    logger.info(f"Self-collision: {model.has_self_collision(q_rad)}")
    logger.info(f"Neutral configuration (rad): {model.neutral_configuration().tolist()}")

    # Add a frame at a fixed offset from the default TCP. robot.add_tcp adds the frame to this
    # model and records the TCP, so use it rather than calling model.add_frame yourself. The
    # robot is redrawn with the new frame.
    robot.add_tcp(name="probe", transform=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], set_active=False)
    logger.info(f"'probe' frame exists: {'probe' in model.frame_names()}")

    recording.flush()


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="Kinematic model Synapse example")
    parser.add_argument("--save-rrd", type=str, default=None,
                        help="Save the Rerun recording to this .rrd file instead of opening the viewer")
    args = parser.parse_args()

    main(save_rrd=args.save_rrd)

Rerun output ​

The viewer shows the UR10e at the configuration, with an axis triad on every frame of the kinematic model. Adding the probe TCP redraws the robot with the new frame. Pass --save-rrd kinematic_model.rrd to save the recording instead of opening the viewer, and open it later with rerun kinematic_model.rrd.

Running the Example ​

bash
python kinematic_model.py

To save the Rerun recording to a file instead of opening the viewer:

bash
python kinematic_model.py --save-rrd kinematic_model.rrd

For all options:

bash
python kinematic_model.py --help

Parameter Configuration ​

MethodParameterTypeDefaultDescription
forward_kinematicsq_radnp.ndarrayrequiredJoint configuration in radians.
frame_namestrrequiredFrame to locate. Raises if the frame does not exist.
all_frame_transformsq_radnp.ndarrayrequiredJoint configuration in radians.
in_joint_limitsq_radnp.ndarrayrequiredJoint configuration in radians.
has_self_collisionq_radnp.ndarrayrequiredJoint configuration in radians.
add_frameparent_frame_namestrrequiredExisting frame to attach to, for example "tool0".
new_frame_namestrrequiredName of the new frame. It must not already exist.
transformnp.ndarrayrequired4x4 homogeneous matrix of the new frame relative to the parent.
build_from_urdfurdf_pathstrrequiredPath to the robot's URDF.
package_dirstrrequiredDirectory that resolves package-relative mesh paths.
srdf_pathstr""SRDF declaring collision pairs to ignore. Empty keeps every pair.
collision_marginfloat0.005Security margin applied to all collision pairs, in meters.

Returns ​

MethodTypeDescription
forward_kinematics(q_rad, frame_name)np.ndarray4x4 pose of the frame in the model's world frame.
all_frame_transforms(q_rad)dict[str, np.ndarray]4x4 pose of every frame except universe, from one kinematics update. Cheaper than calling forward_kinematics per frame.
nq()intSize of a joint configuration vector.
ndof()intJoint count, excluding the universe joint.
joint_names()list[str]Ordered joint names, excluding universe.
frame_names()list[str]All frame names, excluding universe.
lower_position_limits(), upper_position_limits()np.ndarrayJoint limits in radians.
in_joint_limits(q_rad)boolTrue when every joint is strictly within its limits.
has_self_collision(q_rad)boolTrue when any active collision pair is in contact. Pairs removed through an SRDF are not considered.
neutral_configuration()np.ndarrayNeutral configuration in radians: zeros for revolute and prismatic joints.

Raises ​

ExceptionCondition
RuntimeErrorThe model is not built, the frame does not exist, or add_frame gets a missing parent or an already used name.

Method Reference ​

MemberDescription
build_from_urdf(urdf_path, package_dir, srdf_path="", collision_margin=0.005)Build the kinematic and collision model from a URDF and an optional SRDF. Must be called before any other method. Robots build their model for you, so you rarely call it.
forward_kinematics(q_rad, frame_name)Pose of one frame.
all_frame_transforms(q_rad)Poses of all frames in one update.
nq()Number of position variables.
ndof()Degrees of freedom.
joint_names()Ordered joint names.
frame_names()All frame names.
lower_position_limits()Lower joint limits in radians.
upper_position_limits()Upper joint limits in radians.
in_joint_limits(q_rad)Strict joint-limit check.
has_self_collision(q_rad)Self-collision check.
neutral_configuration()Neutral joint configuration.
add_frame(parent_frame_name, new_frame_name, transform)Add an operational frame such as a custom TCP. Reinitializes the model data so forward kinematics stays valid.

Robot-level equivalents ​

Robot memberUnitsModel member
robot.forward_kinematics(q)Degrees in, [x, y, z, rx, ry, rz] out. See Forward Kinematics.forward_kinematics
robot.inverse_kinematics(target_poses)Degrees. See Inverse Kinematics.IK solvers take the model
robot.joint_limitsDegrees, shape (ndof, 2).lower_position_limits, upper_position_limits
robot.ndof, robot.joint_names, robot.frame_namesProperties.ndof(), joint_names(), frame_names()
robot.add_tcp(name, transform)[x, y, z, rx, ry, rz], meters and degrees. See Add TCP.add_frame

Where to Use the Skill ​

  • Feeding native consumers - Pass robot.kinematic_model to a planning scene, a collision checker, an IK solver or the Cartesian interpolator.
  • Batch queries - Use all_frame_transforms when you need many frames at one configuration.
  • Fast limit and self-collision screening - Reject configurations before a heavier check.

When Not to Use the Skill ​

  • You work in degrees and [x, y, z, rx, ry, rz] poses - use robot.forward_kinematics and robot.inverse_kinematics instead.
  • You need collisions with obstacles - the model only knows the robot's own geometry. Use Planning Scene.
  • You need to add a TCP - use Add TCP, so the robot records it. After changing TCPs, call scene.update_kinematic_model(robot.kinematic_model) on any scene you already built.