def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
super(CoreComponentWithMics, self)._initialize(**kwargs)
half_width = WIDTH / 2.0
right_angle = 3.14159265 / 2.0
# rotates clockwise around axis by angle:
mic_pose_1 = Pose(position=Vector3(0, half_width, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(-1, 0, 0)))
mic_pose_2 = Pose(position=Vector3(half_width, 0, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(0, 1, 0)))
mic_pose_3 = Pose(position=Vector3(0, -half_width, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(1, 0, 0)))
mic_pose_4 = Pose(position=Vector3(-half_width, 0, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(0, -1, 0)))
# The second argument ("direction") is the type of the sensor that is checked in SensorFactory.cpp.
# Any existing type of gazebo sensors can be used. Beside that, a new type can be registered in WorldController.cpp
mic_1 = SdfSensor("dir_sensor_1",
"direction",
pose=mic_pose_1,
update_rate=self.conf.sensor_update_rate)
mic_2 = SdfSensor("dir_sensor_2",
"direction",
pose=mic_pose_2,
update_rate=self.conf.sensor_update_rate)
mic_3 = SdfSensor("dir_sensor_3",
"direction",
pose=mic_pose_3,
update_rate=self.conf.sensor_update_rate)
mic_4 = SdfSensor("dir_sensor_4",
"direction",
pose=mic_pose_4,
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(mic_1)
self.link.add_sensor(mic_2)
self.link.add_sensor(mic_3)
self.link.add_sensor(mic_4)
def attach(self, other, other_slot, my_slot, orientation):
"""
Positions all the elements in this body part to align slots
with another body part.
:param other:
:type other: BodyPart
:param other_slot:
:type other_slot: int
:param my_slot:
:type my_slot: int
:param orientation: Orientation in radians
:type orientation: float
:return:
"""
a_slot = self.get_slot_position(my_slot)
b_slot = other.get_slot_position(other_slot)
a_normal = self.get_slot_normal(my_slot)
b_normal = other.get_slot_normal(other_slot)
a_tangent = self.get_slot_tangent(my_slot)
b_tangent = other.get_slot_tangent(other_slot)
if orientation:
# Rotate the a_tangent vector over the normal
# with the given number of radians to apply
# the rotation. Rotating this vector around
# the normal should not break their orthogonality.
rot = Quaternion.from_angle_axis(orientation, a_normal)
a_tangent = rot * a_tangent
self.align(
a_slot,
a_normal,
a_tangent,
b_slot,
b_normal,
b_tangent,
other,
relative_to_child=True
)
# Internal sanity check
norm = (self.to_parent_frame(a_slot)
- other.to_parent_frame(b_slot)).norm()
assert norm < 1e-5, "Incorrect attachment positions!"
my_component = self.get_slot(my_slot)
at_component = other.get_slot(other_slot)
# Create a connection between these two slots to complete
# the body graph.
at_component.create_connection_one_way(my_component)
def attach(self, other, other_slot, my_slot, orientation):
"""
Positions all the elements in this body part to align slots
with another body part.
:param other:
:type other: BodyPart
:param other_slot:
:type other_slot: int
:param my_slot:
:type my_slot: int
:param orientation: Orientation in radians
:type orientation: float
:return:
"""
a_slot = self.get_slot_position(my_slot)
b_slot = other.get_slot_position(other_slot)
a_normal = self.get_slot_normal(my_slot)
b_normal = other.get_slot_normal(other_slot)
a_tangent = self.get_slot_tangent(my_slot)
b_tangent = other.get_slot_tangent(other_slot)
if orientation:
# Rotate the a_tangent vector over the normal
# with the given number of radians to apply
# the rotation. Rotating this vector around
# the normal should not break their orthogonality.
rot = Quaternion.from_angle_axis(orientation, a_normal)
a_tangent = rot * a_tangent
self.align(
a_slot,
a_normal,
a_tangent,
b_slot,
b_normal,
b_tangent,
other,
relative_to_child=True
)
# Internal sanity check
norm = (self.to_parent_frame(a_slot) - other.to_parent_frame(b_slot)).norm()
assert norm < 1e-5, "Incorrect attachment positions!"
my_component = self.get_slot(my_slot)
at_component = other.get_slot(other_slot)
# Create a connection between these two slots to complete
# the body graph.
at_component.create_connection_one_way(my_component)
def get_circle_poses(n, radius):
"""
:param n:
:param radius:
:return:
"""
shift = 2.0 * math.pi / n
poses = []
for i in xrange(n):
angle = i * shift
x, y = radius * math.cos(angle), radius * math.sin(angle)
starting_rotation = Quaternion.from_angle_axis(math.pi + angle, Vector3(0, 0, 1))
poses.append(Pose(position=Vector3(x, y, 0), rotation=starting_rotation))
return poses
def get_circle_poses(n, radius):
"""
:param n:
:param radius:
:return:
"""
shift = 2.0 * math.pi / n
poses = []
for i in xrange(n):
angle = i * shift
x, y = radius * math.cos(angle), radius * math.sin(angle)
starting_rotation = Quaternion.from_angle_axis(math.pi + angle, Vector3(0, 0, 1))
poses.append(Pose(position=Vector3(x, y, 0), rotation=starting_rotation))
return poses
def set_birth_clinic_rotation(self, angle=None):
"""
Sets rotation of the birth clinic.
:param angle: Desired rotation, a random angle is chosen if none is given.
:return:
"""
if angle is None:
angle = random.random() * 2 * math.pi
msg = model_pb2.Model()
msg.name = self.birth_clinic_model.name
quat = Quaternion.from_angle_axis(angle, Vector3(0, 0, 1))
pose = msg.pose
pos, rot = pose.position, pose.orientation
pos.x, pos.y, pos.z = 0, 0, 0
rot.w, rot.x, rot.y, rot.z = quat
fut = yield From(self.model_control.publish(msg))
raise Return(fut)
def set_birth_clinic_rotation(self, angle=None):
"""
Sets rotation of the birth clinic.
:param angle: Desired rotation, a random angle is chosen if none is given.
:return:
"""
if angle is None:
angle = random.random() * 2 * math.pi
msg = model_pb2.Model()
msg.name = self.birth_clinic_model.name
quat = Quaternion.from_angle_axis(angle, Vector3(0, 0, 1))
pose = msg.pose
pos, rot = pose.position, pose.orientation
pos.x, pos.y, pos.z = 0, 0, 0
rot.w, rot.x, rot.y, rot.z = quat
fut = yield From(self.model_control.publish(msg))
raise Return(fut)
