Urdfpy

https://urdfpy.readthedocs.io/en/latest
Admin
Urdfpy is a pure-Python library for loading, manipulating, and exporting URDF files and robot...
Tokens:36,559
Snippets:114
Trust Score:8.3
Update:1 year ago
Show doc for...
Tokens:
Raw
### Install urdfpy using pip

Source: https://urdfpy.readthedocs.io/en/latest/install/index

This command installs the urdfpy package using pip, the standard package installer for Python. Ensure you have Python and pip installed on your system.

```Shell
pip install urdfpy
```

--------------------------------

### Install urdfpy in Development Mode

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Installs urdfpy and its development/documentation dependencies in editable mode. This involves cloning the repository and using pip to install the package with extra features.

```Shell
git clone https://github.com/mmatl/urdfpy.git
cd urdfpy
pip install -e .[dev,docs]
```

--------------------------------

### Install Git Pre-commit Hooks

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Installs git pre-commit hooks to automatically run code style checks (like flake8) before each commit, ensuring code quality and consistency.

```Shell
pre-commit install
```

--------------------------------

### Load URDF from File

Source: https://urdfpy.readthedocs.io/en/latest/examples/index

Loads a URDF file into the urdfpy library. Ensure that links are specified as relative or absolute paths, not ROS resource URLs.

```Python
from urdfpy import URDF
robot = URDF.load('tests/data/ur5/ur5.urdf')
```

--------------------------------

### Visualize Robot Configuration

Source: https://urdfpy.readthedocs.io/en/latest/examples/index

Provides functions to visualize the robot in a static pose or animate its movement over a trajectory. Requires specifying joint configurations or trajectories.

```Python
robot.show(cfg={
    'shoulder_lift_joint': -2.0,
    'elbow_joint': 2.0
})
```

```Python
robot.animate(cfg_trajectory={
    'shoulder_pan_joint' : [-np.pi / 4, np.pi / 4],
    'shoulder_lift_joint' : [0.0, -np.pi / 2.0],
    'elbow_joint' : [0.0, np.pi / 2.0]
})
```

--------------------------------

### Build Documentation with Sphinx

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Builds the project's documentation using Sphinx. This involves navigating to the docs directory and running the 'make' command with the 'html' target for HTML output.

```Shell
cd docs/
make html
```

```Shell
make clean
```

--------------------------------

### Perform Forward Kinematics

Source: https://urdfpy.readthedocs.io/en/latest/examples/index

Calculates the forward kinematics (FK) for robot links and visual/collision geometries. Supports specifying joint configurations to get poses for specific states.

```Python
fk = robot.link_fk()
print(fk[robot.links[0]])
print(fk[robot.links[1]])
fk = robot.link_fk(cfg={'shoulder_pan_joint' : 1.0})
print(fk[robot.links[1]])
```

```Python
fk = robot.visual_trimesh_fk()
print(type(list(fk.keys())[0]))
fk = robot.collision_trimesh_fk()
print(type(list(fk.keys())[0]))
```

--------------------------------

### Generate Code Coverage Reports

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Generates code coverage reports by running tests with the `--cov` option and then building an HTML visualization of the coverage results.

```Shell
pytest --cov=urdfpy tests
```

```Shell
coverage html
```

--------------------------------

### Save URDF to File

Source: https://urdfpy.readthedocs.io/en/latest/examples/index

Exports the current URDF configuration to a specified file path. Relative paths for meshes and images will be maintained relative to the new URDF file.

```Python
robot.save('/tmp/ur5/ur5.urdf')
```

--------------------------------

### Access URDF Links and Joints

Source: https://urdfpy.readthedocs.io/en/latest/examples/index

Iterates through and prints the names of all links and joints in the URDF. It also shows how to access parent-child relationships for joints and identify actuated joints and the base link.

```Python
for link in robot.links:
   print(link.name)
```

```Python
for joint in robot.joints:
   print('{} connects {} to {}'.format(
       joint.name, joint.parent, joint.child
   ))
```

```Python
for joint in robot.actuated_joints:
    print(joint.name)
```

```Python
print(robot.base_link.name)
```

--------------------------------

### Run Tests with pytest

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Executes the test suite using pytest. Includes options to control test execution, such as showing stdout, using a debugger, running only failed tests, or tracing test execution.

```Shell
pytest -v tests
```

```Shell
pytest -v tests/unit/utils/test_transforms.py
```

```Shell
pytest -v tests/unit/utils/test_images.py::test_rgbd_image
```

--------------------------------

### Run Code Style Checks with flake8

Source: https://urdfpy.readthedocs.io/en/latest/development/index

Executes the flake8 style checker on all files under version control to identify potential code problems. It can also be run on specific files or directories.

```Shell
pre-commit run --all-files
```

```Shell
flake8 --ignore=E231,W504 /path/to/files
```

--------------------------------

### Compute Visual Geometry Poses via FK (Python)

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Calculates the poses of URDF visual geometries using forward kinematics. It first computes the FK for the links containing the visual geometries and then applies the visual geometry's origin transform to get the final pose relative to the base link.

```Python
    def visual_geometry_fk(self, cfg=None, links=None):
        """Computes the poses of the URDF's visual geometries using fk.

        Parameters
        ----------
        cfg : dict
            A map from joints or joint names to configuration values for
            each joint. If not specified, all joints are assumed to be
            in their default configurations.
        links : list of str or list of :class:`.Link`
            The links or names of links to perform forward kinematics on.
            Only geometries from these links will be in the returned map.
            If not specified, all links are returned.

        Returns
        -------
        fk : dict
            A map from :class:`Geometry` objects that are part of the visual
            elements of the specified links to the 4x4 homogenous transform
            matrices that position them relative to the base link's frame.
        """
        lfk = self.link_fk(cfg=cfg, links=links)

        fk = {}
        for link in lfk:
            for visual in link.visuals:
                fk[visual.geometry] = lfk[link].dot(visual.origin)
        return fk
```

--------------------------------

### Configure Joint Properties in URDFpy

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Demonstrates how to set and get the 'joint', 'multiplier', and 'offset' properties for joint configurations within the URDFpy library. These properties are essential for defining joint behavior and limits.

```Python
class JointConfig:
    def __init__(self):
        self._joint = None
        self._multiplier = 1.0
        self._offset = 0.0

    @property
    def joint(self):
        """float : The name of the joint to mimic.
        """
        return self._joint

    @joint.setter
    def joint(self, value):
        self._joint = str(value)

    @property
    def multiplier(self):
        """float : The multiplier for the joint configuration.
        """
        return self._multiplier

    @multiplier.setter
    def multiplier(self, value):
        if value is not None:
            value = float(value)
        else:
            value = 1.0
        self._multiplier = value

    @property
    def offset(self):
        """float : The offset for the joint configuration
        """
        return self._offset

    @offset.setter
    def offset(self, value):
        if value is not None:
            value = float(value)
        else:
            value = 0.0
        self._offset = value
```

--------------------------------

### Get Joint Limit Configurations (Python)

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Retrieves the lower and upper bound joint limit configurations for actuated joints in a URDF model. It iterates through actuated joints, checks for defined limits, and stores them in separate dictionaries for lower and upper bounds.

```Python
    @property
    def joint_limit_cfgs(self):
        """tuple of dict : The lower-bound and upper-bound joint configuration
        maps.

        The first map is the lower-bound map, which maps limited joints to
        their lower joint limits.
        The second map is the upper-bound map, which maps limited joints to
        their upper joint limits.
        """
        lb = {}
        ub = {}
        for joint in self.actuated_joints:
            if joint.limit is not None:
                if joint.limit.lower is not None:
                    lb[joint] = joint.limit.lower
                if joint.limit.upper is not None:
                    ub[joint] = joint.limit.upper
        return (lb, ub)
```

--------------------------------

### Get child pose for a URDF joint

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Illustrates how to get the pose of a child link relative to a URDF joint using the `get_child_pose` method. This is useful for understanding the spatial relationship between connected links.

```python
# Assuming 'joint' is a URDF Joint object
# child_pose = joint.get_child_pose()
# print(f"Child pose: {child_pose}")
```

--------------------------------

### Load URDF file using urdfpy

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Demonstrates how to load a URDF file using the `load` static method from the `urdfpy.URDF` class. This is a fundamental step for parsing and interacting with URDF models.

```python
from urdfpy import URDF

# Load a URDF file
robot = URDF.load('path/to/your/robot.urdf')
```

--------------------------------

### Get URDF link information

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Demonstrates how to access information about links within a URDF model. The `links` attribute of a `URDF` object provides a list of all links.

```python
for link in robot.links:
    print(f"Link name: {link.name}")
```

--------------------------------

### Format URDF File Path

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/utils

Ensures a file path is correctly formatted for URDF loading, joining it with a base path if it's not absolute. Optionally creates necessary directories. Requires the `os` module.

```Python
import os

def get_filename(base_path, file_path, makedirs=False):
    """Formats a file path correctly for URDF loading.

    Parameters
    ----------
    base_path : str
        The base path to the URDF's folder.
    file_path : str
        The path to the file.
    makedirs : bool, optional
        If ``True``, the directories leading to the file will be created
        if needed.

    Returns
    -------
    resolved : str
        The resolved filepath -- just the normal ``file_path`` if it was an
        absolute path, otherwise that path joined to ``base_path``.
    """
    fn = file_path
    if not os.path.isabs(file_path):
        fn = os.path.join(base_path, file_path)
    if makedirs:
        d, _ = os.path.split(fn)
        if not os.path.exists(d):
            os.makedirs(d)
    return fn
```

--------------------------------

### Get URDF robot end links

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Retrieves the end links of a URDF robot model. End links are typically the links at the extremities of the robot's kinematic chains.

```python
end_links_names = [link.name for link in robot.end_links]
print(f"End links: {end_links_names}")
```

--------------------------------

### Load Meshes from File

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/utils

Loads triangular meshes from a specified file using the `trimesh` library. Handles cases where the file contains a single mesh, multiple meshes, or a scene, returning a list of `trimesh.Trimesh` objects. Raises errors for invalid file formats or empty mesh lists.

```Python
import trimesh

def load_meshes(filename):
    """Loads triangular meshes from a file.

    Parameters
    ----------
    filename : str
        Path to the mesh file.

    Returns
    -------
    meshes : list of :class:`~trimesh.base.Trimesh`
        The meshes loaded from the file.
    """
    meshes = trimesh.load(filename)

    # If we got a scene, dump the meshes
    if isinstance(meshes, trimesh.Scene):
        meshes = list(meshes.dump())
        meshes = [g for g in meshes if isinstance(g, trimesh.Trimesh)]

    if isinstance(meshes, (list, tuple, set)):
        meshes = list(meshes)
        if len(meshes) == 0:
            raise ValueError('At least one mesh must be pmeshesent in file')
        for r in meshes:
            if not isinstance(r, trimesh.Trimesh):
                raise TypeError('Could not load meshes from file')
    elif isinstance(meshes, trimesh.Trimesh):
        meshes = [meshes]
    else:
        raise ValueError('Unable to load mesh from file')

    return meshes
```

--------------------------------

### Collision Class in URDFpy

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Defines the collision properties of a link in a URDF. It includes methods for initializing, setting, and getting the geometry, name, and origin of the collision element, along with XML parsing and unparsing capabilities.

```Python
class Collision(URDFType):
    """Collision properties of a link.

    Parameters
    ----------
    geometry : :class:`.Geometry` 
        The geometry of the element
    name : str, optional
        The name of the collision geometry.
    origin : (4,4) float, optional
        The pose of the collision element relative to the link frame.
        Defaults to identity.
    """
    _ATTRIBS = {
        'name': (str, False)
    }
    _ELEMENTS = {
        'geometry': (Geometry, True, False),
    }
    _TAG = 'collision'

    def __init__(self, geometry, name=None, origin=None):
        self.geometry = geometry
        self.name = name
        self.origin = origin

    @property
    def geometry(self):
        """:class:`.Geometry` : The geometry of this element.
        """
        return self._geometry

    @geometry.setter
    def geometry(self, value):
        if not isinstance(value, Geometry):
            raise TypeError('Must set geometry with Geometry object')
        self._geometry = value

    @property
    def name(self):
        """str : The name of this collision element.
        """
        return self._name

    @name.setter
    def name(self, value):
        if value is not None:
            value = str(value)
        self._name = value

    @property
    def origin(self):
        """(4,4) float : The pose of this element relative to the link frame.
        """
        return self._origin

    @origin.setter
    def origin(self, value):
        self._origin = configure_origin(value)

    @classmethod
    def _from_xml(cls, node, path):
        kwargs = cls._parse(node, path)
        kwargs['origin'] = parse_origin(node)
        return Collision(**kwargs)

    def _to_xml(self, parent, path):
        node = self._unparse(path)
        node.append(unparse_origin(self.origin))
        return node
```

--------------------------------

### Visualize URDF Robot Configuration

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Displays a URDF robot model in a pyrender window. Users can specify a joint configuration and choose to visualize either the visual or collision geometry of the robot.

```Python
import pyrender

# Assuming 'self' refers to an object with methods like collision_trimesh_fk, visual_trimesh_fk

# Example usage:
# robot.show(cfg={'joint_name': value})
# robot.show(use_collision=True)

if use_collision:
    fk = self.collision_trimesh_fk(cfg=cfg)
else:
    fk = self.visual_trimesh_fk(cfg=cfg)

scene = pyrender.Scene()
for tm in fk:
    pose = fk[tm]
    mesh = pyrender.Mesh.from_trimesh(tm, smooth=False)
    scene.add(mesh, pose=pose)
pyrender.Viewer(scene, use_raymond_lighting=True)
```

--------------------------------

### Access URDF joint mimic settings

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Demonstrates how to access mimic joint settings in URDF. The `JointMimic` class and its attributes like `joint` and `multiplier` define how one joint's movement mimics another.

```python
# Assuming 'joint' is a URDF Joint object that mimics another
# if joint.mimic:
#     print(f"Mimic joint: {joint.mimic.joint}, Multiplier: {joint.mimic.multiplier}")
```

--------------------------------

### Get URDF robot base link

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Retrieves the base link of a URDF robot model loaded with `urdfpy`. The `base_link` attribute provides access to the root link of the robot's kinematic tree.

```python
base_link_name = robot.base_link.name
print(f"Base link: {base_link_name}")
```

--------------------------------

### Calculate collision mesh FK

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Demonstrates the `collision_mesh_fk` method for calculating the forward kinematics of a collision mesh associated with a URDF link.

```python
# Assuming 'robot' is a URDF object and 'link_name' is the name of a link with a collision mesh
# collision_mesh_pose = robot.collision_mesh_fk(link_name=link_name)
# print(f"Collision mesh pose: {collision_mesh_pose}")
```

--------------------------------

### URDF Initialization and Validation

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Initializes the URDF parser by processing links, joints, transmissions, and materials. It validates joints, transmissions, and the link graph, establishing the base and end links. Caches paths to the base link and reverse topological order for efficiency.

```Python
def __init__(self, filename):
        self._filename = filename
        self._links = []
        self._joints = []
        self._transmissions = []
        self._materials = []
        self._other_xml = ''

        # Parse the URDF file
        self._root = ET.parse(filename).getroot()

        # Process all elements
        self._link_map = {}
        self._joint_map = {}
        self._transmission_map = {}
        self._material_map = {}

        for x in self._root.findall('link'):
            link = Link(x)
            self._links.append(link)
            self._link_map[link.name] = link

        for x in self._root.findall('joint'):
            joint = Joint(x)
            self._joints.append(joint)
            self._joint_map[joint.name] = joint

        for x in self._root.findall('transmission'):
            transmission = Transmission(x)
            self._transmissions.append(transmission)
            self._transmission_map[transmission.name] = transmission

        for x in self._root.findall('material'):
            self._material_map[x.name] = x

        # Synchronize materials between links and top-level set
        self._merge_materials()

        # Validate the joints and transmissions
        self._actuated_joints = self._validate_joints()
        self._validate_transmissions()

        # Create the link graph and base link/end link sets
        self._G = nx.DiGraph()

        # Add all links
        for link in self.links:
            self._G.add_node(link)

        # Add all edges from CHILDREN TO PARENTS, with joints as their object
        for joint in self.joints:
            parent = self._link_map[joint.parent]
            child = self._link_map[joint.child]
            self._G.add_edge(child, parent, joint=joint)

        # Validate the graph and get the base and end links
        self._base_link, self._end_links = self._validate_graph()

        # Cache the paths to the base link
        self._paths_to_base = nx.shortest_path(
            self._G, target=self._base_link
        )

        # Cache the reverse topological order (useful for speeding up FK,
        # as we want to start at the base and work outward to cache
        # computation.
        self._reverse_topo = list(reversed(list(nx.topological_sort(self._G))))
```

--------------------------------

### Calculate forward kinematics for a URDF link

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Illustrates how to compute the forward kinematics (FK) for a specific link in a URDF model using the `link_fk` method. This requires specifying the parent link and the transformation.

```python
# Assuming 'parent_link' is a URDF Link object and 'transform' is a numpy array
# link_pose = robot.link_fk(link_name='target_link', parent_link=parent_link, transform=transform)
```

--------------------------------

### Python URDF Joint Validity Check

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

This snippet shows the `is_valid` method for the urdfpy Joint class. It checks if a given configuration value is valid for the joint. Currently, it returns True for 'fixed' and 'revolute' joint types, implying specific validation logic might be applied elsewhere or is simplified in this example.

```Python
    def is_valid(self, cfg):
        """Check if the provided configuration value is valid for this joint.

        Parameters
        ----------
        cfg : float, (2,) float, (6,) float, or (4,4) float
            The configuration of the joint.

        Returns
        -------
        is_valid : bool
            True if the configuration is valid, and False otherwise.
        """
        if self.joint_type not in ['fixed', 'revolute']:
            return True
```

--------------------------------

### Compute Visual Trimesh Poses via FK (Python)

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Computes the poses of URDF visual trimeshes using forward kinematics. This function is intended to calculate the transformation matrices for trimesh geometries associated with visual elements of URDF links.

```Python
    def visual_trimesh_fk(self, cfg=None, links=None):
        """Computes the poses of the URDF's visual trimeshes using fk.

        Parameters
        ----------
        
```

--------------------------------

### Compute Link Poses via Forward Kinematics (Python)

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Calculates the 4x4 homogeneous transform matrices for specified links relative to the base link's frame using forward kinematics. It supports custom joint configurations and allows filtering which links to compute FK for. The computation follows a reverse topological order of the URDF's kinematic tree.

```Python
    def link_fk(self, cfg=None, links=None):
        """Computes the poses of the URDF's links via forward kinematics.

        Parameters
        ----------
        cfg : dict
            A map from joints or joint names to configuration values for
            each joint. If not specified, all joints are assumed to be
            in their default configurations.
        links : list of str or list of :class:`.Link`
            The links or names of links to perform forward kinematics on.
            Only these links will be in the returned map. If not specified,
            all links are returned.

        Returns
        -------
        fk : dict
            A map from links to 4x4 homogenous transform matrices that
            position them relative to the base link's frame.
        """

        # Process config value
        joint_cfg = {}
        if cfg is None:
            cfg = {}
        else:
            for joint in cfg:
                if isinstance(joint, six.string_types):
                    joint_cfg[self._joint_map[joint]] = cfg[joint]
                elif isinstance(joint, Joint):
                    joint_cfg[joint] = cfg[joint]
                else:
                    raise TypeError('Got key of type {} in cfg map'
                                    .format(type(joint)))

        # Process link set
        link_set = set()
        if links is None:
            link_set = self.links
        else:
            for link in links:
                if isinstance(link, six.string_types):
                    link_set.add(self._link_map[link])
                elif isinstance(link, Link):
                    link_set.add(link)
                else:
                    raise TypeError('Got object of type {} in links list'
                                    .format(type(link)))

        # Compute forward kinematics in reverse topological order
        fk = {}
        for link in self._reverse_topo:
            if link not in link_set:
                continue
            pose = np.eye(4)
            path = self._paths_to_base[link]
            for i in range(len(path) - 1):
                child = path[i]
                parent = path[i + 1]
                joint = self._G.get_edge_data(child, parent)['joint']

                cfg = None
                if joint.mimic is not None:
                    mimic_joint = self._joint_map[joint.mimic.joint]
                    if mimic_joint in joint_cfg:
                        cfg = joint_cfg[mimic_joint]
                        cfg = joint.mimic.multiplier * cfg + joint.mimic.offset
                elif joint in joint_cfg:
                    cfg = joint_cfg[joint]
                pose = joint.get_child_pose(cfg).dot(pose)

                # Check existing FK to see if we can exit early
                if parent in fk:
                    pose = fk[parent].dot(pose)
                    break
            fk[link] = pose

        return fk
```

--------------------------------

### Configure URDF Origin Matrix

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/utils

Converts various input formats (None, 6-element array, or 4x4 matrix) into a 4x4 transformation matrix for URDF origins. Supports conversion from XYZ-RPY coordinates. Requires `numpy` and a helper function `xyz_rpy_to_matrix`.

```Python
import numpy as np

def configure_origin(value):
    """Convert a value into a 4x4 transform matrix.

    Parameters
    ----------
    value : None, (6,) float, or (4,4) float
        The value to turn into the matrix.
        If (6,), interpreted as xyzrpy coordinates.

    Returns
    -------
    matrix : (4,4) float or None
        The created matrix.
    """
    if value is None:
        value = np.eye(4)
    elif isinstance(value, (list, tuple, np.ndarray)):
        value = np.asanyarray(value).astype(np.float)
        if value.shape == (6,):
            value = xyz_rpy_to_matrix(value)
        elif value.shape != (4,4):
            raise ValueError('Origin must be specified as a 4x4 ' 
                             'homogenous transformation matrix')
    else:
        raise TypeError('Invalid type for origin, expect 4x4 matrix')
    return value
```

--------------------------------

### Calculate collision geometry FK

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Illustrates the `collision_geometry_fk` method for calculating the forward kinematics of a collision geometry within a URDF link.

```python
# Assuming 'robot' is a URDF object and 'link_name' is the name of a link with collision geometry
# collision_geom_pose = robot.collision_geometry_fk(link_name=link_name)
# print(f"Collision geometry pose: {collision_geom_pose}")
```

--------------------------------

### Create URDF Origin Element

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/utils

Creates an XML element for the URDF origin, setting 'xyz' and 'rpy' attributes from a transformation matrix. It requires the `xml.etree.ElementTree` module and a helper function `matrix_to_rpy`.

```Python
import xml.etree.ElementTree as ET

def create_origin(matrix):
    """Creates an origin element from a matrix.

    Parameters
    ----------
    matrix : (4,4) float
        The matrix to convert.

    Returns
    ------- 
    node : ET.Element
        The origin XML element.
    """
    node = ET.Element('origin')
    node.attrib['xyz'] = '{} {} {}'.format(*matrix[:3,3])
    node.attrib['rpy'] = '{} {} {}'.format(*matrix_to_rpy(matrix[:3,:3]))
    return node
```

--------------------------------

### URDF Link Initialization in urdfpy

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Initializes a URDF link object with its name and optional inertial, visual, and collision properties. Sets up internal attributes for meshes.

```Python
class Link(URDFType):
    """A link of a rigid object.

    Parameters
    ----------
    name : str
        The name of the link.
    inertial : :class:`.Inertial`, optional
        The inertial properties of the link.
    visuals : list of :class:`.Visual`, optional
        The visual properties of the link.
    collsions : list of :class:`.Collision`, optional
        The collision properties of the link.
    """

    _ATTRIBS = {
        'name': (str, True),
    }
    _ELEMENTS = {
        'inertial': (Inertial, False, False),
        'visuals': (Visual, False, True),
        'collisions': (Collision, False, True),
    }
    _TAG = 'link'

    def __init__(self, name, inertial, visuals, collisions):
        self.name = name
        self.inertial = inertial
        self.visuals = visuals
        self.collisions = collisions

        self._visual_meshes = None
        self._collision_mesh = None
```

--------------------------------

### Access URDF actuator data

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Shows how to access actuator information in URDF, which describes the hardware driving the joints. The `Actuator` class includes attributes like `name` and `hardwareInterface`.

```python
# Assuming 'actuator' is a URDF Actuator object
# print(f"Actuator name: {actuator.name}")
# print(f"Hardware interface: {actuator.hardwareInterface}")
```

--------------------------------

### Access URDF joint calibration data

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Demonstrates how to access calibration parameters for URDF joints. The `JointCalibration` class and its attributes like `rising` and `falling` provide information about joint calibration.

```python
# Assuming 'joint' is a URDF Joint object with calibration data
# if joint.calibration:
#     print(f"Joint calibration: Rising={joint.calibration.rising}, Falling={joint.calibration.falling}")
```

--------------------------------

### Access URDF safety controller settings

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Shows how to access safety controller parameters for URDF joints. The `SafetyController` class and its attributes like `k_position` and `k_velocity` define safety limits for joint control.

```python
# Assuming 'joint' is a URDF Joint object with safety controller data
# if joint.safety_controller:
#     print(f"Safety controller: Kp={joint.safety_controller.k_position}, Kv={joint.safety_controller.k_velocity}")
```

--------------------------------

### URDFpy Classes for URDF Elements

Source: https://urdfpy.readthedocs.io/en/latest/api/index

Provides Python classes to represent various components of a URDF file. These classes allow for programmatic creation and manipulation of robot descriptions.

```python
URDFType()
Box(size)
Cylinder(radius, length)
Sphere(radius)
Mesh(filename, scale=None, meshes=None)
Geometry(box=None, cylinder=None, sphere=None, mesh=None)
Texture(filename, image=None)
Material(name, color=None, texture=None)
Collision(name, origin, geometry)
Visual(geometry, name=None, origin=None, material=None)
Inertial(mass, inertia, origin=None)
JointCalibration(rising=None, falling=None)
JointDynamics(damping, friction)
JointLimit(effort, velocity, lower=None, upper=None)
JointMimic(joint, multiplier=None, offset=None)
SafetyController(k_velocity, k_position=None, k_effort=None, mode=None)
Actuator(name, mechanicalReduction=None, output=None, input=None)
TransmissionJoint(name, hardwareInterfaces)
Transmission(name, trans_type, joints=None, actuators=None)
Joint(name, joint_type, parent, child, origin=None, dynamics=None, limit=None, calibration=None, mimic=None, safety_controller=None)
Link(name, inertial, visuals=None, collisions=None)
URDF(name, links=None, joints=None, transmissions=None, materials=None, act_types=None, version='1.0')
```

--------------------------------

### Calculate Visual Geometry FK in Python

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Computes the poses of the URDF's visual trimeshes using forward kinematics. It maps trimesh objects from the specified links to their 4x4 homogenous transform matrices relative to the base link's frame. Dependencies include numpy for matrix operations and the URDFpy library's Link and Trimesh objects.

```python
def visual_trimesh_fk(self, cfg=None, links=None):
        """Computes the poses of the URDF's visual trimeshes using fk.

        Parameters
        ----------
        cfg : dict
            A map from joints or joint names to configuration values for
            each joint. If not specified, all joints are assumed to be
            in their default configurations.
        links : list of str or list of :class:`.Link`
            The links or names of links to perform forward kinematics on.
            Only trimeshes from these links will be in the returned map.
            If not specified, all links are returned.

        Returns
        -------
        fk : dict
            A map from :class:`~trimesh.base.Trimesh` objects that are
            part of the visual geometry of the specified links to the
            4x4 homogenous transform matrices that position them relative
            to the base link's frame.
        """
        lfk = self.link_fk(cfg=cfg, links=links)

        fk = {}
        for link in lfk:
            for visual in link.visuals:
                for mesh in visual.geometry.meshes:
                    pose = lfk[link].dot(visual.origin)
                    if visual.geometry.mesh is not None:
                        if visual.geometry.mesh.scale is not None:
                            S = np.eye(4)
                            S[:3,:3] = np.diag(visual.geometry.mesh.scale)
                            pose = pose.dot(S)
                    fk[mesh] = pose
        return fk
```

--------------------------------

### Link Properties and Collision Mesh Calculation (Python)

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

This snippet demonstrates how to manage properties of a URDF link, including inertial, visual, and collision elements. It also shows the calculation of a combined collision mesh from multiple collision objects, handling transformations and scaling.

```Python
    def name(self, value):
        self._name = str(value)

    @property
    def inertial(self):
        """`:class:`.Inertial` : Inertial properties of the link.
        """
        return self._inertial

    @inertial.setter
    def inertial(self, value):
        if value is not None and not isinstance(value, Inertial):
            raise TypeError('Expected Intertial object')
        self._inertial = value

    @property
    def visuals(self):
        """list of :class:`.Visual` : The visual properties of this link.
        """
        return self._visuals

    @visuals.setter
    def visuals(self, value):
        if value is None:
            value = []
        else:
            value = list(value)
            for v in value:
                if not isinstance(v, Visual):
                    raise ValueError('Expected list of Visual objects')
        self._visuals = value

    @property
    def collisions(self):
        """list of :class:`.Collision` : The collision properties of this link.
        """
        return self._collisions

    @collisions.setter
    def collisions(self, value):
        if value is None:
            value = []
        else:
            value = list(value)
            for v in value:
                if not isinstance(v, Collision):
                    raise ValueError('Expected list of Collision objects')
        self._collisions = value

    @property
    def collision_mesh(self):
        """`:class:`~trimesh.base.Trimesh` : A single collision mesh for
        the link, specified in the link frame, or None if there isn't one.
        """
        if len(self.collisions) == 0:
            return None
        if self._collision_mesh is None:
            meshes = []
            for c in self.collisions:
                for m in c.geometry.meshes:
                    m = m.copy()
                    pose = c.origin
                    if c.geometry.mesh is not None:
                        if c.geometry.mesh.scale is not None:
                            S = np.eye(4)
                            S[:3,:3] = np.diag(c.geometry.mesh.scale)
                            pose = pose.dot(S)
                    m.apply_transform(pose)
                    meshes.append(m)
            if len(meshes) == 0:
                return None
            self._collision_mesh = (meshes[0] + meshes[1:])
        return self._collision_mesh

```

--------------------------------

### Access URDF joint limits

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Shows how to access the joint limit configurations for a URDF model. The `joint_limit_cfgs` attribute provides details on the lower and upper bounds for joint positions, velocities, and efforts.

```python
for joint_name, limits in robot.joint_limit_cfgs.items():
    print(f"Joint: {joint_name}, Lower: {limits.lower}, Upper: {limits.upper}")
```

--------------------------------

### Calculate Collision Trimesh FK in Python

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Computes the poses of the URDF's collision trimeshes using forward kinematics. It maps trimesh objects from the collision geometry of specified links to their 4x4 homogenous transform matrices relative to the base link's frame. This function checks if a collision mesh exists for a link before assigning its pose.

```python
def collision_trimesh_fk(self, cfg=None, links=None):
        """Computes the poses of the URDF's collision trimeshes using fk.

        Parameters
        ----------
        cfg : dict
            A map from joints or joint names to configuration values for
            each joint. If not specified, all joints are assumed to be
            in their default configurations.
        links : list of str or list of :class:`.Link`
            The links or names of links to perform forward kinematics on.
            Only trimeshes from these links will be in the returned map.
            If not specified, all links are returned.

        Returns
        -------
        fk : dict
            A map from :class:`~trimesh.base.Trimesh` objects that are
            part of the collision geometry of the specified links to the
            4x4 homogenous transform matrices that position them relative
            to the base link's frame.
        """
        lfk = self.link_fk(cfg=cfg, links=links)

        fk = {}
        for link in lfk:
            if link.collision_mesh is not None:
                fk[link.collision_mesh] = lfk[link]
        return fk
```

--------------------------------

### Access URDF joint dynamics data

Source: https://urdfpy.readthedocs.io/en/latest/genindex

Shows how to access dynamic parameters for URDF joints. The `JointDynamics` class and its attributes like `damping` and `friction` define the physical properties of the joint's movement.

```python
# Assuming 'joint' is a URDF Joint object with dynamics data
# if joint.dynamics:
#     print(f"Joint dynamics: Damping={joint.dynamics.damping}, Friction={joint.dynamics.friction}")
```

--------------------------------

### Material Class in urdfpy

Source: https://urdfpy.readthedocs.io/en/latest/_modules/urdfpy/urdf

Represents a material for URDF geometry, including an optional color and texture. The Material class can be initialized with a name, color, and texture. It provides properties for name, color (RGBA, range [0,1]), and texture. The color setter validates the input shape and clips values, while the texture setter handles string paths or Texture objects.

```Python
import six
import PIL.Image
import numpy as np
import xml.etree.ElementTree as ET

class Material(URDFType):
    """A material for some geometry.

    Parameters
    ----------
    name : str
        The name of the material.
    color : (4,) float, optional
        The RGBA color of the material in the range [0,1].
    texture : :class:`.Texture`, optional
        A texture for the material.
    """
    _ATTRIBS = {
        'name': (str, True)
    }
    _ELEMENTS = {
        'texture': (Texture, False, False),
    }
    _TAG = 'material'

    def __init__(self, name, color=None, texture=None):
        self.name = name
        self.color = color
        self.texture = texture

    @property
    def name(self):
        """str : The name of the material.
        """
        return self._name

    @name.setter
    def name(self, value):
        self._name = str(value)

    @property
    def color(self):
        """(4,) float : The RGBA color of the material, in the range [0,1].
        """
        return self._color

    @color.setter
    def color(self, value):
        if value is not None:
            value = np.asanyarray(value).astype(np.float)
            value = np.clip(value, 0.0, 1.0)
            if value.shape != (4,):
                raise ValueError('Color must be a (4,) float')
        self._color = value

    @property
    def texture(self):
        """:class:`.Texture` : The texture for the material.
        """
        return self._texture

    @texture.setter
    def texture(self, value):
        if value is not None:
            if isinstance(value, six.string_types):
                image = PIL.Image.open(value)
                value = Texture(filename=value, image=image)
            elif not isinstance(value, Texture):
                raise ValueError('Invalid type for texture -- expect path to ' 
                                 'image or Texture')
        self._texture = value

    @classmethod
    def _from_xml(cls, node, path):
        kwargs = cls._parse(node, path)

        # Extract the color -- it's weirdly an attribute of a subelement
        color = node.find('color')
        if color is not None:
            color = np.fromstring(color.attrib['rgba'], sep=' ')
        kwargs['color'] = color

        return Material(**kwargs)

    def _to_xml(self, parent, path):
        # Simplify materials by collecting them at the top level.

        # For top-level elements, save the full material specification
        if parent.tag == 'robot':
            node = self._unparse(path)
            if self.color is not None:
                color = ET.Element('color')
                color.attrib['rgba'] = np.array2string(self.color)[1:-1]
                node.append(color)

        # For non-top-level elements just save the material with a name
        else:
            node = ET.Element('material')
            node.attrib['name'] = self.name
        return node
```
Tokens:
Raw