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
model = robot.kinematic_model
world_T_tcp = model.forward_kinematics(q_rad, robot.default_tcp)The Code
"""
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
python kinematic_model.pyTo save the Rerun recording to a file instead of opening the viewer:
python kinematic_model.py --save-rrd kinematic_model.rrdFor all options:
python kinematic_model.py --helpParameter Configuration
| Method | Parameter | Type | Default | Description |
|---|---|---|---|---|
forward_kinematics | q_rad | np.ndarray | required | Joint configuration in radians. |
frame_name | str | required | Frame to locate. Raises if the frame does not exist. | |
all_frame_transforms | q_rad | np.ndarray | required | Joint configuration in radians. |
in_joint_limits | q_rad | np.ndarray | required | Joint configuration in radians. |
has_self_collision | q_rad | np.ndarray | required | Joint configuration in radians. |
add_frame | parent_frame_name | str | required | Existing frame to attach to, for example "tool0". |
new_frame_name | str | required | Name of the new frame. It must not already exist. | |
transform | np.ndarray | required | 4x4 homogeneous matrix of the new frame relative to the parent. | |
build_from_urdf | urdf_path | str | required | Path to the robot's URDF. |
package_dir | str | required | Directory that resolves package-relative mesh paths. | |
srdf_path | str | "" | SRDF declaring collision pairs to ignore. Empty keeps every pair. | |
collision_margin | float | 0.005 | Security margin applied to all collision pairs, in meters. |
Returns
| Method | Type | Description |
|---|---|---|
forward_kinematics(q_rad, frame_name) | np.ndarray | 4x4 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() | int | Size of a joint configuration vector. |
ndof() | int | Joint 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.ndarray | Joint limits in radians. |
in_joint_limits(q_rad) | bool | True when every joint is strictly within its limits. |
has_self_collision(q_rad) | bool | True when any active collision pair is in contact. Pairs removed through an SRDF are not considered. |
neutral_configuration() | np.ndarray | Neutral configuration in radians: zeros for revolute and prismatic joints. |
Raises
| Exception | Condition |
|---|---|
RuntimeError | The model is not built, the frame does not exist, or add_frame gets a missing parent or an already used name. |
Method Reference
| Member | Description |
|---|---|
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 member | Units | Model 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_limits | Degrees, shape (ndof, 2). | lower_position_limits, upper_position_limits |
robot.ndof, robot.joint_names, robot.frame_names | Properties. | 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_modelto a planning scene, a collision checker, an IK solver or the Cartesian interpolator. - Batch queries - Use
all_frame_transformswhen 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 - userobot.forward_kinematicsandrobot.inverse_kinematicsinstead. - 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.