Exemplo n.º 1
0
 def __init__(self, name, color, **kwargs):
     super(Highlight, self).__init__(name, static=True, **kwargs)
     self.highlight = Link("hl_link")
     self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False)
     r, g, b, a = color
     self.highlight.make_color(r, g, b, a)
     self.add_element(self.highlight)
Exemplo n.º 2
0
class Wall(Model):
    """
    Simple wall model to wall off the
    """

    def __init__(self, name, start, end, thickness, height, **kwargs):
        """
        Construct a wall of the given thickness and height from
        `start` to `end`. This simply results in a box.

        :param start: Starting point of the wall.
        :type start: Vector3
        :param end: Ending point of the wall.
        :type end: Vector3
        :return:
        """
        super(Wall, self).__init__(name, static=True, **kwargs)
        assert start.z == end.z, "Walls with different start / end z are undefined."

        center = 0.5 * (end + start)
        diff = end - start
        size = abs(diff)
        self.wall = Link("wall_link")
        self.wall.make_box(10e10, size, thickness, height)

        # Rotate the wall so it aligns with the vector from
        # x to y
        self.align(
            Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1),
            center, diff, Vector3(0, 0, 1), Posable("mock")
        )

        self.add_element(self.wall)
Exemplo n.º 3
0
    def __init__(self, name, start, end, thickness, height, **kwargs):
        """
        Construct a wall of the given thickness and height from
        `start` to `end`. This simply results in a box.

        :param start: Starting point of the wall.
        :type start: Vector3
        :param end: Ending point of the wall.
        :type end: Vector3
        :return:
        """
        super(Wall, self).__init__(name, static=True, **kwargs)
        assert start.z == end.z, "Walls with different start / end z are undefined."

        center = 0.5 * (end + start)
        diff = end - start
        size = abs(diff)
        self.wall = Link("wall_link")
        self.wall.make_box(10e10, size, thickness, height)

        # Rotate the wall so it aligns with the vector from
        # x to y
        self.align(
            Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1),
            center, diff, Vector3(0, 0, 1), Posable("mock")
        )

        self.add_element(self.wall)
Exemplo n.º 4
0
class Highlight(Model):
    """
    Model to highlight newly inserted robots / selected parents
    """
    def __init__(self, name, color, **kwargs):
        super(Highlight, self).__init__(name, static=True, **kwargs)
        self.highlight = Link("hl_link")
        self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False)
        r, g, b, a = color
        self.highlight.make_color(r, g, b, a)
        self.add_element(self.highlight)
Exemplo n.º 5
0
class Highlight(Model):
    """
    Model to highlight newly inserted robots / selected parents
    """

    def __init__(self, name, color, **kwargs):
        super(Highlight, self).__init__(name, static=True, **kwargs)
        self.highlight = Link("hl_link")
        self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False)
        r, g, b, a = color
        self.highlight.make_color(r, g, b, a)
        self.add_element(self.highlight)
Exemplo n.º 6
0
class BirthClinic(Model):
    """
    Birth clinic model, consists of two cylinders, one of which is rotated.
    """

    def __init__(self, diameter=3.0, height=3.0, name="birth_clinic"):
        """

        :param diameter: Intended diameter of the birth clinic
        :param name:
        :return:
        """
        super(BirthClinic, self).__init__(name=name, static=True)

        self.diameter = diameter
        scale = diameter / MESH_DIAMETER

        # Cannot go higher than mesh height, or lower than the bottom
        # of the slice.
        scaled_height = scale * MESH_HEIGHT
        self.height = max(min(height, scaled_height), SLICE_FRACTION * scaled_height)

        mesh = Mesh("model://tol_robot/meshes/BirthClinic.dae", scale=scale)

        col = Collision("bc_col", mesh)
        surf = Element(tag_name="surface")
        friction = Friction(
            friction=0.01,
            friction2=0.01,
            slip1=1.0,
            slip2=1.0
        )
        contact = "<contact>" \
                  "<ode>" \
                  "<kd>%s</kd>" \
                  "<kp>%s</kp>" \
                  "</ode>" \
                  "</contact>" % (
                      nf(constants.SURFACE_KD), nf(constants.SURFACE_KP)
                  )
        surf.add_element(friction)
        surf.add_element(contact)
        col.add_element(surf)

        vis = Visual("bc_vis", mesh.copy())
        self.link = Link("bc_link", elements=[col, vis])

        # By default the model has 0.5 * scale * MESH_HEIGHT below
        # and above the surface. Translate such that we have exactly
        # the desired height instead.
        self.link.translate(Vector3(0, 0, self.height - (0.5 * scaled_height)))
        self.add_element(self.link)
Exemplo n.º 7
0
    def _build_analyzer_body(self, model, body_parts, connections):
        """
        Builds a model from body parts in analyzer mode - i.e.
        one link per body part to allow collision detection.
        :param model:
        :param body_parts:
        :type body_parts: list[BodyPart]
        :return:
        """
        part_map = {}
        for body_part in body_parts:
            link = Link("link_"+body_part.id, self_collide=True)
            link.set_position(body_part.get_position())
            link.set_rotation(body_part.get_rotation())
            body_part.set_position(Vector3())
            body_part.set_rotation(Quaternion())
            link.add_element(body_part)
            model.add_element(link)
            part_map[body_part.id] = link

        # Create fake joints for all connections so they will
        # not trigger collisions.
        for a, b in connections:
            joint = FixedJoint(part_map[a], part_map[b])
            model.add_element(joint)
Exemplo n.º 8
0
 def __init__(self, name, color, **kwargs):
     super(Highlight, self).__init__(name, static=True, **kwargs)
     self.highlight = Link("hl_link")
     self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False)
     r, g, b, a = color
     self.highlight.make_color(r, g, b, a)
     self.add_element(self.highlight)
Exemplo n.º 9
0
    def _build_analyzer_body(self, model, body_parts, connections):
        """
        Builds a model from body parts in analyzer mode - i.e.
        one link per body part to allow collision detection.
        :param model:
        :param body_parts:
        :type body_parts: list[BodyPart]
        :return:
        """
        part_map = {}
        for body_part in body_parts:
            link = Link("link_"+body_part.id, self_collide=True)
            link.set_position(body_part.get_position())
            link.set_rotation(body_part.get_rotation())
            body_part.set_position(Vector3())
            body_part.set_rotation(Quaternion())
            link.add_element(body_part)
            model.add_element(link)
            part_map[body_part.id] = link

        # Create fake joints for all connections so they will
        # not trigger collisions.
        for a, b in connections:
            joint = FixedJoint(part_map[a], part_map[b])
            model.add_element(joint)
Exemplo n.º 10
0
    def test_link_inertia(self):
        """
        A similar test to what is done in structure.py to test
        compound inertia, only now we do it in a link.
        :return:
        """
        # First, the comparison inertial
        total_mass = 100
        simple_box = Box(4, 8, 12, mass=total_mass)
        i1 = simple_box.get_inertial()

        # Same split as in structure.py test, only now
        # we're making sure it is distributed around the
        # origin since box inertial above is relative to
        # its center of mass
        link = Link("test_link")
        y_axis = Vector3(0, 1, 0)
        deg90 = 0.5 * math.pi

        # Split 5 / 7 in z-direction
        frac = total_mass / 12.0
        total_up = 5 * frac
        total_down = 7 * frac

        # We split the upper part 6 / 2 in the y-direction
        # and align in the center.
        frac_up = total_up / 8.0
        sub1 = Collision("sub1", geometry=Box(5, 6, 4, mass=6 * frac_up))
        sub1.rotate_around(y_axis, deg90)
        sub1.translate(Vector3(0, 1, 3.5))
        link.add_element(sub1)

        sub2 = Collision("sub2", geometry=Box(5, 2, 4, mass=2 * frac_up))
        sub2.rotate_around(y_axis, deg90)
        sub2.translate(Vector3(0, -3, 3.5))
        link.add_element(sub2)

        sub3 = Collision("sub3", geometry=Box(4, 8, 7, mass=total_down))
        sub3.translate(Vector3(0, 0, -2.5))
        link.add_element(sub3)

        link.calculate_inertial()
        self.assertEqualTensors(i1, link.inertial)
Exemplo n.º 11
0
    def test_direction_conversion(self):
        """
        Tests converting vectors between direction frames.
        :return:
        """
        link = Link("my_link")
        point = Vector3(0, 0, 1)

        # At this point, it should be the same in the
        # parent direction
        x, y, z = link.to_parent_direction(point)
        self.assertAlmostEqual(x, point.x)
        self.assertAlmostEqual(y, point.y)
        self.assertAlmostEqual(z, point.z)

        # Now rotate the link 90 degrees over (1, 1, 0),
        # this should cause the local vector (0, 1, 1)
        # to land at 0.5 * [sqrt(2), -sqrt(2), 0]
        link.rotate_around(Vector3(1, 1, 0), 0.5 * pi, relative_to_child=False)
        hs2 = 0.5 * sqrt(2)
        x, y, z = link.to_parent_direction(point)
        self.assertAlmostEqual(x, hs2)
        self.assertAlmostEqual(y, -hs2)
        self.assertAlmostEqual(z, 0)
Exemplo n.º 12
0
def gen_boxes(dimensions=4, spacing=0.5, size=0.05):
        for x in range(-dimensions,dimensions+1):
            for y in range(-dimensions,dimensions+1):
                l = Link("box")

                #box_geom = Box(1.0, 1.0, 1.0, mass=0.5)
                #b = StructureCombination("box", box_geom)
                #l.add_element(b)

                if (x!=0 or y!=0):
                    l.make_box(1, size, size, 0.01*max(abs(x),abs(y)))

                pos = Vector3(spacing*x,spacing*y,0)

                l.set_position(pos)
                l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False)

                model.add_element(l)
Exemplo n.º 13
0
def gen_boxes(model_name,
              dimensions=4,
              spacing=0.5,
              size=0.05,
              height=0.015,
              center_sq=1):
    model = Model(model_name, static=True)
    for x in range(-dimensions, dimensions + 1):
        for y in range(-dimensions, dimensions + 1):
            l = Link("box")
            if (abs(x) >= center_sq or abs(y) >= center_sq):
                l.make_box(1, size, size, height)
            pos = Vector3(spacing * x, spacing * y, height / 2)
            l.set_position(pos)
            #l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False)
            model.add_element(l)
    return model
Exemplo n.º 14
0
def gen_steps(model_name,
              num_steps=6,
              offset=0.4,
              height=0.02,
              width=3.0,
              depth=0.2,
              incline=0):
    model = Model(model_name, static=True)
    steps = PosableGroup()
    for x in range(0, num_steps):
        l = Link("box")
        l.make_box(1, depth, width, height)
        pos = Vector3(offset + depth * x, 0, height / 2 + x * height)
        l.set_position(pos)
        steps.add_element(l)

    for x in range(0, 4):
        steps.set_rotation(
            Quaternion.from_rpy(0, math.radians(-incline),
                                math.radians(90 * x)))
        model.add_element(steps.copy())

    return model
Exemplo n.º 15
0
    def test_direction_conversion(self):
        """
        Tests converting vectors between direction frames.
        :return:
        """
        link = Link("my_link")
        point = Vector3(0, 0, 1)

        # At this point, it should be the same in the
        # parent direction
        x, y, z = link.to_parent_direction(point)
        self.assertAlmostEqual(x, point.x)
        self.assertAlmostEqual(y, point.y)
        self.assertAlmostEqual(z, point.z)

        # Now rotate the link 90 degrees over (1, 1, 0),
        # this should cause the local vector (0, 1, 1)
        # to land at 0.5 * [sqrt(2), -sqrt(2), 0]
        link.rotate_around(Vector3(1, 1, 0), 0.5 * pi, relative_to_child=False)
        hs2 = 0.5 * sqrt(2)
        x, y, z = link.to_parent_direction(point)
        self.assertAlmostEqual(x, hs2)
        self.assertAlmostEqual(y, -hs2)
        self.assertAlmostEqual(z, 0)
Exemplo n.º 16
0
from sdfbuilder import Link, PosableGroup, SDF, Model
from sdfbuilder.math import Vector3
from math import pi

link = Link("my_link")

minibox = Link("my_minibox")

# Link one is a vertical box
link.make_box(1.0, 2, 2, 4)

# Minibox is... well, a mini box
minibox.make_box(0.1, 0.2, 0.2, 0.2)

minibox.align(
    # Bottom left corner of minibox
    Vector3(-0.1, -0.1, -0.1),

    # Normal vector
    Vector3(-0.1, -0.1, -0.1),

    # Tangent vector
    Vector3(-0.1, -0.1, 0.2),

    # Top left of link
    Vector3(-1, -1, 2),

    # Normal vector
    Vector3(0, 0, 1),

    # Tangent vector
Exemplo n.º 17
0
    # .. align with the top ..
    Vector3(0, 0, 0.5 * box_geom.size[2]),

    # .. and normal vector (other direction) ..
    Vector3(0, 0, 1),

    # .. rotation of the tangent vector ..
    Vector3(0, 1, 0),

    # .. of the box.
    box
)

# Now, create a link and add both items to it
link = Link("my_link", elements=[box, cylinder])

# Calculate the correct inertial properties given
# the collision elements.
link.align_center_of_mass()
link.calculate_inertial()

# Rotate the link 45 degrees around the x-axis, specified in the parent frame
# just to demonstrate how that works (and to demonstrate align is still
# going to work after the rotation).
link.rotate_around(Vector3(1, 0, 0), math.radians(45), relative_to_child=False)

# Okay, not sure what this is supposed to be, but let's another wheel-like cylinder in
# a new link, and connect them with joints
wheel_geom = Cylinder(0.75, 0.1, mass=0.1)
wheel = StructureCombination("wheel", wheel_geom)
Exemplo n.º 18
0
    def _build_body(self, traversed, model, plugin, component, link=None, child_joints=None):
        """
        :param traversed: Set of components that were already traversed,
                          prevents loops (since all connections are two-sided).
        :type traversed: set
        :param model:
        :param plugin:
        :param component:
        :type component: Component
        :param link:
        :type link: Link
        :return: The primary link used in this traversal, i.e. the link
                 passed to it or the first link created.
        """
        if component in traversed:
            return link

        create_link = link is None
        traversed.add(component)
        if create_link:
            # New tree, create a link. The component name
            # should contain the body part ID so this name should
            # be unique.
            # The position of the link is determined later when we decide
            # its center of mass.
            link = Link("link_%s" % component.name, self_collide=True)
            child_joints = []
            model.add_element(link)

        # Add this component to the link.
        position = link.to_local_frame(component.get_position())
        rotation = link.to_local_direction(component.get_rotation())
        component.set_position(position)
        component.set_rotation(rotation)
        link.add_element(component)

        # Add sensors registered on this component
        plugin.add_elements(component.get_plugin_sensors(link))

        for conn in component.connections:
            if conn.joint:
                # Create the subtree after the joint
                other_link = self._build_body(traversed, model, plugin, conn.other)
                if other_link is None:
                    # This connection was already created
                    # from the other side.
                    continue

                # Check whether this component is the parent or the child
                # of the joint connection (it is the parent by default)
                # This is relevant for another reason, because we will
                # be moving the internals of the current joint around
                # to center the inertia later on. This means that for
                # joints for which the current link is the child we need
                # to move the joint as well since the joint position is
                # expressed in the child frame. If the current link is
                # the parent there is no need to move it with this one,
                # and since `create_joint` is called after everything
                # within the child link has been fully neither does
                # the other link code.
                parent_link = link
                child_link = other_link
                is_child = conn.joint.parent is not component

                if is_child:
                    parent_link, child_link = other_link, link

                joint = conn.joint.create_joint(parent_link, child_link,
                                                conn.joint.parent, conn.joint.child)
                model.add_element(joint)

                if is_child:
                    child_joints.append(joint)
            else:
                # Just add this element to the current link
                self._build_body(traversed, model, plugin, conn.other, link, child_joints)

        if create_link:
            # Give the link the inertial properties of the combined collision,
            # only calculated by the item which created the link.
            # Note that we need to align the center of mass with the link's origin,
            # which will move the internal components around. In order to compensate
            # for this in the internal position we need to reposition the link according
            # to this change.
            translation = link.align_center_of_mass()
            link.translate(-translation)
            link.calculate_inertial()

            # Translate all the joints expressed in this link's frame that
            # were created before repositioning
            # Note that the joints need the same translation as the child elements
            # rather than the counter translation of the link.
            for joint in child_joints:
                joint.translate(translation)

        return link
Exemplo n.º 19
0
    Vector3(0, 1, 0),

    # .. align with the top ..
    Vector3(0, 0, 0.5 * box_geom.size[2]),

    # .. and normal vector (other direction) ..
    Vector3(0, 0, 1),

    # .. rotation of the tangent vector ..
    Vector3(0, 1, 0),

    # .. of the box.
    box)

# Now, create a link and add both items to it
link = Link("my_link", elements=[box, cylinder])

# Calculate the correct inertial properties given
# the collision elements.
link.align_center_of_mass()
link.calculate_inertial()

# Rotate the link 45 degrees around the x-axis, specified in the parent frame
# just to demonstrate how that works (and to demonstrate align is still
# going to work after the rotation).
link.rotate_around(Vector3(1, 0, 0), math.radians(45), relative_to_child=False)

# Okay, not sure what this is supposed to be, but let's another wheel-like cylinder in
# a new link, and connect them with joints
wheel_geom = Cylinder(0.75, 0.1, mass=0.1)
wheel = StructureCombination("wheel", wheel_geom)
Exemplo n.º 20
0
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__))+'/../')

import trollius
from trollius import From
from tol.config import Config
from tol.manage import World

from sdfbuilder import SDF, Link, Model
from sdfbuilder.sensor import Sensor

conf = Config(visualize_sensors=True)

sdf = SDF()
model = Model("crash")
link = Link("my_link")
link.make_box(1.0, 1, 1, 1)
sensor = Sensor("sense", "contact")
link.add_element(sensor)
model.add_element(link)
sdf.add_element(model)
model.set_position(Vector3(0, 0, 0.5))


@trollius.coroutine
def run_server():
    world = yield From(World.create(conf))

    counter = 0
    while True:
        model.name = "test_bot_%d" % counter
Exemplo n.º 21
0
m = Model(name="mybot")
sdf = SDF(elements=[m])

geom = Box(SENSOR_BASE_THICKNESS, SENSOR_BASE_WIDTH, SENSOR_BASE_WIDTH, MASS)
base = StructureCombination("base", geom)

x_sensors = 0.5 * (SENSOR_BASE_THICKNESS + SENSOR_THICKNESS)
y_left_sensor = -0.5 * SENSOR_WIDTH - SEPARATION
y_right_sensor = 0.5 * SENSOR_WIDTH + SEPARATION

left = StructureCombination("left", Box(SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS))
left.set_position(Vector3(x_sensors, y_left_sensor, 0))

right = StructureCombination("right", Box(SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS))
right.set_position(Vector3(x_sensors, y_right_sensor, 0))

link = Link("my_link")
link.add_elements([base, left, right])


link.align_center_of_mass()
link.calculate_inertial()

m.add_element(link)

apply_surface_parameters(m)
m.translate(Vector3(0, 0, in_mm(30)))

with open("/home/elte/.gazebo/models/test_bot/model.sdf", "w") as f:
    f.write(str(sdf))
Exemplo n.º 22
0
    def _build_body(self, traversed, model, plugin, component, link=None, child_joints=None):
        """
        :param traversed: Set of components that were already traversed,
                          prevents loops (since all connections are two-sided).
        :type traversed: set
        :param model:
        :param plugin:
        :param component:
        :type component: Component
        :param link:
        :type link: Link
        :return: The primary link used in this traversal, i.e. the link
                 passed to it or the first link created.
        """
        if component in traversed:
            return link

        create_link = link is None
        traversed.add(component)
        if create_link:
            # New tree, create a link. The component name
            # should contain the body part ID so this name should
            # be unique.
            # The position of the link is determined later when we decide
            # its center of mass.
            link = Link("link_%s" % component.name, self_collide=True)
            child_joints = []
            model.add_element(link)

        # Add this component to the link.
        position = link.to_local_frame(component.get_position())
        rotation = link.to_local_direction(component.get_rotation())
        component.set_position(position)
        component.set_rotation(rotation)
        link.add_element(component)

        # Add sensors registered on this component
        plugin.add_elements(component.get_plugin_sensors(link))

        for conn in component.connections:
            if conn.joint:
                # Create the subtree after the joint
                other_link = self._build_body(traversed, model, plugin, conn.other)
                if other_link is None:
                    # This connection was already created
                    # from the other side.
                    continue

                # Check whether this component is the parent or the child
                # of the joint connection (it is the parent by default)
                # This is relevant for another reason, because we will
                # be moving the internals of the current joint around
                # to center the inertia later on. This means that for
                # joints for which the current link is the child we need
                # to move the joint as well since the joint position is
                # expressed in the child frame. If the current link is
                # the parent there is no need to move it with this one,
                # and since `create_joint` is called after everything
                # within the child link has been fully neither does
                # the other link code.
                parent_link = link
                child_link = other_link
                is_child = conn.joint.parent is not component

                if is_child:
                    parent_link, child_link = other_link, link

                joint = conn.joint.create_joint(parent_link, child_link,
                                                conn.joint.parent, conn.joint.child)
                model.add_element(joint)

                if is_child:
                    child_joints.append(joint)
            else:
                # Just add this element to the current link
                self._build_body(traversed, model, plugin, conn.other, link, child_joints)

        if create_link:
            # Give the link the inertial properties of the combined collision,
            # only calculated by the item which created the link.
            # Note that we need to align the center of mass with the link's origin,
            # which will move the internal components around. In order to compensate
            # for this in the internal position we need to reposition the link according
            # to this change.
            translation = link.align_center_of_mass()
            link.translate(-translation)
            link.calculate_inertial()

            # Translate all the joints expressed in this link's frame that
            # were created before repositioning
            # Note that the joints need the same translation as the child elements
            # rather than the counter translation of the link.
            for joint in child_joints:
                joint.translate(translation)

        return link
Exemplo n.º 23
0
from sdfbuilder import Link, PosableGroup, SDF, Model
from sdfbuilder.math import Vector3
from math import pi

link = Link("my_link")

minibox = Link("my_minibox")

# Link one is a vertical box
link.make_box(1.0, 2, 2, 4)

# Minibox is... well, a mini box
minibox.make_box(0.1, 0.2, 0.2, 0.2)

minibox.align(
    # Bottom left corner of minibox
    Vector3(-0.1, -0.1, -0.1),

    # Normal vector
    Vector3(-0.1, -0.1, -0.1),

    # Tangent vector
    Vector3(-0.1, -0.1, 0.2),

    # Top left of link
    Vector3(-1, -1, 2),

    # Normal vector
    Vector3(0, 0, 1),

    # Tangent vector
Exemplo n.º 24
0
from __future__ import print_function
import sys
from sdfbuilder import Link, Model, SDF
from sdfbuilder.math import Vector3

# Create two similar boxes
link1 = Link("box1")
link1.make_box(1.0, 0.1, 0.3, 0.5)

link2 = Link("box2")
link2.make_box(1.0, 0.1, 0.3, 0.5)

# Align the top of box2 with the front of box1
link2.align(Vector3(0, 0, 0.25), Vector3(0, 0, -1), Vector3(1, 0, 0),
            Vector3(0, -0.15, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), link1)

if __name__ == '__main__':
    sdf = SDF()
    model = Model("my_model")
    model.add_element(link1)
    model.add_element(link2)
    sdf.add_element(model)
    print(str(sdf))
Exemplo n.º 25
0
from __future__ import print_function
import sys
from sdfbuilder import Link, Model, SDF
from sdfbuilder.math import Vector3

# Create two similar boxes
link1 = Link("box1")
link1.make_box(1.0, 0.1, 0.3, 0.5)

link2 = Link("box2")
link2.make_box(1.0, 0.1, 0.3, 0.5)

# Align the top of box2 with the front of box1
link2.align(
    Vector3(0, 0, 0.25),
    Vector3(0, 0, -1),
    Vector3(1, 0, 0),

    Vector3(0, -0.15, 0),
    Vector3(0, 1, 0),
    Vector3(0, 0, 1),

    link1
)

if __name__ == '__main__':
    sdf = SDF()
    model = Model("my_model")
    model.add_element(link1)
    model.add_element(link2)
    sdf.add_element(model)