gen_models

Kinematic model generation utilities.

This module provides functions and tools to generate the kinematic models for robots using the Denavit-Hartenberg. (DH) convention, kinematic models and gravity models using URDF robot definitions.

It includes functions to compute the DH transformation matrix, substitute sine and cosine functions in symbolic expressions, and generate Python scripts for the robot’s kinematics and Jacobian matrix and gravity model.

The models are generated symbolically using the sympy library, and the resulting models can be used for forward kinematics, Jacobian computation, gravity models, and analysis of robot motion.

robotblockset.gen_models.show_urdf(urdf_path: str | Path) → None

Lists URDF joint structure

Parameters:

urdf_path (Union[str, Path]) – Path to the URDF file.

Returns:

This function has no return value. It prints the URDF joint table.

Return type:

None

robotblockset.gen_models.show_urdf_tree(urdf_path: str | Path, start_link: str | None = None, max_depth: int | None = None) → None

Print a simple textual tree of a URDF model.

This helper parses the URDF XML and prints parent/child links connected by joints. It does not render geometry; it only shows the kinematic structure.

Parameters:

• urdf_path (Union[str, Path]) – Path to the URDF file.

• start_link (str, optional) – Root link name to start from. If None, the base link is inferred.

• max_depth (int, optional) – Maximum recursion depth to print. If None, prints the full tree.

Returns:

This function has no return value. It prints the URDF kinematic tree.

Return type:

None

robotblockset.gen_models.show_mjcf(xml_path: str | Path) → None

List the joint structure of an MJCF model.

Parameters:

xml_path (Union[str, Path]) – Path to the MJCF XML file.

Returns:

This function has no return value. It prints the MJCF joint table.

Return type:

None

robotblockset.gen_models.show_mjcf_tree(xml_path: str | Path, start_body: str | None = None, max_depth: int | None = None) → None

Print a simple textual tree of an MJCF model.

Parameters:

• xml_path (Union[str, Path]) – Path to the MJCF XML file.

• start_body (str, optional) – Root body to start from. If omitted, the first body under worldbody is used.

• max_depth (int, optional) – Maximum recursion depth to print. If omitted, prints the full tree.

Returns:

This function has no return value. It prints the MJCF body tree.

Return type:

None

robotblockset.gen_models.dh_transform(a: float, alpha: float, d: float, theta: float) → MutableDenseMatrix

Compute the DH transformation matrix.

The function generates the transformation matrix using Denavit-Hartenberg (DH) parameters.

Parameters:

• a (float) – The link length (a) from the previous joint to the current joint.

• alpha (float) – The link twist (alpha), which is the angle between consecutive z-axes.

• d (float) – The link offset (d), which is the distance along the previous z-axis to the common normal.

• theta (float) – The joint angle (theta), which is the angle about the previous z-axis.

Returns:

The DH transformation matrix (4x4).

Return type:

sp.Matrix

robotblockset.gen_models.subsincos(s: str, n: int) → str

Substitute sin and cos functions with shortcuts in a string.

This function replaces sine and cosine terms in a symbolic string with pre-defined shortcuts for efficiency in symbolic expression manipulation.

Parameters:

• s (str) – The input string containing sin and cos expressions.

• n (int) – The number of substitutions to perform based on joint angles and DH parameters.

Returns:

The modified string with sin and cos functions replaced by shortcuts.

Return type:

str

robotblockset.gen_models.gen_kinmodel_dh_all(number_of_joints: int, filename: str | None = None) → None

Generate a symbolic kinematic model for a robot with revolute joints only using Denavit-Hartenberg parameters and write it to a Python file.

This function constructs the forward kinematics and Jacobian of a serial robot manipulator using its Denavit-Hartenberg (DH) parameters. It then generates a Python function implementing these symbolic expressions for later numerical use.

Parameters:

• number_of_joints (int) – Number of joints.

• filename (str, optional) – Path to the output Python file. If not specified, defaults to ‘robot_models.py’.

Returns:

The function writes the symbolic kinematic model to a Python script and does not return a value.

Return type:

None

robotblockset.gen_models.gen_kinmodel_dh(robot: Dict[str, Any], filename: str | None = None) → None

Generate a symbolic kinematic model using Denavit-Hartenberg parameters and write it to a Python file.

This function constructs the forward kinematics and Jacobian of a serial robot manipulator using its Denavit-Hartenberg (DH) parameters. It then generates a Python function implementing these symbolic expressions for later numerical use.

Parameters:

• robot (Dict[str, Any]) –

  A dictionary defining the robot’s configuration with the following keys:

  • ’nj’int

    Number of joints.

  • ’a’list of float

    DH ‘a’ parameters (link lengths).

  • ’alpha’list of float

    DH ‘alpha’ parameters (link twists).

  • ’d’list of float

    DH ‘d’ parameters (link offsets).

  • ’theta’list of float, optional

    DH ‘theta’ parameters (joint angle offsets). If not provided, defaults to zeros.

  • ’name’str, optional

    Name of the robot (used in function name generation).

  • ’description’str, optional

    Description of the robot (used in docstring of the generated function).

• filename (str, optional) – Path to the output Python file. If not specified, defaults to ‘robot_models.py’.

Returns:

The function writes the symbolic kinematic model to a Python script and does not return a value.

Return type:

None

robotblockset.gen_models.symbolic_origin_matrix(origin_np: ndarray, pos_prefix: str = 'p', pos_index: int = 0, angle_prefix: str = 'a', angle_index: int = 0) → Tuple[MutableDenseMatrix, List[Tuple[Symbol, float]], int, List[Tuple[Symbol, float]], int]

Convert a numeric 4x4 transformation matrix into a symbolic transformation matrix.

This function analyzes the given transformation matrix to detect significant translation and rotation values. Nonzero translations and non-multiple-of-pi/2 rotations are replaced with symbolic variables. Useful for creating parameterized symbolic robot models from numeric transformation matrices.

Parameters:

• origin_np (HomogeneousMatrixType) – A 4x4 homogeneous transformation matrix (rotation and translation).

• pos_prefix (str, optional) – Prefix for generated symbolic position variables. Default is “p”.

• pos_index (int, optional) – Starting index for symbolic position variables. Default is 0.

• angle_prefix (str, optional) – Prefix for generated symbolic angle variables. Default is “a”.

• angle_index (int, optional) – Starting index for symbolic angle variables. Default is 0.

Returns:

• T_sym (sympy.Matrix) – The 4x4 symbolic transformation matrix.

• pos_vars (list of tuple) – List of (symbol, value) tuples for the position components that were symbolized.

• pos_index (int) – Final position variable index after processing.

• angle_vars (list of tuple) – List of (symbol, value) tuples for the angle components that were symbolized.

• angle_index (int) – Final angle variable index after processing.

robotblockset.gen_models.joint_transform(joint_type: str, q: Symbol | float) → MutableDenseMatrix

Return the homogeneous transformation matrix for a single joint based on its type and parameter.

This function generates a 4x4 symbolic transformation matrix for a joint transformation using either a rotation or translation along or about a principal axis, depending on the joint type. The transformation is symbolic when q is a sympy.Symbol.

Parameters:

• joint_type (str) –

  Type of the joint. Supported types are:

  • ”Rx”, “-Rx” : Rotation about the X-axis

  • ”Ry”, “-Ry” : Rotation about the Y-axis

  • ”Rz”, “-Rz”, “R”, “-R” : Rotation about the Z-axis

  • ”Px”, “-Px” : Translation along the X-axis

  • ”Py”, “-Py” : Translation along the Y-axis

  • ”Pz”, “-Pz”, “P”, “-P” : Translation along the Z-axis

  • ”F” : Fixed joint (identity transform)

• q (Union[sp.Symbol, float]) – The joint variable (angle or displacement). Typically a symbolic variable.

Returns:

T – A 4x4 symbolic homogeneous transformation matrix representing the joint motion.

Return type:

sp.Matrix

Raises:

ValueError – If the joint type is not recognized.

robotblockset.gen_models.get_joint_chain(robot: yourdfpy.URDF, start_link: str, end_link: str) → List[str]

Reconstruct the sequence of joint names forming the kinematic chain from start_link to end_link.

This function traverses the robot’s kinematic tree using yourdfpy’s internal maps to determine the joint path between two links.

Parameters:

• robot (URDF) – The robot model parsed with yourdfpy, containing joint and link information.

• start_link (str) – The name of the starting link in the kinematic chain.

• end_link (str) – The name of the target/end link in the kinematic chain.

Returns:

A list of joint names representing the ordered kinematic path from start_link to end_link.

Return type:

List[str]

Raises:

ValueError – If the path cannot be found (e.g., links are not connected or reachable).

robotblockset.gen_models.infer_joint_type(joint_type: str, axis: ndarray) → str

Infer symbolic joint type string (e.g., “Rx”, “Pz”, “F”) from URDF joint type and axis.

Parameters:

• joint_type (str) – The type of joint as defined in the URDF. Valid values include: - “revolute” - “continuous” - “prismatic” - “fixed”

• axis (Vector3DType) – A 3-element vector indicating the axis of motion or rotation.

Returns:

A symbolic joint type used in the kinematic model: - “Rx”, “Ry”, “Rz”, “-Rx”, etc. for revolute/continuous joints - “Px”, “Py”, “Pz”, “-Px”, etc. for prismatic joints - “F” for fixed joints

Return type:

str

Raises:

ValueError – If the joint type is unknown or the axis is unsupported.

robotblockset.gen_models.replace_trig_expressions(expr_str: str, nq: int, na: int) → str

Replace standard trigonometric expressions in a string with abbreviated variable names.

This function is typically used to simplify symbolic expressions for code generation by replacing cos(qi) with ci, sin(qi) with si, and similarly for angle parameters ai.

Parameters:

• expr_str (str) – The input expression as a string (e.g., symbolic expression converted to string).

• nq (int) – Number of joint variables q. Replaces cos(qi) and sin(qi) for i = 1 to nq.

• na (int) – Number of angle variables a. Replaces cos(ai) and sin(ai) for i = 1 to na.

Returns:

Modified string with trigonometric expressions replaced by shorthand notation.

Return type:

str

robotblockset.gen_models.compute_forward_kinematics(robot: yourdfpy.URDF, start_link: str, end_link: str) → Tuple[MutableDenseMatrix, MutableDenseMatrix, MutableDenseMatrix, MutableDenseMatrix, MutableDenseMatrix, List[Tuple[Symbol, float]], List[Tuple[Symbol, float]]]

Compute symbolic forward kinematics and Jacobians from start_link to end_link.

This function traverses the joint chain between two links in a URDF model (via yourdfpy), constructs symbolic transformation matrices, and computes the end-effector position, rotation, and Jacobians with respect to joint variables.

Parameters:

• robot (URDF) – A parsed URDF robot model using yourdfpy.

• start_link (str) – The base link of the kinematic chain.

• end_link (str) – The end-effector or target link.

Returns:

• p (sympy.Matrix (3x1)) – The symbolic position vector of the end-effector.

• R (sympy.Matrix (3x3)) – The symbolic rotation matrix of the end-effector.

• Jp (sympy.Matrix (3 x n)) – The positional Jacobian of the end-effector with respect to joint variables.

• Jr (sympy.Matrix (3 x n)) – The rotational Jacobian (joint axes) for revolute joints; zeros for prismatic.

• Q (sympy.Matrix (n x 1)) – Column vector of symbolic joint variables.

• a_vars (list of tuple) – List of (symbol, value) tuples for symbolic rotation angles introduced by symbolic_origin_matrix.

• p_vars (list of tuple) – List of (symbol, value) tuples for symbolic translations introduced by symbolic_origin_matrix.

Raises:

ValueError – If the joint chain cannot be found between the specified links.

robotblockset.gen_models.gen_kinmodel_urdf(robot_name: str, urdf_path: str, description: str | None = None, initial_link: str | None = None, final_link: str | None = None, prefix: str = '', filename: str | None = None) → None

Generate a Python script implementing the symbolic kinematic model of a robot using a URDF file.

This function parses the URDF model of a robot, symbolically computes forward kinematics and Jacobians, and writes the resulting symbolic expressions to a Python function in a script file. It supports optional TCP transforms and generates code ready for numerical evaluation.

Parameters:

• robot_name (str) – The name of the robot (used in generated function name).

• urdf_path (str) – Path to the URDF file defining the robot model.

• description (str, optional) – Description of the robot to include in the generated docstring. Defaults to the value of robot_name.

• initial_link (str, optional) – The base link from which to start the kinematic chain. Defaults to “world”.

• final_link (str, optional) – The target link to which forward kinematics are computed. Defaults to “<robot_name>_flange”.

• prefix (str, optional) – Optional prefix for the generated function name (e.g., for namespacing).

• filename (str, optional) – File where the kinematic model will be written. Defaults to “robot_models.py”.

Returns:

This function writes the output to a Python file and prints a success message.

Return type:

None

robotblockset.gen_models.compute_gravity_model(robot: yourdfpy.URDF, start_link: str, end_link: str, include_link_masses: bool = True) → Tuple[MutableDenseMatrix, List[Symbol], List[Tuple[Symbol, float]], List[Tuple[Symbol, float]], List[Tuple[Symbol, float]], List[Tuple[Symbol, float]]]

Compute the symbolic gravity load model of a robot from its URDF.

This function calculates the symbolic gravitational torque vector acting on each joint due to the link masses and an optional external load at the end-effector.

Parameters:

• robot (URDF) – The robot model parsed with yourdfpy.

• start_link (str) – The base link from which to begin the kinematic chain.

• end_link (str) – The end-effector or terminal link of the chain.

• include_link_masses (bool, optional) – If True, include the robot link masses in the gravity model. If False, only the external load at the end-effector is included.

Returns:

• grav (sympy.Matrix (n x 1)) – Symbolic gravity torque vector.

• Q (list of sympy.Symbol) – List of symbolic joint variables.

• a_vars (list of tuple) – List of (symbol, value) tuples for rotation angle parameters from link origins.

• p_vars (list of tuple) – List of (symbol, value) tuples for translation parameters from link origins.

• Lc_vars (list of tuple) – List of (symbol, value) tuples for center-of-mass offset positions.

• mass_vars (list of tuple) – List of (symbol, value) tuples for link masses used in the gravity model.

Notes

• Uses symbolic variables for link masses and COM locations if they are nonzero.

• Adds external load torque at the end-effector using symbolic mass mLoad and symbolic COM mCOM.

• Only links with inertial information are included in the computation.

robotblockset.gen_models.gen_gravmodel_urdf(robot_name: str, urdf_path: str, description: str | None = None, initial_link: str | None = None, final_link: str | None = None, prefix: str = '', filename: str | None = None) → None

Generate a Python function that computes symbolic gravity torques for a robot defined in a URDF.

This function parses the URDF of a robot, computes symbolic gravitational torques for all joints, and generates a Python function that can evaluate these torques numerically given joint positions, load mass, and load center-of-mass.

Parameters:

• robot_name (str) – Base name of the robot (used in function naming).

• urdf_path (str) – Path to the URDF file describing the robot.

• description (str, optional) – Descriptive text for the robot used in docstring. Defaults to robot_name.

• initial_link (str, optional) – Base link for the gravity model computation. Defaults to “world”.

• final_link (str, optional) – End-effector link. Defaults to ‘<robot_name>_flange’.

• prefix (str, optional) – Optional prefix for the generated function name.

• filename (str, optional) – Output filename to write the generated Python model. Defaults to ‘robot_models.py’.

Returns:

This function writes the gravity model function into a file and prints a confirmation message.

Return type:

None

robotblockset.gen_models.gen_kinmodel_mjcf(robot_name: str, xml_path: str, description: str | None = None, initial_link: str | None = None, final_link: str | None = None, prefix: str = '', filename: str | None = None) → None

Generate a Python script implementing the symbolic kinematic model of a robot using an MJCF file.

Parameters:

• robot_name (str) – The name of the robot (used in generated function name).

• xml_path (str) – Path to the MJCF XML file defining the robot model.

• description (str, optional) – Description of the robot to include in the generated docstring.

• initial_link (str, optional) – The base link from which to start the kinematic chain. Defaults to “world”.

• final_link (str, optional) – The target link to which forward kinematics are computed.

• prefix (str, optional) – Optional prefix for the generated function name.

• filename (str, optional) – File where the kinematic model will be written. Defaults to “robot_models.py”.

Returns:

This function writes the kinematic model into a Python file and prints a confirmation message.

Return type:

None

robotblockset.gen_models.gen_gravmodel_mjcf(robot_name: str, xml_path: str, description: str | None = None, initial_link: str | None = None, final_link: str | None = None, prefix: str = '', filename: str | None = None, load_only: bool = False) → None

Generate a Python function that computes symbolic gravity torques for a robot defined in an MJCF file.

Parameters:

• robot_name (str) – Base name of the robot (used in function naming).

• xml_path (str) – Path to the MJCF XML file describing the robot.

• description (str, optional) – Descriptive text for the robot used in docstring.

• initial_link (str, optional) – Base link for the gravity model computation. Defaults to “world”.

• final_link (str, optional) – End-effector link.

• prefix (str, optional) – Optional prefix for the generated function name.

• filename (str, optional) – Output filename to write the generated Python model. Defaults to “robot_models.py”.

• load_only (bool, optional) – and ignores the masses of the robot links.

Returns:

This function writes the gravity model into a Python file and prints a confirmation message.

Return type:

None

Functions

compute_forward_kinematics(robot, ...)	Compute symbolic forward kinematics and Jacobians from start_link to end_link.
compute_gravity_model(robot, start_link, ...)	Compute the symbolic gravity load model of a robot from its URDF.
dh_transform(a, alpha, d, theta)	Compute the DH transformation matrix.
gen_gravmodel_mjcf(robot_name, xml_path[, ...])	Generate a Python function that computes symbolic gravity torques for a robot defined in an MJCF file.
gen_gravmodel_urdf(robot_name, urdf_path[, ...])	Generate a Python function that computes symbolic gravity torques for a robot defined in a URDF.
gen_kinmodel_dh(robot[, filename])	Generate a symbolic kinematic model using Denavit-Hartenberg parameters and write it to a Python file.
gen_kinmodel_dh_all(number_of_joints[, filename])	Generate a symbolic kinematic model for a robot with revolute joints only using Denavit-Hartenberg parameters and write it to a Python file.
gen_kinmodel_mjcf(robot_name, xml_path[, ...])	Generate a Python script implementing the symbolic kinematic model of a robot using an MJCF file.
gen_kinmodel_urdf(robot_name, urdf_path[, ...])	Generate a Python script implementing the symbolic kinematic model of a robot using a URDF file.
get_joint_chain(robot, start_link, end_link)	Reconstruct the sequence of joint names forming the kinematic chain from start_link to end_link.
infer_joint_type(joint_type, axis)	Infer symbolic joint type string (e.g., "Rx", "Pz", "F") from URDF joint type and axis.
joint_transform(joint_type, q)	Return the homogeneous transformation matrix for a single joint based on its type and parameter.
replace_trig_expressions(expr_str, nq, na)	Replace standard trigonometric expressions in a string with abbreviated variable names.
show_mjcf(xml_path)	List the joint structure of an MJCF model.
show_mjcf_tree(xml_path[, start_body, max_depth])	Print a simple textual tree of an MJCF model.
show_urdf(urdf_path)	Lists URDF joint structure
show_urdf_tree(urdf_path[, start_link, ...])	Print a simple textual tree of a URDF model.
subsincos(s, n)	Substitute sin and cos functions with shortcuts in a string.
symbolic_origin_matrix(origin_np[, ...])	Convert a numeric 4x4 transformation matrix into a symbolic transformation matrix.