def get_slot_normal(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-1, 0, 0)
else:
return self.tail.to_sibling_direction(Vector3(1, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-0.5 * SLOT_THICKNESS, 0, 0)
else:
return self.tail.to_sibling_frame(
Vector3(0.5 * SLOT_THICKNESS, 0, 0), self)
def __init__(self, name, start, end, thickness, height, **kwargs):
"""
Construct a wall of the given thickness and height from
`start` to `end`. This simply results in a box.
:param start: Starting point of the wall.
:type start: Vector3
:param end: Ending point of the wall.
:type end: Vector3
:return:
"""
super(Wall, self).__init__(name, static=True, **kwargs)
assert start.z == end.z, "Walls with different start / end z are undefined."
center = 0.5 * (end + start)
diff = end - start
size = abs(diff)
self.wall = Link("wall_link")
self.wall.make_box(10e10, size, thickness, height)
# Rotate the wall so it aligns with the vector from
# x to y
self.align(
Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1),
center, diff, Vector3(0, 0, 1), Posable("mock")
)
self.add_element(self.wall)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-0.5 * SLOT_THICKNESS, 0, 0)
else:
return self.connection.to_sibling_frame(
Vector3(0.5 * JOINT_CONNECTION_THICKNESS, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * self.SLOT_THICKNESS
if slot_id == 0:
# Root has no relative pose so this is easy
return Vector3(-offset, 0, 0)
else:
# This is a posable group, so we must use
# sibling conversion.
return self.tail.to_sibling_frame(Vector3(offset, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * SLOT_THICKNESS
if slot_id == 0:
# The root slot is positioned half the slot
# thickness to the left
return Vector3(-offset, 0, 0)
else:
# A `BodyPart` is a posable group, so item positions are
# in the parent frame. If we convert to the local frame we can
# simply add the offset in the x-direction
tail_pos = self.to_local_frame(self.hinge_tail.get_position())
return tail_pos + Vector3(offset, 0, 0)
def _robot_inserted(self, robot_name, tree, robot, initial_battery,
parents, msg, return_future):
"""
Registers a newly inserted robot and marks the insertion
message response as handled.
:param tree:
:param robot_name:
:param tree:
:param robot:
:param initial_battery:
:param parents:
:param msg:
:type msg: pygazebo.msgs.response_pb2.Response
:param return_future: Future to resolve with the created robot object.
:type return_future: Future
:return:
"""
inserted = ModelInserted()
inserted.ParseFromString(msg.serialized_data)
model = inserted.model
time = Time(msg=inserted.time)
p = model.pose.position
position = Vector3(p.x, p.y, p.z)
robot = self.create_robot_manager(robot_name=robot_name,
tree=tree,
robot=robot,
position=position,
time=time,
battery_level=initial_battery,
parents=parents)
self.register_robot(robot)
return_future.set_result(robot)
def get_slot_position(self, slot):
"""
Returns the attachment position of each of the slots.
"""
# Again, prevent errors
self.check_slot(slot)
# The constructor of `BodyPart` stores the initialization's kwargs parameters
# in `self.part_params`.
length = self.part_params["length"]
# The center of the base box lies at (0, 0), move 1/2 x to the
# left to get that slot, or move 1/2 x to the right, plus the variable length
# minus the offset to get the other.
return Vector3(-0.5 * self.x, 0, 0) if slot == 0 else Vector3(
0.5 * self.x + length - self.JOINT_OFFSET, 0, 0)
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 place_birth_clinic(self, diameter, height, angle=None):
"""
CURRENTLY NOT USED.
Places the birth clinic. Since we're lazy and rotating appears to cause errors,
we're just deleting the old birth clinic and inserting a new one every time.
Inserts the birth clinic
:param height:
:param diameter:
:param angle:
:return:
"""
if self.birth_clinic_model:
# Delete active birth clinic and place new
yield From(
wait_for(self.delete_model(self.birth_clinic_model.name)))
self.birth_clinic_model = None
if angle is None:
angle = random.random() * 2 * math.pi
name = "birth_clinic_" + str(self.get_robot_id())
self.birth_clinic_model = bc = BirthClinic(name=name,
diameter=diameter,
height=height)
bc.rotate_around(Vector3(0, 0, 1), angle)
future = yield From(self.insert_model(SDF(elements=[bc])))
raise Return(future)
def get_slot_position(self, slot):
self.check_slot(slot)
# TODO Still seem to have some positioning issues
return self.component.to_sibling_frame(
Vector3(-0.5 * SENSOR_BASE_THICKNESS, 0, 0),
self
)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * SLOT_THICKNESS
if slot_id == 0:
# The root slot is positioned half the slot
# thickness to the left
return self.hinge_root.to_sibling_frame(Vector3(-offset, 0, 0),
self)
else:
# A `BodyPart` is a PosableGroup, so child positions are
# similar to sibling positions. We can thus take the position
# in the tail, and use sibling conversion to get the position
# in the body part.
return self.hinge_tail.to_sibling_frame(Vector3(offset, 0, 0),
self)
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.mass = MASS
self.x = SENSOR_BASE_THICKNESS
self.y = self.z = SENSOR_BASE_WIDTH
super(LightSensor, self)._initialize(**kwargs)
# Add the SDF camera sensor
camera = SdfSensor(
name="{}_light_sensor".format(self.id),
sensor_type="camera",
update_rate=self.conf.sensor_update_rate)
# TODO: Set field of view
cam_details = "<camera>" \
"<image>" \
"<width>1</width><height>1</height>" \
"</image>" \
"<clip><near>{}</near><far>{}</far></clip>" \
"</camera>".format(nf(in_mm(1)), nf(in_mm(50000)))
camera.add_element(cam_details)
camera.set_position(Vector3(0.5 * self.x, 0, 0))
self.component.add_sensor(camera, "light")
if self.conf.visualize_sensors:
camera.add_element("<visualize>1</visualize>")
self.apply_color()
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 analysis_func():
analyzer = yield From(BodyAnalyzer.create(address=("127.0.0.1", 11346)))
# Try a maximum of 100 times
for i in range(100):
# Generate a new robot
robot = generate_robot()
sdf = get_analysis_robot(robot, builder)
# Find out its intersections and bounding box
intersections, bbox = yield From(
analyzer.analyze_robot(robot, builder=builder))
if intersections:
print("Invalid model - intersections detected.", file=sys.stderr)
else:
print("No model intersections detected!", file=sys.stderr)
if bbox:
# Translate the model in z direction so it stands directly on
# the ground
print("Model bounding box: ({}, {}, {}), ({}, {}, {})".format(
bbox.min.x, bbox.min.y, bbox.min.z, bbox.max.x, bbox.max.y,
bbox.max.z),
file=sys.stderr)
model = sdf.elements[0]
model.translate(Vector3(0, 0, -bbox.min.z))
print(str(robot_to_sdf(robot, "test_bot", "controllerplugin.so")))
break
def run(self, conf):
conf.min_parts = 1
conf.max_parts = 3
conf.arena_size = (3, 3)
conf.max_lifetime = 99999
conf.initial_age_mu = 99999
conf.initial_age_sigma = 1
conf.age_cutoff = 99999
self.body_spec = get_body_spec(conf)
self.brain_spec = get_brain_spec(conf)
self.nn_parser = NeuralNetworkParser(self.brain_spec)
print "OPENING FILES!!!!!!!!!!!!!!!!!!!"
with open("body/{0}".format(self.body_file), 'r') as robot_file:
robot_yaml = robot_file.read()
for filename in self.brain_files:
with open("brains/{0}".format(filename, 'r')) as brain_file:
br_yaml = brain_file.read()
self.brain_genotypes.append(
yaml_to_genotype(br_yaml, self.brain_spec))
yield From(wait_for(self.pause(True)))
pose = Pose(position=Vector3(0, 0, 0.5), rotation=rotate_vertical(0))
robot_pb = yaml_to_robot(self.body_spec, self.brain_spec, robot_yaml)
tree = Tree.from_body_brain(robot_pb.body, robot_pb.brain,
self.body_spec)
print "INSERTING ROBOT!!!!!!!!!!!!!!!!!!!!!!"
robot = yield From(wait_for(self.insert_robot(tree, pose)))
self.robot_name = robot.name
self.modify_nn_publisher = yield From(
self.manager.advertise(
'/gazebo/default/{0}/modify_neural_network'.format(
self.robot_name),
'gazebo.msgs.ModifyNeuralNetwork',
))
# Wait for connections
yield From(self.modify_nn_publisher.wait_for_listener())
brain_num = 0
num_of_brains = len(self.brain_genotypes)
print "Number of brains = {0}".format(num_of_brains)
yield From(wait_for(self.pause(False)))
while (True):
if self.timers.is_it_time('evaluate', self.time_period,
self.get_world_time()):
n = brain_num % num_of_brains
print "Switching brain to #{0}!!!!!!!!!".format(n)
yield From(self.insert_brain(self.brain_genotypes[n]))
self.timers.reset('evaluate', self.get_world_time())
brain_num += 1
yield From(trollius.sleep(0.1))
def get_slot_tangent(self, slot_id):
"""
We use the same tangent vector for every slot, pointing
horizontally.
:param slot_id:
:return:
"""
return Vector3(1, 0, 0)
def gen_boxes(dimensions=4, spacing=0.5, size=0.05):
for x in range(-dimensions,dimensions+1):
for y in range(-dimensions,dimensions+1):
l = Link("box")
#box_geom = Box(1.0, 1.0, 1.0, mass=0.5)
#b = StructureCombination("box", box_geom)
#l.add_element(b)
if (x!=0 or y!=0):
l.make_box(1, size, size, 0.01*max(abs(x),abs(y)))
pos = Vector3(spacing*x,spacing*y,0)
l.set_position(pos)
l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False)
model.add_element(l)
def get_slot_position(self, slot):
"""
Return slot position
"""
self.check_slot(slot)
vmax = WIDTH / 2.0
if slot == 0:
# Front face
return Vector3(0, -vmax, 0)
elif slot == 1:
# Back face
return Vector3(0, vmax, 0)
elif slot == 2:
# Right face
return Vector3(vmax, 0, 0)
# Left face
return Vector3(-vmax, 0, 0)
def get_slot_position(self, slot_id):
"""
Modify `get_slot_position` to return the attachment of the
motor instead.
:param slot_id:
:return:
"""
v = super(Wheel, self).get_slot_position(slot_id)
return v + Vector3(0, 0, self.MOTOR_SIZE)
def get_slot_position(self, slot_id):
"""
Returns the slot position for the given slot. Slot 0 is
defined as the top, slot 1 is the bottom.
:param slot_id:
:return:
"""
z = self.length / 2.0
return Vector3(0, 0, z if slot_id == 0 else -z)
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Call super to initialize the cylinder part of the wheel
super(Wheel, self)._initialize(**kwargs)
# Create the small box that serves as the motor
box_size = self.MOTOR_SIZE
self.attachment = self.create_component(
BoxGeom(box_size, box_size, box_size, 0.01), "cylinder-attach")
# Get attachment position and axis of the motor joint
anchor = Vector3(0, 0, 0.5 * self.LENGTH)
axis = Vector3(0, 0, 1)
# Size and position the box
self.attachment.set_position(anchor + Vector3(0, 0, 0.5 * box_size))
# Create revolute joint. Remember: joint position is in child frame
motor_joint = Joint("revolute",
self.component,
self.attachment,
axis=axis)
motor_joint.set_position(Vector3(0, 0, -0.5 * box_size))
# Set a force limit on the joint
motor_joint.axis.limit = Limit(effort=0.01)
self.add_joint(motor_joint)
# Register a joint motor with a maximum velocity of
# 50 rounds per minute (the speed is in radians per second)
# We also give it a simple PID controller
pid = PID(proportional_gain=1.0, integral_gain=0.1)
max_speed = 2 * math.pi * 50.0 / 60
self.motors.append(
VelocityMotor(self.id,
"rotate",
motor_joint,
pid=pid,
min_velocity=-max_speed,
max_velocity=max_speed))
self.apply_color()
def _initialize(self, **kwargs):
"""
Initialize method to generate the hinge. Inheriting from
"box" makes sure there is a box of the given size in
`self.link`.
"""
super(PassiveHinge, self)._initialize(**kwargs)
# Create the variable size block. `create_link` is the recommended
# way of doing this, because it sets some non-default link properties
# (such as self_collide) which you generally need.
length = kwargs["length"]
self.var_block = self.create_component(
BoxGeom(length, self.y, self.z, 0.1), "var-block")
# We move the block in the x-direction so that it
# just about overlaps with the other block (to
# make it seem like a somewhat realistic joint)
self.var_block.translate(
Vector3(0.5 * (length + self.x) - self.JOINT_OFFSET, 0, 0))
# Now create a revolute joint at this same position. The
# joint axis is in the y-direction.
axis = Vector3(0, 1, 0)
passive_joint = Joint("revolute",
self.component,
self.var_block,
axis=axis)
# Set some movement limits on the joint
passive_joint.axis.limit = Limit(lower=math.radians(-45),
upper=math.radians(45),
effort=1.0)
# Set the joint position - in the child frame!
passive_joint.set_position(
Vector3(-0.5 * length + self.JOINT_OFFSET, 0, 0))
# Don't forget to add the joint and the link
self.add_joint(passive_joint)
# Apply the generated color
self.apply_color()
def _initialize(self, **kwargs):
# Initialize the root
self.root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
# Initialize the servo
servo = self.create_component(
Box(SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT, MASS_SERVO))
x_servo = 0.5 * (SLOT_THICKNESS + SERVO_LENGTH) + SEPARATION
# z_servo = 0.5 * (SERVO_HEIGHT - SLOT_WIDTH) + SERVO_Z_OFFSET
z_servo = 0
servo.set_position(Vector3(x_servo, 0, z_servo))
# Initialize the connection
self.connection = self.create_component(
Box(JOINT_CONNECTION_THICKNESS, JOINT_CONNECTION_WIDTH,
JOINT_CONNECTION_WIDTH, MASS_CONNECTION_SLOT), "connection")
x_conn = x_servo + 0.5 * (SERVO_LENGTH -
JOINT_CONNECTION_THICKNESS) + SEPARATION
self.connection.set_position(Vector3(x_conn, 0, 0))
# Fix the links
# root <-> servo
self.fix(self.root, servo)
# servo <revolute> connection
self.joint = Joint("revolute",
servo,
self.connection,
axis=Vector3(1, 0, 0))
self.joint.axis.limit = Limit(
effort=constants.MAX_SERVO_TORQUE_ROTATIONAL,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(self.joint)
# Now we add a motor that powers the joint. This particular servo
# targets a velocity. Use a simple PID controller initially.
pid = constants.SERVO_POSITION_PID
self.motors.append(PositionMotor(self.id, "rotate", self.joint, pid))
# Call color mixin
self.apply_color()
def get_slot_tangent(self, slot):
"""
Return slot tangent
"""
self.check_slot(slot)
# The top face is tangent to all supported
# slots in the plane, front, back, right, left
# Since there's no particular reason for choosing any
# other we just return the top face for all.
return Vector3(0, 0, 1)
def birth(self, tree, bbox, parents):
"""
Birth process, picks a robot position and inserts
the robot into the world.
Robots are currently placed at a random position within the circular
birth clinic. In this process, 5 attempts are made to place the robot
at the minimum drop distance from other robots. If this fails however
the last generated position is used anyway.
:param tree:
:param bbox:
:param parents:
:return:
"""
s = len(tree)
if (s + self.total_size()) > self.conf.part_limit:
print("Not enough parts in pool to create robot of size %d." % s)
raise Return(False)
# Pick a random radius and angle within the birth clinic
pos = Vector3()
pos.z = -bbox.min.z + self.conf.drop_height
done = False
min_drop = self.conf.min_drop_distance
for i in range(5):
angle = random.random() * 2 * math.pi
radius = self.conf.birth_clinic_diameter * 0.5 * random.random()
pos.x = radius * math.cos(angle)
pos.y = radius * math.sin(angle)
done = True
for robot in self.robots.values():
if robot.distance_to(pos, planar=True) < min_drop:
done = False
break
if done:
break
if not done:
logger.warning("Warning: could not satisfy minimum drop distance.")
# Note that we register the reproduction only if
# the child is actually born, i.e. there were enough parts
# left in the world to satisfy the request.
if parents:
ra, rb = parents
ra.did_mate_with(rb)
rb.did_mate_with(ra)
self.births += 1
fut = yield From(self.insert_robot(tree, Pose(position=pos), parents=parents))
raise Return(fut)
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 get_slot_tangent(self, slot):
"""
The tangent vectors determine the "zero orientation", meaning
if a body part has an orientation of 0 it will have its tangent
vector aligned with its parent. The tangent vector has to be
orthogonal to the slot normal.
We have no specific orientation requirements, so we simply
always return one tangent vector which is orthogonal to both
slot normals, i.e. the vector (0, 0, 1).
"""
self.check_slot(slot)
return Vector3(0, 0, 1)
def _initialize(self, **kwargs):
self.radius = in_mm(kwargs['radius'])
wheel = self.create_component(
Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel")
self.root = self.create_component(
Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT))
# Create the wheel
z_wheel = 0.5 * SLOT_THICKNESS - (
SLOT_THICKNESS + SLOT_CONNECTION_THICKNESS - SLOT_WHEEL_OFFSET)
wheel.set_position(Vector3(0, 0, z_wheel))
# Attach the wheel and the root with a revolute joint
joint = Joint("revolute", self.root, wheel, axis=Vector3(0, 0, -1))
# TODO Adequate force limit for passive wheel
joint.axis.limit = Limit(effort=constants.MAX_SERVO_TORQUE_ROTATIONAL)
self.add_joint(joint)
# Call color mixin
self.apply_color()
def _initialize(self, **kwargs):
self.radius = in_mm(kwargs['radius'])
# Create the root
self.root = self.create_component(
Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT), "root")
# Create the servo
servo = self.create_component(Box(SERVO_HEIGHT, SERVO_WIDTH, SERVO_LENGTH, MASS_SERVO), "servo")
servo.set_position(Vector3(0, 0, X_SERVO))
# Create the wheel
wheel = self.create_component(Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel")
wheel.set_position(Vector3(0, 0, X_WHEEL))
# Fix the root to the servo
self.fix(self.root, servo)
# Attach the wheel and the root with a revolute joint
self.joint = Joint("revolute", servo, wheel, axis=Vector3(0, 0, -1))
self.joint.set_position(Vector3(0, 0, -WHEEL_THICKNESS))
self.joint.axis.limit = Limit(
effort=constants.MAX_SERVO_TORQUE_ROTATIONAL,
velocity=constants.MAX_SERVO_VELOCITY
)
self.add_joint(self.joint)
# Now we add a motor that powers the joint. This particular servo
# targets a velocity. Use a simple PID controller initially.
pid = constants.SERVO_VELOCITY_PID
self.motors.append(VelocityMotor(
self.id, "rotate", self.joint, pid=pid,
max_velocity=constants.MAX_SERVO_VELOCITY,
min_velocity=-constants.MAX_SERVO_VELOCITY
))
# Call color mixin
self.apply_color()
