def __init__(self, port): """ Initialization of the supervisor. :param port: Port number reserved for service :type port: int """ Supervisor.__init__(self) self._connections = [] try: self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) except socket.error, msg: print msg[1]
def __init__(self, population_size=10): """ Initializes supervisor. """ Supervisor.__init__(self) self.root = self.getRoot() # Collect all available controller nodes self.modules = [] self.modules += [self.getFromDef('MODULE_1')] self.modules += [self.getFromDef('MODULE_2')] self.modules += [self.getFromDef('MODULE_3')] self.modules += [self.getFromDef('MODULE_4')] self.modules += [self.getFromDef('MODULE_5')] self.modules += [self.getFromDef('MODULE_6')] self.modules += [self.getFromDef('MODULE_7')] self.modules += [self.getFromDef('MODULE_8')] # Register fitness functions self.fitness_functions = { 'velocity': self.evaluate_velocity, 'distance': self.evaluate_distance } # Zero the robot position and timestamp self.robot_x = None self.robot_y = None self.robot_z = None self.time_record = None # Load configuration self.config = {} try: self.load_configuration() except IOError: # If the config file doesn't exist yet, setup an initial configuration self.reset_configuration(population_size)
def __init__(self, port): """ Initialization of the supervisor. :param port: Port number reserved for service :type port: int """ Supervisor.__init__(self) self._connections = [] self._prev_t = 0.0 self._t = 0.0 # simulation time ball = self.getFromDef("BALL") self._ball_trans = ball.getField('translation') self._prev_ball_pos = np.array(self._ball_trans.getSFVec3f()) self._ball_pos = self._prev_ball_pos try: self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) except socket.error, msg: print(msg[1])
def __init__(self): self.BODY_PARTS_NUMBER = 13 self.WALK_SEQUENCES_NUMBER = 8 self.ROOT_HEIGHT = 1.27 self.CYCLE_TO_DISTANCE_RATIO = 0.22 self.speed = 1.15 self.current_height_offset = 0 self.joints_position_field = [] self.joint_names = [ "leftArmAngle", "leftLowerArmAngle", "leftHandAngle", "rightArmAngle", "rightLowerArmAngle", "rightHandAngle", "leftLegAngle", "leftLowerLegAngle", "leftFootAngle", "rightLegAngle", "rightLowerLegAngle", "rightFootAngle", "headAngle" ] self.height_offsets = [ -0.02, 0.04, 0.08, -0.03, -0.02, 0.04, 0.08, -0.03 ] # those coefficients are empirical coefficients which result in a realistic walking gait self.angles = [ # those coefficients are empirical coefficients which result in a realistic walking gait [-0.52, -0.15, 0.58, 0.7, 0.52, 0.17, -0.36, -0.74], # left arm [0.0, -0.16, -0.7, -0.38, -0.47, -0.3, -0.58, -0.21], # left lower arm [0.12, 0.0, 0.12, 0.2, 0.0, -0.17, -0.25, 0.0], # left hand [0.52, 0.17, -0.36, -0.74, -0.52, -0.15, 0.58, 0.7], # right arm [-0.47, -0.3, -0.58, -0.21, 0.0, -0.16, -0.7, -0.38], # right lower arm [0.0, -0.17, -0.25, 0.0, 0.12, 0.0, 0.12, 0.2], # right hand [-0.55, -0.85, -1.14, -0.7, -0.56, 0.12, 0.24, 0.4], # left leg [1.4, 1.58, 1.71, 0.49, 0.84, 0.0, 0.14, 0.26], # left lower leg [0.07, 0.07, -0.07, -0.36, 0.0, 0.0, 0.32, -0.07], # left foot [-0.56, 0.12, 0.24, 0.4, -0.55, -0.85, -1.14, -0.7], # right leg [0.84, 0.0, 0.14, 0.26, 1.4, 1.58, 1.71, 0.49], # right lower leg [0.0, 0.0, 0.42, -0.07, 0.07, 0.07, -0.07, -0.36], # right foot [0.18, 0.09, 0.0, 0.09, 0.18, 0.09, 0.0, 0.09] # head ] Supervisor.__init__(self)
def __init__(self, name, args=None): super().__init__(name) self.declare_parameter( 'synchronization', Parameter('synchronization', Parameter.Type.BOOL, False)) parser = argparse.ArgumentParser() parser.add_argument( '--webots-robot-name', dest='webotsRobotName', default='', help='Specifies the "name" field of the robot in Webots.') # use 'parse_known_args' because ROS2 adds a lot of internal arguments arguments, unknown = parser.parse_known_args() if arguments.webotsRobotName: os.environ['WEBOTS_ROBOT_NAME'] = arguments.webotsRobotName self.robot = Supervisor() self.timestep = int(self.robot.getBasicTimeStep()) self.clockPublisher = self.create_publisher(Clock, 'clock', 10) timer_period = 0.001 * self.timestep # seconds self.stepService = self.create_service(SetInt, 'step', self.step_callback) self.timer = self.create_timer(timer_period, self.timer_callback) self.sec = 0 self.nanosec = 0
def calculateCOM(robot): links = [ "left_foot", "left_shin", "left_thigh", "left_lower_arm", "left_upper_arm", "right_foot", "right_shin", "right_thigh", "right_lower_arm", "right_upper_arm", "torso", "head", "pelvis" ] centerOfMass = [0, 0, 0] for i in links: temp = Supervisor.getFromDef(i) tempCOM = temp.getCenterOfMass() centerOfMass += tempCOM return centerOfMass / 8.18
def get_robots(supervisor: Supervisor) -> List[Tuple[int, Node]]: robots = [] # List[Tuple[int, Supervisor]] for webots_id_str, zone_id in sr_controller.ROBOT_IDS_TO_CORNERS.items(): robot = supervisor.getFromId(int(webots_id_str)) if robot is None: msg = "Failed to get Webots node for zone {} (id: {})".format( zone_id, webots_id_str, ) print(msg) raise ValueError(msg) robots.append((zone_id, robot)) return robots
def main() -> None: quit_if_development_mode() supervisor = Supervisor() prepare(supervisor) # Check after we've paused the sim so that any errors won't be masked by # subsequent console output from a robot. check_required_libraries(REPO_ROOT / 'libraries.txt') remove_unused_robots(supervisor) with record_animation(supervisor, REPO_ROOT / 'recordings' / recording_path()): run_match(supervisor)
def remove_unused_robots(supervisor: Supervisor) -> None: for webots_id_str, zone_id in sr_controller.ROBOT_IDS_TO_CORNERS.items(): if sr_controller.get_zone_robot_file_path(zone_id).exists(): continue # Remove the robot robot = supervisor.getFromId(int(webots_id_str)) if robot is None: msg = "Failed to get Webots node for zone {} (id: {})".format( zone_id, webots_id_str, ) print(msg) raise ValueError(msg) robot.remove()
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME) time_step = int(supervisor.getBasicTimeStep()) duration_ms = time_step * int(1000 * GAME_DURATION_SECONDS // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def get_simulation_run_mode(supervisor: Supervisor) -> 'SimulationMode': # webots 2020b is buggy and can raise TypeError when getDevice is passed a str if supervisor.getDevice("2021a-compatibility") is None: # we are running version 2020b so the old command is used return Supervisor.SIMULATION_MODE_RUN else: # webots-2021a removed the RUN mode and now uses FAST print( "This simulator is running a different version of Webots to the " "one that will be used for the next official competition matches " "(You can check the docs to see which version will be used)", file=sys.stderr, ) print( "As such it is possible that some behaviour may not " "match that of the official competition matches", file=sys.stderr, ) return Supervisor.SIMULATION_MODE_FAST
def main(): """Main""" # Get supervisor to take over the world world = Supervisor() n_joints = 10 timestep = int(world.getBasicTimeStep()) # Get and control initial state of salamander reset = RobotResetControl(world, n_joints) # Simulation arguments arguments = world.getControllerArguments() pylog.info("Arguments passed to simulation: {}".format(arguments)) # Exercise example to show how to run a grid search if "example" in arguments: exercise_example(world, timestep, reset) # Exercise 9b - Phase lag + amplitude study if "9b" in arguments: exercise_9b(world, timestep, reset) # Exercise 9c - Gradient amplitude study if "9c" in arguments: exercise_9c(world, timestep, reset) # Exercise 9d1 - Turning if "9d1" in arguments: exercise_9d1(world, timestep, reset) # Exercise 9d2 - Backwards swimming if "9d2" in arguments: exercise_9d2(world, timestep, reset) # Exercise 9f - Walking if "9f" in arguments: exercise_9f(world, timestep, reset) # Exercise 9g - Transitions if "9g" in arguments: exercise_9g(world, timestep, reset) # Pause world.simulationSetMode(world.SIMULATION_MODE_PAUSE) pylog.info("Simulations complete")
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor): robot.getField('customData').setSFString('start') # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME) time_step = int(supervisor.getBasicTimeStep()) duration_ms = time_step * int(1000 * GAME_DURATION_SECONDS // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor, skip_missing=True): robot.getField('customData').setSFString('start') # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(get_simulation_run_mode(supervisor)) time_step = int(supervisor.getBasicTimeStep()) duration = controller_utils.get_match_duration_seconds() duration_ms = time_step * int(1000 * duration // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor, skip_missing=True): inform_start(robot) inform_start(webots_utils.node_from_def(supervisor, 'WALL_CTRL')) # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(get_simulation_run_mode(supervisor)) time_step = int(supervisor.getBasicTimeStep()) duration = controller_utils.get_match_duration_seconds() duration_ms = time_step * int(1000 * duration // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def main(): """Main""" # Duration of each simulation simulation_duration = 20 # Get supervisor to take over the world world = Supervisor() n_joints = 10 timestep = int(world.getBasicTimeStep()) freqs = 1 # Get and control initial state of salamander reset = RobotResetControl(world, n_joints) # Simulation example amplitude = None phase_lag = None turn = None parameter_set = [ [freqs, amplitude, phase_lag, turn], [freqs, amplitude, phase_lag, turn] ] for simulation_i, parameters in enumerate(parameter_set): reset.reset() run_simulation( world, parameters, timestep, int(1000*simulation_duration/timestep), logs="./logs/example/simulation_{}.npz".format(simulation_i) ) # Pause world.simulationSetMode(world.SIMULATION_MODE_PAUSE) pylog.info("Simulations complete") world.simulationQuit(0)
def record_video(supervisor: Supervisor, file_path: Path) -> Iterator[None]: file_path.parent.mkdir(parents=True, exist_ok=True) print("Saving video to {}".format(file_path)) supervisor.movieStartRecording( str(file_path), width=1920, height=1080, quality=100, codec=0, acceleration=1, caption=False, ) yield supervisor.movieStopRecording() while not supervisor.movieIsReady(): time.sleep(0.1) if supervisor.movieFailed(): print("Movie failed to record")
def record_video(supervisor: Supervisor, file_path: Path) -> Iterator[None]: file_path.parent.mkdir(parents=True, exist_ok=True) print("Saving video to {}".format(file_path)) config = controller_utils.get_recording_config() supervisor.movieStartRecording( str(file_path), width=config.resolution.width, height=config.resolution.height, quality=config.quality, codec=0, acceleration=1, caption=False, ) yield supervisor.movieStopRecording() while not supervisor.movieIsReady(): time.sleep(0.1) if supervisor.movieFailed(): print("Movie failed to record")
ANGLE_BINS = 667 MAX_LIDAR_ANGLE = math.radians(240) LIDAR_SENSOR_MAX_RANGE = 5.5 MAP_HEIGHT = 400 MAP_WIDTH = 400 DAMAGE_THRESHOLD = .95 INCREMENT_CMAP = 0.005 NOISE_THRESHOLD = 1.0 FORTY_FIVE_DEGREES = math.pi / 4 NINETY_DEGREES = math.pi / 2 SINUSOIDAL_FUNCTION_MULTIPLIER = 2.0 * math.pi * 0.5 mode = 'mapping' #mode = 'repair' # create the Robot instance. robot = Supervisor() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # Initialize leg motors motorNames = [ 'RPC', 'RPF', 'RPT', 'RMC', 'RMF', 'RMT', 'RAC', 'RAF', 'RAT', 'LPC', 'LPF', 'LPT', 'LMC', 'LMF', 'LMT', 'LAC', 'LAF', 'LAT' ] motors = [] for i in range(0, 18): motors.append(robot.getDevice(motorNames[i])) # Initialize lidar sensor lidar = robot.getDevice('Hokuyo URG-04LX-UG01')
from controller import Robot, Supervisor, Field, Node from Arm import * from Gripper import * from Base import * import numpy as np import math robot = Supervisor() timestep = int(robot.getBasicTimeStep()) # Initialize the base, arm and gripper of the youbot robot base = Base(robot) arm = Arm(robot) gripper = Gripper(robot) # Enable compass/gps modules compass = robot.getDevice('compass') compass.enable(timestep) gps = robot.getDevice('gps') gps.enable(timestep) # Initialize waypoints and state machine initial state waypoints = [(22.26, 24.61), (22.2, 24.6), (22.03, 26.06), (26.0, 26.4), (28.7, 25.0)] #(25.5, 25.0), current_waypoint = waypoints.pop(0) state = 'lower arm' # Establish the gains for feedback controls x_gain = 1.5 theta_gain = 2.0
from controller import Supervisor TIME_STEP = 32 supervisor = Supervisor() ball_node = supervisor.getFromDef("BALL") trans_field = ball_node.getField("translation") radius_field = ball_node.getField("radius") r = radius_field.getSFFloat() INITIAL_POS = [0, r, 0] while supervisor.step(TIME_STEP) != -1: # this is done repeatedly values = trans_field.getSFVec3f() print("MY_ROBOT is at position: %g %g %g" % (values[0], values[1], values[2])) # GOAL TEST if ((values[0] < -4.5 or values[0] > 4.5) and values[2] > -0.75 and values[2] < 0.75 and values[1] < r): trans_field.setSFVec3f(INITIAL_POS) ball_node.resetPhysics() print("goal log") if (values[0] < -4.5 or values[0] > 4.5 or values[2] > 3 or values[2] < -3): ball_node.resetPhysics() print("out of bounds log")
class SupervisorEnv(ABC): """ This class represents the basic template which contains the necessary methods to train a reinforcement learning algorithm. The interface class follows the gym interface which is standardized in much many reinforcement learning algorithms. The OpenAI gym environment can be described by the following figure: +----------+ (action) +---------------+ | |-------------------------------->| | | Agent | | SupervisorEnv | | |<--------------------------------| | +----------+ (observation, reward) +---------------+ """ def __init__(self): self.supervisor = Supervisor() @abstractmethod def get_observations(self): """ Return the observations of the robot. For example, metrics from sensors, camera image etc. :returns: An object of observations """ pass @abstractmethod def step(self, action): """ Each timestep, the agent chooses an action, and the environment returns the observation, the reward and the state of the problem (done or not). observation: The observation from the environment reward: The amount of reward achieved by the previous action. is_done: If the problem is solved info: Diagnostic information mostly useful for debugging. :param action: The agent's action :return: tuple, (observation, reward, is_done, info) """ pass @abstractmethod def get_reward(self, action): """ :param action: The agent's action :return: the amount of reward achieved by the previous action. """ pass @abstractmethod def is_done(self): """ Used to inform the agent that the problem solved. :return: bool, True if the problem have been solved """ pass def reset(self): """ Used to reset the world to an initial state. Default implementation of reset method, using Webots-provided methods. *Note that this works properly only with Webots versions >R2020b and must be overridden with a custom reset method when using earlier versions. It is backwards compatible due to the fact that the new reset method gets overridden by whatever the user has previously implemented, so an old supervisor such as SupervisorCSV can be migrated easily to use this class. :return: default observation provided by get_default_observation() implementation """ self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() return self.get_default_observation() def get_default_observation(self): """ This method should be implemented to return a default/starting observation that is use-case dependant. It is mainly used by the reset implementation above. :return: list-like, contains default agent observation """ return NotImplementedError @abstractmethod def get_info(self): """ Diagnostic information mostly useful for debugging. """ pass
def __del__(self): Supervisor.__del__(self) print("Cleanup") for conn in self._connections: conn.close() # Close the connection self._s.close()
from controller import Supervisor #Create the instance of the supervisor class supervisor = Supervisor() class Human(): '''Human object holding the boundaries''' def __init__(self, node, ap: int, vtype: str, score: int): '''Initialises the radius and position of the human''' self.wb_node = node self.wb_translationField = self.wb_node.getField('translation') self.arrayPosition = ap self.scoreWorth = score self.radius = 0.15 self.victim_type = vtype self.simple_victim_type = self.get_simple_victim_type() self.identified = False @property def position(self) -> list: return self.wb_translationField.getSFVec3f() @position.setter def position(self, pos: list) -> None: self.wb_translationField.setSFVec3f(pos)
Changelog: - Only get points if you have a human loaded and enter a base - Robots can only pick up one human at a time - Robot must have stopped for 2 seconds to deposit and pick up human - Added check for message from position supervisor before taking human positions - Remove clock controls and controller restarting - Remove robot controller handling """ from controller import Supervisor import os import random # Create the instance of the supervisor class supervisor = Supervisor() # Get the robot nodes by their DEF names robot0 = supervisor.getFromDef("ROBOT0") robot1 = supervisor.getFromDef("ROBOT1") # Get the translation fields robot0Pos = robot0.getField("translation") robot1Pos = robot1.getField("translation") # Get the output from the object placement supervisor objectPlacementOutput = supervisor.getFromDef("OBJECTPLACER").getField( "customData") # Get this supervisor node - so that it can be rest when game restarts mainSupervisor = supervisor.getSelf()
'front right 0', 'front right 1', 'front right 2', 'front left 0', 'front left 1', 'front left 2', 'rear', 'rear left', 'rear right', 'right', 'left'] sensors = {} colorFields = {} supervisor = Supervisor() # get and enable the distance sensors for name in sensorsNames: sensors[name] = supervisor.getDevice('distance sensor ' + name) sensors[name].enable(TIME_STEP) defName = name.upper().replace(' ', '_') colorFields[name] = supervisor.getFromDef(defName + '_VISUALIZATION').getField('diffuseColor') # get the color fields childrenField = supervisor.getSelf().getField('children') for i in range(childrenField.getCount()): node = childrenField.getMFNode(i) if node.getTypeName() == 'DistanceSensorVisualization': colorFields[node.getDef()] = node.getField('diffuseColor') colorFields[node.getDef()].setSFColor([0.0, 1.0, 0.0])
supervisor.step(10 * timeStep) take_screenshot(camera, category, objectDirectory, os.path.dirname(protoPath), protoName, options, background, colorThreshold, shadowColor) # remove the object supervisor.step(timeStep) count = rootChildrenfield.getCount() importedNode.remove() supervisor.step(timeStep) if rootChildrenfield.getCount() != count - 1: sys.exit(protoName + ' was not removed sucessfully.') # Initialize the Supervisor. controller = Supervisor() timeStep = int(controller.getBasicTimeStep()) camera = controller.getCamera('camera') camera.enable(timeStep) options = get_options() if os.path.exists('.' + os.sep + 'images'): shutil.rmtree('.' + os.sep + 'images') # Get required fields rootChildrenfield = controller.getRoot().getField('children') supervisorTranslation = controller.getFromDef('SUPERVISOR').getField( 'translation') supervisorRotation = controller.getFromDef('SUPERVISOR').getField('rotation') viewpointPosition = controller.getFromDef('VIEWPOINT').getField('position') viewpointOrientation = controller.getFromDef('VIEWPOINT').getField(
translationValues = np.array(self.translationField.getSFVec3f()) translationStep = np.array([0., 0., -0.005*self.movementFactor]) self.translationField.setSFVec3f((translationValues + translationStep).tolist()); elif k==ord('W'): print 'translate front' translationValues = np.array(self.translationField.getSFVec3f()) translationStep = np.array([-0.005*self.movementFactor, 0., 0.]) self.translationField.setSFVec3f((translationValues + translationStep).tolist()); elif k==ord('S'): print 'translate rear' translationValues = np.array(self.translationField.getSFVec3f()) translationStep = np.array([0.005*self.movementFactor, 0., 0.]) self.translationField.setSFVec3f((translationValues + translationStep).tolist()); elif k==ord('J'): print 'rotate left' rotationValues = self.rotationField.getSFRotation() rotationStep = np.array([0.,0.,0.,0.02*self.movementFactor]) self.rotationField.setSFRotation((rotationValues + rotationStep).tolist()); elif k==ord('L'): print 'rotate right' rotationValues = self.rotationField.getSFRotation() rotationStep = np.array([0.,0.,0.,-0.02*self.movementFactor]) self.rotationField.setSFRotation((rotationValues + rotationStep).tolist()); if self.step(self.timeStep) == -1: break controller = Supervisor() controller.initialization() controller.run()
def __init__(self): Supervisor.__init__(self) self.updates = [] # list of objects for which location updates are requested
class TerritoryController: _emitters: Dict[StationCode, Emitter] _receivers: Dict[StationCode, Receiver] def __init__(self, claim_log: ClaimLog, attached_territories: AttachedTerritories) -> None: self._claim_log = claim_log self._attached_territories = attached_territories self._robot = Supervisor() self._claim_timer = ActionTimer(2, self.handle_claim_timer_tick) self._connected_territories = self._attached_territories.build_attached_capture_trees( ) self._emitters = { station_code: get_robot_device(self._robot, station_code + "Emitter", Emitter) for station_code in StationCode } self._receivers = { station_code: get_robot_device(self._robot, station_code + "Receiver", Receiver) for station_code in StationCode } for receiver in self._receivers.values(): receiver.enable(RECEIVE_TICKS) for station_code in StationCode: self.set_node_colour(station_code, ZONE_COLOURS[Claimant.UNCLAIMED]) for claimant in Claimant.zones(): self.set_score_display(claimant, 0) def set_node_colour(self, node_id: str, new_colour: Tuple[float, float, float]) -> None: node = self._robot.getFromDef(node_id) if node is None: logging.error(f"Failed to fetch node {node_id}") else: set_node_colour(node, new_colour) def set_territory_ownership( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: station = self._robot.getFromDef(station_code) if station is None: logging.error(f"Failed to fetch territory node {station_code}", ) return new_colour = ZONE_COLOURS[claimed_by] set_node_colour(station, new_colour) self._claim_log.log_territory_claim(station_code, claimed_by, claim_time) def prune_detached_stations( self, connected_territories: Tuple[Set[StationCode], Set[StationCode]], claim_time: float, ) -> None: broken_links = False # skip regenerating capture trees unless something changed # find territories which lack connections back to their claimant's corner for station in StationCode: # for territory in station_codes if self._claim_log.get_claimant(station) == Claimant.UNCLAIMED: # unclaimed territories can't be pruned continue if station in connected_territories[0]: # territory is linked back to zone 0's starting corner continue if station in connected_territories[1]: # territory is linked back to zone 1's starting corner continue # all disconnected territory is unclaimed self.set_territory_ownership(station, Claimant.UNCLAIMED, claim_time) broken_links = True if broken_links: self._connected_territories = \ self._attached_territories.build_attached_capture_trees() def claim_territory( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: if not self._attached_territories.can_capture_station( station_code, claimed_by, self._connected_territories, ): # This claimant doesn't have a connection back to their starting zone logging.error( f"Robot in zone {claimed_by} failed to capture {station_code}") return if claimed_by == self._claim_log.get_claimant(station_code): logging.error( f"{station_code} already owned by {claimed_by.name}, resetting to UNCLAIMED", ) claimed_by = Claimant.UNCLAIMED self.set_territory_ownership(station_code, claimed_by, claim_time) # recalculate connected territories to account for # the new capture and newly created islands self._connected_territories = self._attached_territories.build_attached_capture_trees( ) self.prune_detached_stations(self._connected_territories, claim_time) def process_packet( self, station_code: StationCode, packet: bytes, receive_time: float, ) -> None: try: robot_id, is_conclude = struct.unpack( "!BB", packet) # type: Tuple[int, int] claimant = Claimant(robot_id) operation_args = (station_code, claimant, receive_time) if is_conclude: if self._claim_timer.has_begun_action_in_time_window( *operation_args): self.claim_territory(*operation_args) else: self._claim_timer.begin_action(*operation_args) except ValueError: logging.error( f"Received malformed packet at {receive_time} on {station_code}: {packet!r}", ) def receive_territory(self, station_code: StationCode, receiver: Receiver) -> None: simulation_time = self._robot.getTime() while receiver.getQueueLength(): try: data = receiver.getData() self.process_packet(station_code, data, simulation_time) finally: # Always advance to the next packet in queue: if there has been an exception, # it is safer to advance to the next. receiver.nextPacket() def update_territory_links(self) -> None: for stn_a, stn_b in TERRITORY_LINKS: if isinstance(stn_a, TerritoryRoot): # starting zone is implicitly owned if stn_a == TerritoryRoot.z0: stn_a_claimant = Claimant.ZONE_0 else: stn_a_claimant = Claimant.ZONE_1 else: stn_a_claimant = self._claim_log.get_claimant(stn_a) stn_b_claimant = self._claim_log.get_claimant(stn_b) # if both ends are owned by the same Claimant if stn_a_claimant == stn_b_claimant: claimed_by = stn_a_claimant else: claimed_by = Claimant.UNCLAIMED self.set_node_colour(f'{stn_a}-{stn_b}', LINK_COLOURS[claimed_by]) def update_displayed_scores(self) -> None: scores = self._claim_log.get_scores() for zone, score in scores.items(): self.set_score_display(zone, score) def set_score_display(self, zone: Claimant, score: int) -> None: # the text is not strictly monospace # but the subset of characters used roughly approximates this character_width = 40 character_spacing = 4 starting_spacing = 2 score_display = get_robot_device( self._robot, f'SCORE_DISPLAY_{zone.value}', Display, ) # fill with background colour score_display.setColor(0x183acc) score_display.fillRectangle( 0, 0, score_display.getWidth(), score_display.getHeight(), ) # setup score text score_display.setColor(0xffffff) score_display.setFont('Arial Black', 48, True) score_str = str(score) # Approx center value x_used = ( len(score_str) * character_width + # pixels used by characters (len(score_str) - 1) * character_spacing # pixels used between characters ) x_offset = int( (score_display.getWidth() - x_used) / 2) - starting_spacing # Add the score value score_display.drawText(score_str, x_offset, 8) def get_tower_led(self, station_code: StationCode, led: int) -> LED: return get_robot_device( self._robot, f"{station_code.value}Territory led{led}", LED, ) def handle_claim_timer_tick( self, station_code: StationCode, claimant: Claimant, progress: float, prev_progress: float, ) -> None: zone_colour = convert_to_led_colour(ZONE_COLOURS[claimant]) if progress in {ActionTimer.TIMER_EXPIRE, ActionTimer.TIMER_COMPLETE}: for led in range(NUM_TOWER_LEDS): tower_led = self.get_tower_led(station_code, led) if tower_led.get() == zone_colour: tower_led.set(0) else: # map the progress value to the LEDs led_progress = min(int(progress * NUM_TOWER_LEDS), NUM_TOWER_LEDS - 1) led_prev_progress = min(int(prev_progress * NUM_TOWER_LEDS), NUM_TOWER_LEDS - 1) if led_progress == led_prev_progress and prev_progress != 0: # skip setting an LED that was already on return tower_led = self.get_tower_led(station_code, led_progress) if led_progress == NUM_TOWER_LEDS - 1: if not self._attached_territories.can_capture_station( station_code, claimant, self._connected_territories, ): # station can't be captured by this team, the claim will fail # skip setting top LED return tower_led.set(zone_colour) def receive_robot_captures(self) -> None: for station_code, receiver in self._receivers.items(): self.receive_territory(station_code, receiver) if self._claim_log.is_dirty(): self.update_territory_links() self.update_displayed_scores() self._claim_log.record_captures() def transmit_pulses(self) -> None: for station_code, emitter in self._emitters.items(): emitter.send( struct.pack( "!2sb", station_code.encode("ASCII"), int(self._claim_log.get_claimant(station_code)), ), ) def main(self) -> None: timestep = self._robot.getBasicTimeStep() steps_per_broadcast = (1 / BROADCASTS_PER_SECOND) / (timestep / 1000) counter = 0 while True: counter += 1 self.receive_robot_captures() current_time = self._robot.getTime() self._claim_timer.tick(current_time) if counter > steps_per_broadcast: self.transmit_pulses() counter = 0 self._robot.step(int(timestep))
virtualMarkers.append(prefix + 'O') virtualMarkers.append(prefix + 'A') virtualMarkers.append(prefix + 'L') virtualMarkers.append(prefix + 'P') if name in virtualMarkers: return True return False @staticmethod def removeMarkers(): markerField = supervisor.getSelf().getField('markers') for i in range(markerField.getCount()): markerField.removeMF(-1) supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) enableValueGraphs = [] # parse arguments if len(sys.argv) < 3 or sys.argv[1] == 'None': sys.exit('C3D file not defined.') playbackSpeed = float(sys.argv[2]) # make one step to be sure markers are not imported before pressing play supervisor.step(timestep) # remove possible previous marker (at regeneration for example) c3dFile.removeMarkers()
"""weighted_landmark_supervisor controller.""" # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Supervisor import struct from functions import * # Import Simulations Stuff from simulation import * # Set up Webots stuff WALL_THR = 300 supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) emitter = supervisor.getDevice('emitter') # Set up Simulation stuff simulation = Simulations(supervisor, timestep) simulation.plt_only_weights_and_vectors(to_plot='W', fig_tag=-1) for t in range(0, int(total_time / dt)): dumy = supervisor.step(timestep) timestep = int(supervisor.getBasicTimeStep()) simulation.sim_pre_step(t, emitter, supervisor, timestep) max_tdelta = simulation.agents.get_max_webot_tdelta() for i in range(0, max_tdelta): dumy = supervisor.step(timestep)
from controller import Robot, Supervisor import numpy as np from scipy.optimize import minimize, basinhopping, brute, differential_evolution, shgo, dual_annealing import fileinput TIME_STEP = 3 supervisor = Supervisor() #initial guess for the knee and heel levers #the y component of KNEE_INITIAL is the lever length #the z component of HEEL_INITIAL is the lever length KNEE_INITIAL = np.array([0.0, 0.325, -0.01]) HEEL_INITIAL = np.array([0.0, 0.0, 0.15]) #these are ideal values for a 80kg robot #KNEE_TENDON_INITIAL = 143526 #HEEL_TENDON_INITIAL = 160106 #this is the initial guess for the tendon values KNEE_TENDON_INITIAL = 25000 HEEL_TENDON_INITIAL = 25000 #get a handle to the root node and create the robot root = supervisor.getRoot() children = root.getField("children") children.importMFNode(-1, "Robot.wbo") #set the leg spring constants to the initial values change_spring_constant(int(x[0]), int(x[1]))
from controller import Motor from controller import Camera from controller import DistanceSensor from controller import TouchSensor import endpoint_env import preprocessing from pipe import make_pipe, close_pipe, open_write_pipe, open_read_pipe, write_to_pipe, read_from_pipe from ikpy.chain import Chain from ikpy.link import OriginLink, URDFLink from ArmUtil import ToArmCoord, PSFunc import PandaController from controller import PositionSensor armChain = Chain.from_urdf_file("panda_with_bound.urdf") supervisor = Supervisor() # create the Robot instance. env = endpoint_env.EndP_env3D(supervisor) env = preprocessing.RobotPreprocessing(env) # rl = DDPG(2, 9, [-1, 1]) # get the time step of the current world. timestep = env.timestep # Environment Reset def reset(): env.reset()
from os import truncate from controller import Robot, Supervisor, Display from shapely.geometry import Polygon #for rectangle intersections from shapely.geometry import Point import time supervisor = Supervisor() global TIMESTEP TIMESTEP = int(supervisor.getBasicTimeStep()) robotNode = supervisor.getRoot() children = robotNode.getField("children") robot = children.getMFNode(5) startTime = time.time() #get devices gyro = supervisor.getDevice("gyro") accel = supervisor.getDevice("accelerometer") gyro.enable(TIMESTEP) accel.enable(TIMESTEP) right_shoulder_pitch = supervisor.getDevice("RShoulderPitch") left_shoulder_pitch = supervisor.getDevice("LShoulderPitch") right_torso_pitch = supervisor.getDevice("RHipPitch") left_torso_pitch = supervisor.getDevice("LHipPitch")
def __init__(self): self.supervisor = Supervisor()