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:
"""
self.link = self.create_component(
Box(WIDTH, WIDTH, HEIGHT, MASS),
"box",
visual=Mesh("model://tol_robot/meshes/CoreComponent.dae"))
# Now we will add the IMU sensor. First, we must
# create a sensor in SDF. Be careful to give the
# sensor a name which is unique for the entire
# robot, adding the ID will help us do that.
imu = SdfSensor("imu_sensor",
"imu",
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(imu)
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.link = self.create_component(
Box(WIDTH, WIDTH, HEIGHT, MASS),
"box",
visual=Mesh("model://tol_robot/meshes/CoreComponent.dae"))
if not self.conf.disable_sensors:
# Now we will add the IMU sensor. First, we must
# create a sensor in SDF. The sensor must have a name which
# is unique for the robot - `add_sensor` ensures this for us
# assuming the `prefix` argument is left to its default.
imu = SdfSensor("imu_sensor",
"imu",
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(imu)
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.mass = MASS
self.x = SENSOR_BASE_THICKNESS
self.y = self.z = SENSOR_BASE_WIDTH
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
visual = Visual("touch_visual", Mesh("model://rg_robot/meshes/TouchSensor.dae"))
visual.translate(Vector3(0.5 * SENSOR_THICKNESS, 0, 0))
self.component = self.create_component(
BoxGeom(self.x, self.y, self.z, self.mass), "box",
visual=visual
)
self._sensor_helper("left", x_sensors, y_left_sensor)
self._sensor_helper("right", x_sensors, y_right_sensor)
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Shorthand for variables
# Initialize root
self.hinge_root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
# Make frame
visual1 = Visual("frame_visual",
Mesh("model://rg_robot/meshes/ActiveHinge_Frame.dae"))
frame = self.create_component(Box(FRAME_LENGTH, SLOT_WIDTH,
FRAME_HEIGHT, MASS_FRAME),
"frame",
visual=visual1)
x_frame = SLOT_THICKNESS / 2.0 + SEPARATION + FRAME_LENGTH / 2.0
frame.set_position(Vector3(x_frame, 0, 0))
# Make servo
visual2 = Visual(
"servo_visual",
Mesh("model://rg_robot/meshes/ActiveCardanHinge_Servo_Holder.dae"))
servo = self.create_component(Box(SERVO_LENGTH, SLOT_WIDTH,
SERVO_HEIGHT, MASS_SERVO),
"servo",
visual=visual2)
x_servo = x_frame + (FRAME_ROTATION_OFFSET - 0.5 * FRAME_LENGTH) + \
(-0.5 * SERVO_LENGTH + SERVO_ROTATION_OFFSET)
servo.set_position(Vector3(x_servo, 0, 0))
# TODO Color servo?
# Make the tail. Visual is provided by Servo_Holder above.
self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"tail",
visual=False)
x_tail = x_servo + SERVO_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0
self.hinge_tail.set_position(Vector3(x_tail, 0, 0))
# Create joints to hold the pieces in position
# root <-> frame
self.fix(self.hinge_root, frame)
# 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", servo, frame, axis=Vector3(0, 1, 0))
self.joint.set_position(
Vector3(-0.5 * FRAME_LENGTH + FRAME_ROTATION_OFFSET, 0, 0))
self.joint.axis.limit = Limit(lower=-constants.SERVO_LIMIT,
upper=constants.SERVO_LIMIT,
effort=constants.MAX_SERVO_TORQUE,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(self.joint)
# connection part b <-> tail
self.fix(servo, self.hinge_tail)
# Now we add a motor that powers the joint. This particular servo
# targets a position. Use a simple PID controller initially.
pid = constants.SERVO_POSITION_PID
self.motors.append(PositionMotor(self.id, "rotate", self.joint, pid))
# 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()
def _initialize(self, **kwargs):
self.component = self.create_component(
BoxGeom(self.x, self.y, self.z, self.mass), "box",
visual=Mesh("model://tol_robot/meshes/FixedBrick.dae"))
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()
