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_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
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
# them around together.
group = PosableGroup()
group.add_element(link)
group.add_element(minibox)
# Move and rotate the group to confuse the mechanism
# (this should just be undone at the align later)
group.rotate_around(Vector3(1, 1, 0), 0.1 * pi)
group.translate(Vector3(0.6, 0.7, 0.8))
# Create a new, larger box called link 2
link2 = Link("my_link_2")
link2.make_box(2.0, 4, 3, 3)
# Translate and rotate just to add some extra complexity
link2.translate(Vector3(0.5, 0.5, 2))
link2.rotate_around(Vector3(1, 1, 1), 0.5 * pi)
# Now align the group so its right center lands at
# the top center of link 2
group.align(
# Center of the right face of box 1
Vector3(1, 0, 0),
# Vector normal to box 2 right face
Vector3(1, 0, 0),
# Vector normal to box 2 top face should align with...(*)
Vector3(0, 0, 1),
# Center of the top face of box 2
# them around together.
group = PosableGroup()
group.add_element(link)
group.add_element(minibox)
# Move and rotate the group to confuse the mechanism
# (this should just be undone at the align later)
group.rotate_around(Vector3(1, 1, 0), 0.1 * pi)
group.translate(Vector3(0.6, 0.7, 0.8))
# Create a new, larger box called link 2
link2 = Link("my_link_2")
link2.make_box(2.0, 4, 3, 3)
# Translate and rotate just to add some extra complexity
link2.translate(Vector3(0.5, 0.5, 2))
link2.rotate_around(Vector3(1, 1, 1), 0.5 * pi)
# Now align the group so its right center lands at
# the top center of link 2
group.align(
# Center of the right face of box 1
Vector3(1, 0, 0),
# Vector normal to box 2 right face
Vector3(1, 0, 0),
# Vector normal to box 2 top face should align with...(*)
Vector3(0, 0, 1),
# Center of the top face of box 2
