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 _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 _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
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
print("Inserting %s..." % model.name)
fut = yield From(world.insert_model(sdf))
yield From(fut)
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