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)
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)
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)
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)
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)
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)
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)
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)
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)
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)
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)
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)
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
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
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)
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
# .. 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)
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
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)
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
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))
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
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
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))
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)
