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) if start.z != end.z: raise AssertionError( "Walls with different start / end z-axis 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 parse_vec3(source: str): # example source: Vector3(2.394427e+00, 3.195821e-01, 2.244915e-02) assert (source[:8] == 'Vector3(') assert (source[-1:] == ')') source = source[8:-1] try: x, y, z = [float(n) for n in source.split(', ')] except ValueError: print(f'could not parse vector3 for "{source}"') return Vector3() return Vector3(x, y, z)
def export_life_data(self, folder): life_measures = { 'distance': str(measures.displacement(self.manager)[0]), 'distance_magnitude': str(measures.displacement(self.manager)[0].magnitude()), 'velocity': str(measures.velocity(self.manager)), 'displacement_velocity': str(measures.displacement_velocity(self.manager)), 'path_length': str(measures.path_length(self.manager)), } life = { 'starting_time': float(self.manager.starting_time), 'age': float(self.age()), 'charge': self.charge(), 'start_pos': str(self.starting_position()), 'last_pos': str(self.pos()), 'avg_orientation': str( Vector3(self.manager.avg_roll, self.manager.avg_pitch, self.manager.avg_yaw)), 'avg_pos': str( Vector3(self.manager.avg_x, self.manager.avg_y, self.manager.avg_z)), 'last_mate': str(self.manager.last_mate), 'alive': str(self.alive()), 'birth': str(self.birth_type), 'parents': [parent.name for parent in self.parents] if self.parents is not None else 'None', 'children': [child.name for child in self.children], 'measures': life_measures, } with open(os.path.join(folder, f'life_{self.id}.yaml'), 'w') as f: f.write(str(yaml.dump(life)))
def _robot_inserted( self, robot, msg ): """ Registers a newly inserted robot and marks the insertion message response as handled. :param robot: RevolveBot :param msg: :type msg: pygazebo.msgs.response_pb2.Response :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_manager = self.create_robot_manager( robot, position, time ) self.register_robot(robot_manager) return robot_manager
async def _evaluate_robot(self, simulator_connection, robot, conf): if robot.failed_eval_attempt_count == 3: logger.info( f'Robot {robot.phenotype.id} evaluation failed (reached max attempt of 3), fitness set to None.' ) robot_fitness = None return robot_fitness, None else: # Change this `max_age` from the command line parameters (--evalution-time) max_age = conf.evaluation_time robot_manager = await simulator_connection.insert_robot( robot.phenotype, Vector3(0, 0, self._settings.z_start), max_age) start = time.time() # Start a run loop to do some stuff while not robot_manager.dead: # robot_manager.age() < max_age: await asyncio.sleep(1.0 / 2) # 5= state_update_frequency end = time.time() elapsed = end - start logger.info(f'Time taken: {elapsed}') robot_fitness = conf.fitness_function(robot_manager, robot) simulator_connection.unregister_robot(robot_manager) # await simulator_connection.delete_all_robots() # await simulator_connection.delete_robot(robot_manager) # await simulator_connection.pause(True) await simulator_connection.reset(rall=True, time_only=True, model_only=False) return robot_fitness, measures.BehaviouralMeasurements( robot_manager, robot)
async def run(): """ The main coroutine, which is started below. """ # Parse command line / file input arguments settings = parser.parse_args() # Load a robot from yaml robot = revolve_bot.RevolveBot() if settings.robot_yaml is None: robot.load_file("experiments/bo_learner/yaml/spider.yaml") else: robot.load_file(settings.robot_yaml) robot.update_substrate() # Connect to the simulator and pause world = await World.create(settings) await world.pause(True) await world.delete_model(robot.id) await asyncio.sleep(2.5) # Insert the robot in the simulator robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.025)) # Resume simulation await world.pause(False) # Start a run loop to do some stuff while True: await asyncio.sleep(5.0)
async def run(): """ The main coroutine, which is started below. """ # Parse command line / file input arguments settings = parser.parse_args() # Load a robot from yaml robot = revolve_bot.RevolveBot() robot.load_file("experiments/examples/yaml/spider.yaml") # Connect to the simulator and pause world = await World.create(settings) await world.pause(True) # Insert the robot in the simulator robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.05)) # Resume simulation await world.pause(False) # Start a run loop to do some stuff while True: # Print robot fitness every second print("Robot fitness is {fitness}".format( fitness=robot_manager.fitness())) await asyncio.sleep(1.0)
async def run(): """ The main coroutine, which is started below """ log = logger.create_logger('experiment', handlers=[ logging.StreamHandler(sys.stdout), ]) # Set debug level to DEBUG log.setLevel(logging.DEBUG) # Parse command line / file input arguments settings = parser.parse_args() # Start Simulator if settings.simulator_cmd != 'debug': simulator_supervisor = DynamicSimSupervisor( world_file=settings.world, simulator_cmd=settings.simulator_cmd, simulator_args=["--verbose"], plugins_dir_path=os.path.join('.', 'build', 'lib'), models_dir_path=os.path.join('.', 'models'), simulator_name='gazebo') await simulator_supervisor.launch_simulator(port=settings.port_start) await asyncio.sleep(0.1) # Connect to the simulator and pause connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) await asyncio.sleep(1) # initialization finished # load robot file robot = RevolveBot(_id=settings.test_robot) robot.load_file("/Users/nihed/Desktop/nihedssnakes/nihedssnake6.yaml", conf_type='yaml') robot.save_file( f'{"/Users/nihed/Desktop/nihedssnakes/nihedssnake6.yaml"}.sdf', conf_type='sdf') # insert robot into the simulator robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 0.25), life_timeout=None) await asyncio.sleep(1.0) # Start the main life loop while True: # Print robot fitness every second status = 'dead' if robot_manager.dead else 'alive' print( f"Robot fitness ({status}) is \n" f" OLD: {fitness.online_old_revolve(robot_manager)}\n" f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") await asyncio.sleep(1.0)
async def analyze_robot(self, robot, max_attempts=5): """ Performs body analysis of a given Robot object. :param robot: :type robot: Robot :param max_attempts: :return: """ sdf = robot.to_sdf(pose=Vector3(0, 0, 0)) ret = await self.analyze_sdf(sdf, max_attempts=max_attempts) return ret
async def run(): """ The main coroutine, which is started below. """ robot_file_path = "experiments/examples/yaml/spider.yaml" # Parse command line / file input arguments settings = parser.parse_args() # Start Simulator if settings.simulator_cmd != 'debug': simulator_supervisor = DynamicSimSupervisor( world_file=settings.world, simulator_cmd=settings.simulator_cmd, simulator_args=["--verbose"], plugins_dir_path=os.path.join('.', 'build', 'lib'), models_dir_path=os.path.join('.', 'models'), simulator_name='gazebo') await simulator_supervisor.launch_simulator(port=settings.port_start) await asyncio.sleep(0.1) # Load a robot from yaml robot = revolve_bot.RevolveBot() robot.load_file(robot_file_path) robot.update_substrate() # robot._brain = BrainRLPowerSplines() # Connect to the simulator and pause connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) await asyncio.sleep(1) # Starts the simulation await connection.pause(False) # Insert the robot in the simulator robot_manager = await connection.insert_robot( robot, Vector3(0, 0, settings.z_start)) # Start a run loop to do some stuff while True: # Print robot fitness every second status = 'dead' if robot_manager.dead else 'alive' print( f"Robot fitness ({status}) is \n" f" OLD: {fitness.online_old_revolve(robot_manager)}\n" f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") await asyncio.sleep(1.0)
def displacement(self): """ Returns a tuple of the displacement in both time and space between the first and last registered element in the speed window. :return: Tuple where the first item is a displacement vector and the second a `Time` instance. :rtype: tuple(Vector3, Time) """ if self.last_position is None: return Vector3(0, 0, 0), Time() return (self._positions[-1] - self._positions[0], self._times[-1] - self._times[0])
def displacement(robot_manager): """ Returns a tuple of the displacement in both time and space between the first and last registered element in the speed window. :return: Tuple where the first item is a displacement vector and the second a `Time` instance. :rtype: tuple(Vector3, Time) """ if len(robot_manager._positions) == 0: return Vector3(0, 0, 0), Time() return ( robot_manager._positions[-1] - robot_manager._positions[0], robot_manager._times[-1] - robot_manager._times[0] )
async def insert_robot( self, revolve_bot, pose=Vector3(0, 0, 0.05), life_timeout=None, ): """ Inserts a robot into the world. This consists of two steps: - Sending the insert request message - Receiving a ModelInfo response This method is a coroutine because of the first step, writing the message must be yielded since PyGazebo doesn't appear to support writing multiple messages simultaneously. For the response, i.e. the message that confirms the robot has been inserted, a future is returned. :param revolve_bot: :type revolve_bot: RevolveBot :param pose: Insertion pose of a robot :type pose: Pose|Vector3 :param life_timeout: Life span of the robot :type life_timeout: float|None :return: A future that resolves with the created `Robot` object. """ # if the ID is digit, when removing the robot, the simulation will try to remove random stuff from the # environment and give weird crash errors assert(not str(revolve_bot.id).isdigit()) sdf_bot = revolve_bot.to_sdf(pose) # if self.output_directory: # robot_file_path = os.path.join( # self.output_directory, # 'robot_{}.sdf'.format(revolve_bot.id) # ) # with open(robot_file_path, 'w') as f: # f.write(sdf_bot) response = await self.insert_model(sdf_bot, life_timeout) robot_manager = self._robot_inserted( robot=revolve_bot, msg=response ) return robot_manager
async def _evaluate_robot(simulator_connection, robot, conf): await simulator_connection.pause(True) robot_manager = await simulator_connection.insert_robot( robot, Vector3(0, 0, 0.25)) await simulator_connection.pause(False) start = time.time() # Start a run loop to do some stuff max_age = conf.evaluation_time while robot_manager.age() < max_age: await asyncio.sleep(1.0 / 5) # 5= state_update_frequency end = time.time() elapsed = end - start logger.info(f'Time taken: {elapsed}') robot_fitness = conf.fitness_function(robot_manager) await simulator_connection.pause(True) delete_future = await simulator_connection.delete_all_robots( ) # robot_manager await delete_future return robot_fitness
async def insert_robot( self, revolve_bot, pose=Vector3(0, 0, 0.05), ): """ Inserts a robot into the world. This consists of two steps: - Sending the insert request message - Receiving a ModelInfo response This method is a coroutine because of the first step, writing the message must be yielded since PyGazebo doesn't appear to support writing multiple messages simultaneously. For the response, i.e. the message that confirms the robot has been inserted, a future is returned. :param revolve_bot: :type revolve_bot: RevolveBot :param pose: Insertion pose of a robot :type pose: Pose|Vector3 :return: A future that resolves with the created `Robot` object. """ sdf_bot = revolve_bot.to_sdf(pose) if self.output_directory: robot_file_path = os.path.join( self.output_directory, 'robot_{}.sdf'.format(revolve_bot.id)) with open(robot_file_path, 'w') as f: f.write(sdf_bot) future = Future() insert_future = await self.insert_model(sdf_bot) # TODO: Unhandled error in exception handler. Fix this. insert_future.add_done_callback(lambda fut: self._robot_inserted( robot=revolve_bot, msg=fut.result(), return_future=future)) return future
def update_state(self, world, time, state, poses_file): """ Updates the robot state from a state message. :param world: Instance of the world :param time: The simulation time at the time of this position update. :type time: Time :param state: State message :param poses_file: CSV writer to write pose to, if applicable :type poses_file: csv.writer :return: """ pos = state.pose.position position = Vector3(pos.x, pos.y, pos.z) if self.starting_time is None: self.starting_time = time self.last_update = time self.last_position = position if poses_file: age = world.age() poses_file.writerow([ self.robot.id, age.sec, age.nsec, position.x, position.y, position.z, self.get_battery_level() ]) if float(self.age()) < self.warmup_time: # Don't update position values within the warmup time self.last_position = position self.last_update = time return # Calculate the distance the robot has covered as the Euclidean # distance over the x and y coordinates (we don't care for flying), # as well as the time it took to cover this distance. last = self.last_position ds = np.sqrt((position.x - last.x)**2 + (position.y - last.y)**2) dt = float(time - self.last_update) # Velocity is of course sum(distance) / sum(time) # Storing all separate distance and time values allows us to # efficiently calculate the new speed over the window without # having to sum the entire arrays each time, by subtracting # the values we're about to remove from the _dist / _time values. self._dist += ds self._time += dt if len(self._dt) >= self.speed_window: # Subtract oldest values if we're about to override it self._dist -= self._ds[-1] self._time -= self._dt[-1] self.last_position = position self.last_update = time self._positions.append(position) self._times.append(time) self._ds.append(ds) self._dt.append(dt)
async def run(): """ The main coroutine, which is started below. """ arguments = parser.parse_args() if arguments.robot_name is not None: # this split will give errors on windows robot_file_path = "experiments/bodies/" + arguments.robot_name + ".yaml" if arguments.iterations is not None: # this split will give errors on windows iterations = int(arguments.iterations) #robot_file_path = "experiments/examples/yaml/spider.yaml" # Parse command line / file input arguments settings = parser.parse_args() # Start Simulator if settings.simulator_cmd != 'debug': simulator_supervisor = DynamicSimSupervisor( world_file=settings.world, simulator_cmd=settings.simulator_cmd, simulator_args=["--verbose"], plugins_dir_path=os.path.join('.', 'build', 'lib'), models_dir_path=os.path.join('.', 'models'), simulator_name='gazebo') await simulator_supervisor.launch_simulator(port=settings.port_start) await asyncio.sleep(0.1) # Load a robot from yaml robot = revolve_bot.RevolveBot() robot.load_file(robot_file_path) robot.update_substrate() # robot._brain = BrainRLPowerSplines() # Connect to the simulator and pause connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) await asyncio.sleep(1) # Starts the simulation await connection.pause(False) # Insert the robot in the simulator #life_timeout = Fixed time a robot stays in the simulator robot_manager = await connection.insert_robot( robot, Vector3(0, 0, settings.z_start)) robot_fitness = [] for x in range(1, iterations): # Print robot fitness every second robot_fitness.append( fitness.displacement_velocity(robot_manager, robot)) await asyncio.sleep(0.1) print("**The Robot Fitness is: ", str(sum(robot_fitness) / len(robot_fitness)))
def random_spawn_pos(): return Vector3(random.uniform(-LIMIT_X, LIMIT_X), random.uniform(-LIMIT_Y, LIMIT_Y), Z_SPAWN_DISTANCE)
def random_uniform_unit_vec(): return Vector3(random.uniform(-1, 1), random.uniform(-1, 1), Z_SPAWN_DISTANCE).normalized()