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 _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Create components. Visuals are provided by the conn_a/conn_b meshes
self.hinge_root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_a",
visual=False)
self.hinge_tail = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_b",
visual=False)
mesh = Mesh("model://tol_robot/meshes/PassiveHinge.dae")
visual_a = Visual("conn_a_visual", mesh)
visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS))
conn_a = self.create_component(
Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_a", visual=visual_a)
# Flip visual along the x-axis by rotating PI degrees over z
# This will put it upside down, so we also flip it PI degrees over x
visual_b = Visual("conn_b_visual", mesh.copy())
visual_b.rotate_around(Vector3(1, 0, 0), math.pi, relative_to_child=False)
visual_b.rotate_around(Vector3(0, 0, 1), math.pi, relative_to_child=False)
visual_b.translate(Vector3(0.5 * SLOT_THICKNESS))
conn_b = self.create_component(
Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_b", visual=visual_b)
# Shorthand for variables
# Position connection part a
x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0
conn_a.set_position(Vector3(x_part_a, 0, 0))
# Position connection part b
x_part_b = x_part_a + (CONNECTION_PART_LENGTH / 2.0 - (CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2
conn_b.set_position(Vector3(x_part_b, 0, 0))
# Make the tail
x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0
self.hinge_tail.set_position(Vector3(x_tail, 0, 0))
# Fix components
self.fix(self.hinge_root, conn_a)
self.fix(self.hinge_tail, conn_b)
# Connection part a <(hinge)> connection part b
# Hinge joint axis should point straight up, and anchor
# the points in the center. Note that the position of a joint
# is expressed in the child link frame, so we need to take the
# position from the original code and subtract conn_b's position
self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1))
self.joint.set_position(Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET, 0, 0))
self.joint.axis.limit = Limit(
upper=constants.HINGE_LIMIT,
lower=-constants.HINGE_LIMIT
)
self.add_joint(self.joint)
# Apply color mixin
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Create components. Visuals are provided by the conn_a/conn_b meshes
self.hinge_root = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"slot_a",
visual=False)
self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"slot_b",
visual=False)
mesh = Mesh("model://tol_robot/meshes/PassiveHinge.dae")
visual_a = Visual("conn_a_visual", mesh)
visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS))
conn_a = self.create_component(Box(CONNECTION_PART_LENGTH,
CONNECTION_PART_THICKNESS,
CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_a",
visual=visual_a)
# Flip visual along the x-axis by rotating PI degrees over z
# This will put it upside down, so we also flip it PI degrees over x
visual_b = Visual("conn_b_visual", mesh.copy())
visual_b.rotate_around(Vector3(1, 0, 0),
math.pi,
relative_to_child=False)
visual_b.rotate_around(Vector3(0, 0, 1),
math.pi,
relative_to_child=False)
visual_b.translate(Vector3(0.5 * SLOT_THICKNESS))
conn_b = self.create_component(Box(CONNECTION_PART_LENGTH,
CONNECTION_PART_THICKNESS,
CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_b",
visual=visual_b)
# Shorthand for variables
# Position connection part a
x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0
conn_a.set_position(Vector3(x_part_a, 0, 0))
# Position connection part b
x_part_b = x_part_a + (
CONNECTION_PART_LENGTH / 2.0 -
(CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2
conn_b.set_position(Vector3(x_part_b, 0, 0))
# Make the tail
x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0
self.hinge_tail.set_position(Vector3(x_tail, 0, 0))
# Fix components
self.fix(self.hinge_root, conn_a)
self.fix(self.hinge_tail, conn_b)
# Connection part a <(hinge)> connection part b
# Hinge joint axis should point straight up, and anchor
# the points in the center. Note that the position of a joint
# is expressed in the child link frame, so we need to take the
# position from the original code and subtract conn_b's position
self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1))
self.joint.set_position(
Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET,
0, 0))
self.joint.axis.limit = Limit(upper=constants.HINGE_LIMIT,
lower=-constants.HINGE_LIMIT)
self.add_joint(self.joint)
# Apply color mixin
self.apply_color()
