def test_euler_angles(self):
"""
Tests Euler angles from a quaternion that used to fail. Other Euler
angles are tested from visual inspection in the `posable` test case.
:return:
"""
q = Quaternion(-0.5, 0.5, 0.5, 0.5)
roll, pitch, yaw = q.get_rpy()
self.assertAlmostEquals(-1.5707963267948968, roll, msg="Invalid roll.")
self.assertAlmostEquals(-1.5707963267948968, pitch, msg="Invalid roll.")
self.assertAlmostEquals(0, yaw, msg="Invalid yaw")
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 _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 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)
def gen_steps(model_name,
num_steps=6,
offset=0.4,
height=0.02,
width=3.0,
depth=0.2,
incline=0):
model = Model(model_name, static=True)
steps = PosableGroup()
for x in range(0, num_steps):
l = Link("box")
l.make_box(1, depth, width, height)
pos = Vector3(offset + depth * x, 0, height / 2 + x * height)
l.set_position(pos)
steps.add_element(l)
for x in range(0, 4):
steps.set_rotation(
Quaternion.from_rpy(0, math.radians(-incline),
math.radians(90 * x)))
model.add_element(steps.copy())
return model
