Source code for simulation.utils.urdf.link

"""Class definitions making up the <link>.

Example:

    A simple link with the shape of a rectangular box
    can be defined as follows:

    >>> from kitcar_utils.geometry import Vector
    >>> from simulation.utils.urdf import (Geometry, Box, Link, Collision, Visual, Gazebo)
    >>> box = Geometry(Box(Vector(2, 2, 2)))
    >>> link = Link(
    ...     "link",
    ...     collision=Collision(geometry=box),
    ...     visual=Visual(geometry=box),
    ... )
    >>> # Calculate inertial tensor from kitcar_utils.geometry and mass
    >>> link.use_inertial_from_collision(mass=1.0)
    >>> # Add a gazebo material script
    >>> gazebo_material = Gazebo(
    ...     material="Gazebo/Gray", reference="link", inner=None
    ... )
"""

import xml.etree.ElementTree as ET
from dataclasses import dataclass, field
from typing import Tuple

from kitcar_utils.geometry import Vector

from .core import Attribute, Origin, XmlObject



[docs]def _origin_default_list():
    return [0, 0, 0]




[docs]@dataclass
class Color(XmlObject):
    TAG = "color"

    xyz: Attribute = field(default_factory=_origin_default_list)




[docs]@dataclass
class Inertia(XmlObject):
    TAG = "inertia"

    ixx: Attribute = 0
    ixy: Attribute = 0
    ixz: Attribute = 0
    iyx: Attribute = 0
    iyy: Attribute = 0
    iyz: Attribute = 0
    izx: Attribute = 0
    izy: Attribute = 0
    izz: Attribute = 0




[docs]@dataclass
class Inertial(XmlObject):
    TAG = "inertial"

    mass: float
    inertia: Inertia
    origin: Origin = None


[docs]    def create_xml(self, root_el=None):
        m = self.mass
        del self.mass
        el = super().create_xml(root_el)
        ET.SubElement(el, "mass", attrib={"value": str(m)})
        return el




[docs]@dataclass
class Geometry(XmlObject):
    TAG = "geometry"

    value: XmlObject




[docs]@dataclass
class Box(XmlObject):
    TAG = "box"

    size: Attribute


[docs]    def calculate_inertia(self, mass) -> Inertia:
        def rect_inertia(l1, l2):
            return 1 / 12 * mass * (l1 * l1 + l2 * l2)

        return Inertia(
            ixx=rect_inertia(self.size.y, self.size.z),
            iyy=rect_inertia(self.size.x, self.size.z),
            izz=rect_inertia(self.size.x, self.size.y),
        )




[docs]@dataclass
class Sphere(XmlObject):
    TAG = "sphere"

    radius: Attribute


[docs]    def calculate_inertia(self, mass) -> Inertia:
        ixx = 1 / 12 * mass * (3 * pow(self.radius, 2) + pow(self.radius, 2))
        return Inertia(ixx=ixx, iyy=ixx, izz=1 / 2 * mass * pow(self.radius, 2))




[docs]@dataclass
class Cylinder(XmlObject):
    TAG = "cylinder"

    radius: Attribute
    length: Attribute


[docs]    def calculate_inertia(self, mass) -> Inertia:
        ixx = 1 / 12 * mass * (3 * pow(self.radius, 2) + pow(self.length, 2))
        return Inertia(ixx=ixx, iyy=ixx, izz=1 / 2 * mass * pow(self.radius, 2))




[docs]@dataclass
class Mesh(XmlObject):
    TAG = "mesh"

    filename: Attribute
    scale: Vector = None




[docs]@dataclass
class Collision(XmlObject):
    TAG = "collision"

    origin: Origin = None
    geometry: Geometry = None




[docs]@dataclass
class Visual(XmlObject):
    TAG = "visual"

    origin: Tuple = None
    geometry: Geometry = None