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 simulation.utils.geometry import Vector
    >>> from simulation.utils.urdf import Geometry, Box, Material, Link, Collision, Visual
    >>> box = Geometry(Box(Vector(2, 2, 2)))
    >>> material = Material("mat", color=Vector(1, 1, 1))
    >>> link = Link(
    ...     "link",
    ...     collision=Collision(geometry=box),
    ...     visual=Visual(geometry=box, material=material),
    ... )
    >>> link.use_inertial_from_collision(mass=1.0)
    ...  # Calculate inertial tensor from simulation.utils.geometry and mass
"""

import xml.etree.cElementTree as cET
from dataclasses import dataclass, field
from typing import Tuple

from simulation.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)
        cET.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 Material(XmlObject):
    TAG = "material"

    name: Attribute = None
    color: Vector = None
    texture: str = None




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

    origin: Tuple = None
    geometry: Geometry = None
    material: Material = None