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()