def get_robots(supervisor: Supervisor, *, skip_missing: bool = False) -> List[Tuple[int, Node]]: """ Get a list of (zone id, robot node) tuples. By default this raises a `ValueError` if it fails to fetch a robot node for a given zone, however this behaviour can be altered if the caller wishes to instead skip the missing nodes. This should only be done if the caller has already validated that the node ids being used for the lookups are definitely valid (and that therefore missing nodes are expected rather than a signal of an internal error). """ robots = [] # List[Tuple[int, Supervisor]] for zone_id in range(controller_utils.NUM_ZONES): robot = supervisor.getFromDef(f"ROBOT-{zone_id}") if robot is None: if skip_missing: continue msg = "Failed to get Webots node for zone {}".format(zone_id) print(msg) raise ValueError(msg) robots.append((zone_id, robot)) return robots
def main(): supervisor = Supervisor() # set positions of the robot and pedestrian human_node = supervisor.getFromDef("HUMAN") trans_field = human_node.getField("translation") print(trans_field.getSFVec3f()) pos = getPosition() trans_field.setSFVec3f([pos[0], pos[1], 0]) print(trans_field.getSFVec3f())
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
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))
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( 'orientation') cameraNear = controller.getFromDef('CAMERA').getField('near') if options.singleShot: node = controller.getFromDef('OBJECTS') if node is None: sys.exit('No node "OBJECTS" found.') take_original_screenshot(camera, '.' + os.sep + 'images') take_screenshot(camera, 'objects', '.' + os.sep + 'images', os.path.dirname(controller.getWorldPath()), node.getTypeName(), None) elif options.appearance:
- Places humans and obstacles randomly - Will not intersect walls or other objects - Will not be within 4 units of bases Changelog: - """ from controller import Supervisor import random # Create the instance of the supervisor class supervisor = Supervisor() # Get field to output information to outputField = supervisor.getFromDef("OBJECTPLACER").getField("customData") # Standard human radius humanRadius = 0.35 def getAllWalls(numberWalls: int) -> list: '''Returns a 3D list of each wall, containing a x,y,z position and x,y,z scale''' # List to contain all the walls walls = [] # Iterate for each of the walls for wallNumber in range(0, numberWalls): # Get the wall's position from the wall solid object wallObj = supervisor.getFromDef("WALL" + str(wallNumber)) position = wallObj.getField("translation").getSFVec3f()
sys.path.append(includePath) from robotbenchmark import robotbenchmarkRecord except ImportError: sys.stderr.write("Warning: 'robotbenchmark' module not found.\n") sys.exit(0) # Get random generator seed value from 'controllerArgs' field seed = 1 if len(sys.argv) > 1 and sys.argv[1].startswith('seed='): seed = int(sys.argv[1].split('=')[1]) robot = Supervisor() timestep = int(robot.getBasicTimeStep()) jointParameters = robot.getFromDef("PENDULUM_PARAMETERS") positionField = jointParameters.getField("position") emitter = robot.getEmitter("emitter") time = 0 force = 0 forceStep = 800 random.seed(seed) run = True while robot.step(timestep) != -1: if run: time = robot.getTime() robot.wwiSendText("time:%-24.3f" % time) robot.wwiSendText("force:%.2f" % force)
from controller import Supervisor import math supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) blue_score = 0 red_score = 0 blue_goal = supervisor.getFromDef("_blue_goal") red_goal = supervisor.getFromDef("_red_goal") def updateScore(): supervisor.setLabel(0, "RED " + str(red_score), 0, 0, 0.1, 0xff0000, 0, "Impact") supervisor.setLabel(1, "BLUE " + str(blue_score), 0, 0.05, 0.1, 0x0000ff, 0, "Impact") def check_goals(): def check_goal(ball, goal): ball_pos = ball.getPosition() goal_pos = goal.getPosition() goal_angle = goal.getField("rotation").getSFRotation()[ 3] # [0, -1, 0, angle] goal_width = 1 # meter goal_precision = 0.05 # meter # pole vector
def node_from_def(supervisor: Supervisor, name: str) -> Node: node = supervisor.getFromDef(name) if node is None: raise ValueError(f"Unable to fetch node {name!r} from Webots") return node
"""coin handler.""" from controller import Supervisor import math supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) player = supervisor.getFromDef("kedi") ACCEPT_DISTANCE = 0.5 coinRoot = supervisor.getFromDef("COINS").getField("children") def make_coin(pos): coinRoot.importMFNode(0, "../../protos/coin.wbo") coin = coinRoot.getMFNode(0) translation = coin.getField("translation") translation.setSFVec3f(pos) def update(): coinRoot = supervisor.getFromDef("COINS").getField("children") count = coinRoot.getCount() if count == 0: supervisor.setLabel(1, "VICTORY", 0.35, 0.35, 0.2, 0xffffff, 0)
"""Supervisor Controller Prototype v3.1 for prototype release Appended from Robbie's Supervisor Controller Prototype v2 Written by Robbie Goldman and Alfred Roberts """ 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") #Maximum time for a match maxTime = 120 class Robot: '''Robot object to hold values whether its in a base or holding a human''' def __init__(self): '''Initialises the in a base, has a human loaded and score values''' self.inBase = True self.humanLoaded = False self.score = 0
import os sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__), '../..'))) import pycontroller as ctrl import numpy as np DEBUG = True MAX_SPEED = 440 mult = 1 odd = -mult * MAX_SPEED even = mult * MAX_SPEED super = Supervisor() drone = super.getFromDef('drone') prop1 = super.getDevice("prop1") prop2 = super.getDevice("prop2") prop3 = super.getDevice("prop3") prop4 = super.getDevice("prop4") prop5 = super.getDevice("prop5") prop6 = super.getDevice("prop6") gyro = super.getDevice('GYRO') gps = super.getDevice('GPS') compass = super.getDevice('COMPASS') prop1.setPosition(float('-inf')) prop2.setPosition(float('+inf')) prop3.setPosition(float('-inf')) prop4.setPosition(float('+inf')) prop5.setPosition(float('-inf'))
class TerritoryController: _emitters: Dict[StationCode, Emitter] _receivers: Dict[StationCode, Receiver] def __init__(self, claim_log: ClaimLog) -> None: self._claim_log = claim_log self._robot = Supervisor() self._claim_starts: Dict[Tuple[StationCode, Claimant], float] = {} 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) def begin_claim( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: self._claim_starts[station_code, claimed_by] = claim_time def has_begun_claim_in_time_window( self, station_code: StationCode, claimant: Claimant, current_time: float, ) -> bool: try: start_time = self._claim_starts[station_code, claimant] except KeyError: return False time_delta = current_time - start_time return 1.8 <= time_delta <= 2.1 def claim_territory( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: if self._claim_log.get_claimant(station_code) == claimed_by: # This territory is already claimed by this claimant. return new_colour = ZONE_COLOURS[claimed_by] self._robot.getFromDef(station_code).getField("zoneColour").setSFColor( list(new_colour), ) self._claim_log.log_territory_claim(station_code, claimed_by, self._robot.getTime()) 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) if is_conclude: if self.has_begun_claim_in_time_window( station_code, claimant, receive_time, ): self.claim_territory( station_code, claimant, receive_time, ) else: self.begin_claim( station_code, claimant, receive_time, ) except ValueError: print( # noqa:T001 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 receive_robot_captures(self) -> None: for station_code, receiver in self._receivers.items(): self.receive_territory(station_code, receiver) 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() if counter > steps_per_broadcast: self.transmit_pulses() counter = 0 self._robot.step(int(timestep))
class ArmEnv(object): def __init__(self, step_max=100, add_noise=False): self.observation_dim = 12 self.action_dim = 6 """ state """ self.state = np.zeros(self.observation_dim) self.init_state = np.zeros(self.observation_dim) self.uncode_init_state = np.zeros(self.observation_dim) """ action """ self.action_high_bound = 1 self.action = np.zeros(self.action_dim) """ reward """ self.step_max = step_max self.insert_depth = 40 """setting""" self.add_noise = add_noise # or True """information for action and state""" self.action_space = spaces.Box(low=-0.4, high=0.4, shape=(self.action_dim, ), dtype=np.float32) self.observation_space = spaces.Box(low=-10, high=10, shape=(self.observation_dim, ), dtype=np.float32) """Enable PD controler""" self.pd = True """Setup controllable variables""" self.movementMode = 1 # work under FK(0) or IK(1) """Timer""" self.timer = 0 """Initialize the Webots Supervisor""" self.supervisor = Supervisor() self.timeStep = int(8) # TODO: It's there a way to start simulation automatically? '''enable world devices''' # Initialize the arm motors. self.motors = [] for motorName in [ 'A motor', 'B motor', 'C motor', 'D motor', 'E motor', 'F motor' ]: motor = self.supervisor.getMotor(motorName) self.motors.append(motor) # Get the arm, target and hole nodes. self.target = self.supervisor.getFromDef('TARGET') self.arm = self.supervisor.getFromDef('ARM') self.hole = self.supervisor.getFromDef('HOLE') self.armEnd = self.supervisor.getFromDef('INIT') # Get the absolute position of the arm base and target. self.armPosition = self.arm.getPosition() self.targetPosition = self.target.getPosition() self.initPosition = self.armEnd.getPosition() # Get the translation field fo the hole self.hole_translation = self.hole.getField('translation') self.hole_rotation = self.hole.getField('rotation') # Get initial position of hole self.hole_new_position = [] self.hole_new_rotation = [] self.hole_init_position = self.hole_translation.getSFVec3f() self.hole_init_rotation = self.hole_rotation.getSFRotation() print("Hole init position", self.hole_init_position) print("Hole init rotation", self.hole_init_rotation) # get and enable sensors # Fxyz: N, Txyz: N*m self.fz_sensor = self.supervisor.getMotor('FZ_SENSOR') self.fz_sensor.enableForceFeedback(self.timeStep) self.fx_sensor = self.supervisor.getMotor('FX_SENSOR') self.fx_sensor.enableForceFeedback(self.timeStep) self.fy_sensor = self.supervisor.getMotor('FY_SENSOR') self.fy_sensor.enableForceFeedback(self.timeStep) self.tx_sensor = self.supervisor.getMotor('TX_SENSOR') self.tx_sensor.enableTorqueFeedback(self.timeStep) self.ty_sensor = self.supervisor.getMotor('TY_SENSOR') self.ty_sensor.enableTorqueFeedback(self.timeStep) self.tz_sensor = self.supervisor.getMotor('TZ_SENSOR') self.tz_sensor.enableTorqueFeedback(self.timeStep) self.FZ = self.fz_sensor.getForceFeedback() self.FX = self.fx_sensor.getForceFeedback() self.FY = self.fy_sensor.getForceFeedback() self.TX = self.tx_sensor.getTorqueFeedback() self.TY = self.ty_sensor.getTorqueFeedback() self.TZ = self.tz_sensor.getTorqueFeedback() # # Used to plot F/T_t # self.plt_FX = [] # self.plt_FY = [] # self.plt_FZ = [] # self.plt_TX = [] # self.plt_TY = [] # self.plt_TZ = [] # self.plt_time = [] # self.plt_current_time = 0 """Initial Position of the robot""" # x/y/z in meters relative to world frame self.x = 0 self.y = 0 self.z = 0 # alpha/beta/gamma in rad relative to initial orientation self.alpha = 0 self.beta = 0 self.gamma = 0 """reset world""" self.reset() def step(self, action): self.supervisor.step(self.timeStep) self.timer += 1 # read state uncode_state, self.state = self.__get_state() # # record graph # self.plt_current_time += self.timeStep * 0.001 # self.plt_time.append(self.plt_current_time) # self.plt_FX.append(self.state[6]) # self.plt_FY.append(self.state[7]) # self.plt_FZ.append(self.state[8]) # self.plt_TX.append(self.state[9]) # self.plt_TY.append(self.state[10]) # self.plt_TZ.append(self.state[11]) # adjust action to usable motion action = cal.actions(self.state, action, self.pd) # print('action', action) # take actions self.__execute_action(action) # uncode_state, self.state = self.__get_state() # safety check safe = cal.safetycheck(self.state) # done and reward r, done = cal.reward_step(self.state, safe, self.timer) return self.state, uncode_state, r, done, safe def directstep(self, action): self.supervisor.step(self.timeStep) self.timer += 1 uncode_state, self.state = self.__get_state() # # record graph # self.plt_current_time += self.timeStep * 0.001 # self.plt_time.append(self.plt_current_time) # self.plt_FX.append(self.state[6]) # self.plt_FY.append(self.state[7]) # self.plt_FZ.append(self.state[8]) # self.plt_TX.append(self.state[9]) # self.plt_TY.append(self.state[10]) # self.plt_TZ.append(self.state[11]) # print('action', action) # take actions self.__execute_action(action) # uncode_state, self.state = self.__get_state() # safety check safe = cal.safetycheck(self.state) # done and reward r, done = cal.reward_step(self.state, safe, self.timer) return self.state, r, done, safe def restart(self): """restart world""" cal.clear() #print("*******************************world restart*******************************") '''set random position for hole''' hole_new_position = self.hole_new_position hole_new_rotation = self.hole_new_rotation self.hole_translation.setSFVec3f( [hole_new_position[0], 2, hole_new_position[2]]) self.hole_rotation.setSFRotation([ hole_new_rotation[0], hole_new_rotation[1], hole_new_rotation[2], hole_new_rotation[3] ]) '''reset signals''' self.timer = 0 "Initial Position of the robot" # x/y/z in meters relative to world frame self.x = self.initPosition[0] - self.armPosition[0] self.y = -(self.initPosition[2] - self.armPosition[2]) self.z = self.initPosition[1] - self.armPosition[1] # alpha/beta/gamma in rad relative to initial orientation self.alpha = 0.0 self.beta = 0.0 self.gamma = 0.0 # Call "ikpy" to compute the inverse kinematics of the arm. # ikpy only compute position ikResults = armChain.inverse_kinematics([[1, 0, 0, self.x], [0, 1, 0, self.y], [0, 0, 1, self.z], [0, 0, 0, 1]]) # Actuate the 3 first arm motors with the IK results. for i in range(3): self.motors[i].setPosition(ikResults[i + 1]) self.motors[3].setPosition(self.alpha) self.motors[4].setPosition(-ikResults[2] - ikResults[3] + self.beta) self.motors[5].setPosition(self.gamma) for i in range(6): self.motors[i].setVelocity(1.0) """wait for robot to move to initial place""" for i in range(20): self.supervisor.step(self.timeStep) for i in range(6): self.motors[i].setVelocity(0.07) '''state''' # get self.uncode_init_state, self.init_state, = self.__get_state() # '''reset graph''' # # Used to plot F/T_t # self.plt_FX.clear() # self.plt_FY.clear() # self.plt_FZ.clear() # self.plt_TX.clear() # self.plt_TY.clear() # self.plt_TZ.clear() # self.plt_time.clear() # self.plt_current_time = 0 # print('initial state:') # print("State 0-3", self.init_state[0:3]) # print("State 3-6", self.init_state[3:6]) # print("State 6-9", self.init_state[6:9]) # print("State 9-12", self.init_state[9:12]) done = False # reset simulation self.supervisor.simulationResetPhysics() return self.init_state, self.uncode_init_state, done def reset(self): """restart world""" cal.clear() #print("*******************************world rested*******************************") '''set random position for hole''' self.hole_new_position = self.hole_init_position + (np.random.rand(3) - 0.5) / 500 self.hole_new_rotation = self.hole_init_rotation + (np.random.rand(4) - 0.5) / 80 self.hole_translation.setSFVec3f( [self.hole_new_position[0], 2, self.hole_new_position[2]]) self.hole_rotation.setSFRotation([ self.hole_new_rotation[0], self.hole_new_rotation[1], self.hole_new_rotation[2], self.hole_new_rotation[3] ]) '''reset signals''' self.timer = 0 "Initial Position of the robot" # x/y/z in meters relative to world frame self.x = self.initPosition[0] - self.armPosition[0] self.y = -(self.initPosition[2] - self.armPosition[2]) self.z = self.initPosition[1] - self.armPosition[1] # alpha/beta/gamma in rad relative to initial orientation self.alpha = 0.0 self.beta = 0.0 self.gamma = 0.0 # Call "ikpy" to compute the inverse kinematics of the arm. # ikpy only compute position ikResults = armChain.inverse_kinematics([[1, 0, 0, self.x], [0, 1, 0, self.y], [0, 0, 1, self.z], [0, 0, 0, 1]]) # Actuate the 3 first arm motors with the IK results. for i in range(3): self.motors[i].setPosition(ikResults[i + 1]) self.motors[3].setPosition(self.alpha) self.motors[4].setPosition(-ikResults[2] - ikResults[3] + self.beta) self.motors[5].setPosition(self.gamma) for i in range(6): self.motors[i].setVelocity(1.0) """wait for robot to move to initial place""" for i in range(20): self.supervisor.step(self.timeStep) for i in range(6): self.motors[i].setVelocity(0.07) '''state''' # get self.uncode_init_state, self.init_state, = self.__get_state() # '''reset graph''' # # Used to plot F/T_t # self.plt_FX.clear() # self.plt_FY.clear() # self.plt_FZ.clear() # self.plt_TX.clear() # self.plt_TY.clear() # self.plt_TZ.clear() # self.plt_time.clear() # self.plt_current_time = 0 # print('initial state:') # print("State 0-3", self.init_state[0:3]) # print("State 3-6", self.init_state[3:6]) # print("State 6-9", self.init_state[6:9]) # print("State 9-12", self.init_state[9:12]) done = False # reset simulation self.supervisor.simulationResetPhysics() return self.init_state, self.uncode_init_state, done def __get_state(self): self.FZ = self.fz_sensor.getForceFeedback() self.FX = self.fx_sensor.getForceFeedback() self.FY = self.fy_sensor.getForceFeedback() self.TX = self.tx_sensor.getTorqueFeedback() self.TY = self.ty_sensor.getTorqueFeedback() self.TZ = -self.tz_sensor.getTorqueFeedback() currentPosition = [] currentPosition.append(self.targetPosition[0] - (self.x + self.armPosition[0])) currentPosition.append(self.targetPosition[2] - (self.armPosition[2] - self.y)) currentPosition.append(self.targetPosition[1] - (self.z + self.armPosition[1] - 0.14)) # state state = np.concatenate( (currentPosition, [self.alpha, self.beta, self.gamma], [self.FX, self.FY, self.FZ], [self.TX, self.TY, self.TZ])) code_state = cal.code_state(state) return state, code_state def __execute_action(self, action): """ execute action """ # do action self.x += action[0] self.y += action[1] self.z -= action[2] self.alpha += action[3] self.beta += action[4] self.gamma -= action[5] # bound position self.x = np.clip(self.x, self.initPosition[0] - self.armPosition[0] - 0.02, self.initPosition[0] - self.armPosition[0] + 0.02) self.y = np.clip(self.y, self.armPosition[2] - self.initPosition[2] - 0.02, self.armPosition[2] - self.initPosition[2] + 0.02) self.z = np.clip(self.z, self.initPosition[1] - self.armPosition[1] - 0.06, self.initPosition[1] - self.armPosition[1] + 0.04) self.alpha = np.clip(self.alpha, -1, 1) self.beta = np.clip(self.beta, -1, 1) self.gamma = np.clip(self.gamma, -1, 1) # Call "ikpy" to compute the inverse kinematics of the arm. # ikpy only compute position ikResults = armChain.inverse_kinematics([[1, 0, 0, self.x], [0, 1, 0, self.y], [0, 0, 1, self.z], [0, 0, 0, 1]]) # Actuate the 3 first arm motors with the IK results. for i in range(3): self.motors[i].setPosition(ikResults[i + 1]) self.motors[3].setPosition(self.alpha) self.motors[4].setPosition(-ikResults[2] - ikResults[3] + self.beta) self.motors[5].setPosition(self.gamma) def test_action(self, action): self.supervisor.step(self.timeStep) """ execute action """ # do action self.x += action[0] self.y += action[1] self.z -= action[2] self.alpha += action[3] self.beta += action[4] self.gamma -= action[5] # bound position self.x = np.clip(self.x, 0.94455 - self.armPosition[0] - 0.02, 0.94455 - self.armPosition[0] + 0.02) self.y = np.clip(self.y, self.armPosition[2] - 0.02, self.armPosition[2] + 0.02) self.z = np.clip(self.z, 2.255 - self.armPosition[1] - 0.06, 2.255 - self.armPosition[1] + 0.04) self.alpha = np.clip(self.alpha, -1, 1) self.beta = np.clip(self.beta, -1, 1) self.gamma = np.clip(self.gamma, -1, 1) # Call "ikpy" to compute the inverse kinematics of the arm. # ikpy only compute position ikResults = armChain.inverse_kinematics([[1, 0, 0, self.x], [0, 1, 0, self.y], [0, 0, 1, self.z], [0, 0, 0, 1]]) # ikResults = armChain.inverse_kinematics(self.EulerToMatrix(self.end_effector[:3], self.end_effector[3:6])) # print('ikresults', ikResults) # Actuate the 3 first arm motors with the IK results. # for i in range(6): # self.motors[i].setPosition(ikResults[i + 1]) # Actuate the 3 first arm motors with the IK results. for i in range(3): self.motors[i].setPosition(ikResults[i + 1]) self.motors[3].setPosition(self.alpha) self.motors[4].setPosition(-ikResults[2] - ikResults[3] + self.beta) self.motors[5].setPosition(self.gamma) _, self.state = self.__get_state() return self.state @staticmethod def sample_action(): return (np.random.rand(6) - 0.5) / 10
armChain = Chain.from_urdf_file(filename) for i in [0, 6]: armChain.active_links_mask[0] = False # Initialize the arm motors and encoders. motors = [] for link in armChain.links: if 'motor' in link.name: motor = supervisor.getDevice(link.name) motor.setVelocity(1.0) position_sensor = motor.getPositionSensor() position_sensor.enable(timeStep) motors.append(motor) # Get the arm and target nodes. target = supervisor.getFromDef('TARGET') arm = supervisor.getSelf() # Loop: Move the arm hand to the target. print('Move the yellow and black sphere to move the arm...') while supervisor.step(timeStep) != -1: # Get the absolute postion of the target and the arm base. targetPosition = target.getPosition() armPosition = arm.getPosition() # Compute the position of the target relatively to the arm. # x and y axis are inverted because the arm is not aligned with the Webots global axes. x = targetPosition[0] - armPosition[0] y = -(targetPosition[2] - armPosition[2]) z = targetPosition[1] - armPosition[1]
class BigBrother(Spot_Node): """Supervisor class for Webots """ def __init__(self): self.supervisor = Supervisor() super().__init__(self.supervisor) self.spot_node = self.supervisor.getFromDef( "Spot_Node") # Taken from DEF in .wbt file self.rot_vec = self.spot_node.getField("rotation") self.com_node = self.supervisor.getFromDef("CentMass") self.goal_pt = np.array([0, 0, -16.71]) self.goal_rad = 2 def check_terminal(self, pos): if np.linalg.norm(pos - self.goal_pt) < self.goal_rad: return True else: return False def actuate_motors(self, motor_vals): for i, motor in enumerate(self.motors): # TODO - Wait for motors to reach position as given in docs: https://cyberbotics.com/doc/reference/motor?tab-language=python#wb_motor_set_position motor.setPosition(float(motor_vals[i])) return self.robot.step(int(2 * self.time_step)), self.check_terminal( self.spot_node.getPosition()) def action_rollout(self, motor_vals): self.update_com() return self.actuate_motors(motor_vals) def update_com(self): com = self.spot_node.getCenterOfMass() trans_field = self.com_node.getField("translation") trans_field.setSFVec3f(com) def reset_run_mode(self): self.supervisor.simulationSetMode(self.supervisor.SIMULATION_MODE_RUN) self.supervisor.simulationSetMode( self.supervisor.SIMULATION_MODE_PAUSE) self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() def reset_pause(self): self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() def _reset_env(self): """Reset physics of Sim""" self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() # self._pause_sim() def prep_rollout(self): self._reset_env() def get_position(self): return self.spot_node.getPosition() def get_obs(self): """Get observation, will be input to model""" self.cameras[0].recognitionEnable(int(2 * self.time_step)) self.cameras[1].recognitionEnable(int(2 * self.time_step)) # Get object data #left_cam = np.asarray(self.cameras[0].getRecognitionObjects()) #right_cam = np.asarray(self.cameras[1].getRecognitionObjects()) # Get motor vals motor_vals = np.nan_to_num(self.get_motor_vals()) # Get robot position and center of mass com = np.asarray(self.spot_node.getCenterOfMass()) pos = np.asarray(self.spot_node.getPosition()) return (motor_vals, com, pos) def get_supervisor_obs(self): com = self.spot_node.getCenterOfMass() orientation = self.spot_node.getOrientation() position = self.spot_node.getPosition() velocity = self.spot_node.getVelocity() num_contact_points = self.spot_node.getNumberOfContactPoints() contact_points = [ self.spot_node.getContactPoint(i) for i in range(num_contact_points) ] return { 'contact_points': contact_points, 'num_contact_points': num_contact_points, 'com': com, 'velocity': velocity, 'position': position, 'orientation': orientation } def get_reward(self, prev_obs: tuple, terminal: bool) -> float: """Get reward for MAML""" orientation = np.asarray(self.rot_vec.getSFRotation()) # Euler angles rot_z = np.abs(orientation[2]) alpha = np.abs(orientation[3]) # Angle of rotation rotation_reward = rot_z * alpha # Penalize rotation on z com = self.spot_node.getCenterOfMass() com_height: float = com[1] - 0.35 (prev_motor_vals, com, prev_pos) = prev_obs cur_pos: np.ndarray = np.asarray(self.spot_node.getPosition()) cur_pos[1] = 0 # Remove height from position prev_pos[1] = 0 # largest motor change delta_motor = np.max(self.get_motor_vals() - prev_motor_vals) # Change in position delta_position = np.linalg.norm((cur_pos - prev_pos), ord=2) prev_to_goal = np.linalg.norm((prev_pos - self.goal_pt), ord=2) new_to_goal = np.linalg.norm((cur_pos - self.goal_pt), ord=2) # Distance to goal: delta_goal_dist: np.float32 = prev_to_goal - new_to_goal # Will be positive if we got closer delta_goal_dist *= 10 survive_reward = 0.1 # TODO: collision reward_arr = np.array([ delta_goal_dist, delta_position, -delta_motor, com_height, survive_reward, -rotation_reward, ]) reward = reward_arr.sum() #print("reward function===========") #np.set_printoptions(precision=3) #print("prev pos", prev_pos) #print("cur pos", cur_pos) #print("prv 2 goal ", prev_to_goal) #print("cur2 goal", new_to_goal) #print("Change in goal dist: ", delta_goal_dist, reward_arr[0]) #print("change in position: ", delta_position, reward_arr[1]) #print("largest change in motor vals", -delta_motor, reward_arr[2]) #print("Center of mass height", com_height, reward_arr[3]) #print("survive_reward", survive_reward, reward_arr[4]) #print("rotation reward", rotation_reward, reward_arr[5]) #print("TOTAL REWARD: ", reward) #print("") if terminal: reward += 200 return reward
# Copyright 1996-2018 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. """Set the BOX.size field (firing the SolidBox regeneration) after 10 steps.""" from controller import Supervisor supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) for i in range(10): supervisor.step(timestep) print('Set BOX.size to [1, 1, 1].') node = supervisor.getFromDef("BOX") field = node.getField("size") field.setSFVec3f([1, 1, 1]) supervisor.step(timestep)
from controller import Supervisor from controller import Emitter import struct # Supervisor setup supervisor = Supervisor() TIMESTEP = int(supervisor.getBasicTimeStep()) COMM_CHANNEL = 1 # Supervisor interpret world soccerball = supervisor.getFromDef("BALL") trans_field = soccerball.getField("translation") ball_radius = 0.113 # soccerball.getField("radius") INITIAL_TRANS = [0, ball_radius, 0] # Emitter setup emitter = supervisor.getEmitter('emitter') emitter.setChannel(COMM_CHANNEL) tInitial = supervisor.getTime() while supervisor.step(TIMESTEP) != -1: values = trans_field.getSFVec3f() t = supervisor.getTime() # Emit ball position if (t - tInitial) >= 1: # print(t-tInitial) print("Ball is at position: %g %g %g" %
class InvaderBot(): # Initalize the motors. def setup(self): self.robot = Supervisor() self.motor_left = self.robot.getMotor("motor_left") self.motor_right = self.robot.getMotor("motor_right") self.timestep = int(self.robot.getBasicTimeStep()) # Do one update step. Calls Webots' robot.step(). # Return True while simulation is running. # Return False if simulation is ended def step(self): return (self.robot.step(self.timestep) != -1) # Set the velocity of the motor [-1, 1]. def set_motor(self, motor, velocity): mult = 1 if velocity > 0 else -1 motor.setPosition(mult * float('+inf')) motor.setVelocity(velocity * motor.getMaxVelocity()) # Set the velocity of left and right motors [-1, 1]. def set_motors(self, left, right): self.set_left_motor(left) self.set_right_motor(right) # Set the velocity of left motors [-1, 1]. def set_left_motor(self, velocity): self.set_motor(self.motor_left, velocity) # Set the velocity of right motors [-1, 1]. def set_right_motor(self, velocity): self.set_motor(self.motor_right, velocity) # Returns the current simulation time in seconds def get_time(self): return self.robot.getTime() # Returns the position of the robot in [x, z, angle] format # The coordinate system is as follows (top-down view) # .-------------------->x # |\ (angle) # | \ # | \ # | \ # | \ # | \ # | # V # z # def get_position(self): subject = self.robot.getSelf() position = subject.getPosition() orientation = subject.getOrientation() orientation = math.atan2(orientation[0], orientation[2]) orientation = math.degrees(orientation) return [position[0], position[2], orientation] # Returns the position of the balls in the following format # [ # [ # [green_ball_0_x, green_ball_0_z], # [green_ball_1_x, green_ball_1_z] # ], # # [ # [yellow_ball_0_x, yellow_ball_0_z], # [yellow_ball_1_x, yellow_ball_1_z], # [yellow_ball_2_x, yellow_ball_2_z], # ] # ] def get_balls(self): green = [] green_root = self.robot.getFromDef("_green_balls").getField("children") for idx in reversed(range(green_root.getCount())): try: ball = green_root.getMFNode(idx) pos = ball.getPosition() green.append([pos[0], pos[2]]) except: pass yellow = [] yellow_root = self.robot.getFromDef("_yellow_balls").getField( "children") for idx in reversed(range(yellow_root.getCount())): try: ball = yellow_root.getMFNode(idx) pos = ball.getPosition() yellow.append([pos[0], pos[2]]) except: pass return [green, yellow]
for priortyDeviceType in priortyDeviceTypes: if d1['type'] == priortyDeviceType and d2['type'] == priortyDeviceType: return _cmp(d1['name'].lower(), d2['name'].lower()) elif d1['type'] == priortyDeviceType: return -1 elif d2['type'] == priortyDeviceType: return 1 return _cmp(d1['name'].lower(), d2['name'].lower()) userGuidePath = os.path.join(os.getenv('WEBOTS_HOME'), 'docs', 'guide') supervisor = Supervisor() timeStep = int(supervisor.getBasicTimeStep()) robot = supervisor.getFromDef('ROBOT') robotName = robot.getField('name').getSFString() # Get target paths. scenePath = os.path.join(userGuidePath, 'scenes', robotName) targetHTMLFile = os.path.join(scenePath, robotName + '.html') targetAnimationFile = os.path.join(scenePath, robotName + '.json') targetMetaFile = os.path.join(scenePath, robotName + '.meta.json') targetX3DFile = os.path.join(scenePath, robotName + '.x3d') # Store the scene. if os.path.exists(scenePath): shutil.rmtree(scenePath) if not os.path.exists(scenePath): os.makedirs(scenePath) supervisor.animationStartRecording(targetHTMLFile)
def isPositionChanged(v1, v2): return abs(v1[0] - v2[0]) > 0.001 or abs(v1[2] - v2[2]) > 0.001 def isMazeEndReached(position): return position[0] < 0.60 and position[0] > 0.45 and position[ 2] < 0.15 and position[2] > -0.15 robot = Supervisor() timestep = int(4 * robot.getBasicTimeStep()) robot.step(10 * timestep) thymio2 = robot.getFromDef("THYMIO2") robot.step(10 * timestep) mazeBlocksList = [] mazeBlocksListCount = 0 topChildrenField = robot.getFromDef("MAZE_WALLS").getField("children") topNodesCount = topChildrenField.getCount() for i in range(topNodesCount): node = topChildrenField.getMFNode(i) if node.getTypeName() == "MazeBlock": object = {"node": node, "initialPosition": node.getPosition()} mazeBlocksList.append(object) mazeBlocksListCount += 1 running = True
try: includePath = os.environ.get("WEBOTS_HOME") + "/projects/samples/robotbenchmark/include" includePath.replace('/', os.sep) sys.path.append(includePath) from robotbenchmark import robotbenchmarkRecord except ImportError: sys.stderr.write("Warning: 'robotbenchmark' module not found.\n") sys.exit(0) TIME_STEP = 64 supervisor = Supervisor() targetDist = 0.1989 robot_node = supervisor.getFromDef("MY_ROBOT") if robot_node is None: sys.stderr.write("No DEF MY_ROBOT node found in the current world file\n") sys.exit(1) trans_field = robot_node.getField("translation") while supervisor.step(TIME_STEP) != -1: values = trans_field.getSFVec3f() dist = sqrt(values[0] * values[0] + values[2] * values[2]) percent = 1 - abs(dist - targetDist) / targetDist message = "percent:" + str(percent) supervisor.wwiSendText(message) print("Distance: {}".format(dist)) print("Percent: {}".format(percent))
try: includePath = os.environ.get( "WEBOTS_HOME") + "/projects/samples/robotbenchmark/include" includePath.replace('/', os.sep) sys.path.append(includePath) from robotbenchmark import robotbenchmarkRecord except ImportError: sys.stderr.write("Warning: 'robotbenchmark' module not found.\n") sys.exit(0) robot = Supervisor() timestep = int(robot.getBasicTimeStep()) thymio = robot.getFromDef("THYMIO2") translation = thymio.getField("translation") tz = 0 running = True while robot.step(timestep) != -1: t = translation.getSFVec3f() if running: percent = 1 - abs(0.25 - t[2]) / 0.25 if percent < 0: percent = 0 if t[2] > 0.01 and abs( tz - t[2] ) < 0.0001: # away from starting position and not moving any more message = "stop" running = False
camera.enableRecognitionSegmentation() number = 0 print("hasRecognition(): " + str(camera.hasRecognition())) print("hasRecognitionSegmentation(): " + str(camera.hasRecognitionSegmentation())) cv2.startWindowThread() cv2.namedWindow("preview") ball_color = ( 0, 0 , 255, 255) enemy1_color = ( 0, 255, 0, 255) enemy2_color = (255, 0, 0, 255) enemy3_color = (255, 255, 0, 255) while supervisor.step(timestep) != -1: supervisor.getFromDef('BALL').getField('translation').setSFVec3f([random.uniform(0.0, 4.0), random.uniform(-1.0, 1.0), 0.1]) supervisor.getFromDef('ENEMY1').getField('rotation').setSFRotation([0, 0, 1, random.uniform(-3.14, 3.14)]) for i in range(10): supervisor.step(timestep) camera.saveImage(deviceImagePath + '/images/image' + str(number) + '.jpg', 80) camera.saveRecognitionSegmentationImage(deviceImagePath + '/images/segmentation_image' + str(number) + '.jpg', 80) number += 1 seg_img = camera.getRecognitionSegmentationImage() img = np.frombuffer(seg_img, np.uint8).reshape((camera.getHeight(), camera.getWidth(), 4)) ball = cv2.inRange(img, ball_color, ball_color) x, y, width, height = cv2.boundingRect(ball) cv2.rectangle(img, (x, y), (x + width, y + height), color=ball_color, thickness=2) print("ball x: " + str(x) + ", y: " + str(y) + ", with: " + str(width) + ", height: " + str(height)) enemy1 = cv2.inRange(img, enemy1_color, enemy1_color) x, y, width, height = cv2.boundingRect(enemy1) cv2.rectangle(img, (x, y), (x + width, y + height), color=enemy1_color, thickness=2)
'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]) while supervisor.step(TIME_STEP) != -1: # adjust the color according to the value returned by the front distance sensor for name in sensorsNames: ratio = math.pow(sensors[name].getValue() / sensors[name].getMaxValue(), 2.0) colorFields[name].setSFColor([1.0 - ratio, 0.0, ratio])
class Agent(object): """docstring for Agent.""" def __init__(self, mdNames, objName): super().__init__() self.robot = Supervisor() self.objName = objName self.energy = 10000 self.timestep = int(self.robot.getBasicTimeStep()) self.camera = self.robot.getDevice('camera') self.camera.enable(self.timestep) self.camera.recognitionEnable(self.timestep) self.MAX_SPEED = 10 self.LOW_SPEED = -10 self.MAX_ENERGY = 10000 self.consumptionEnergy = 2000 self.ds = [] self.md = [] self.dsNames = ['ds_left', 'ds_right', 'ds_left(1)', 'ds_right(1)'] for i in range(4): self.ds.append(self.robot.getDevice(self.dsNames[i])) self.ds[i].enable(self.timestep) self.length = len(self.mdNames) for i in range(self.length): self.md.append(self.robot.getDevice(self.mdNames[i])) def multiMoveMotorPos(self, devices, dPos): for device in devices: device.setPosition(dPos) def setMultiMotorVel(self, devices, vel): for device in devices: device.setVelocity(vel*self.MAX_SPEED) self.velocity = vel def getEnergy(self): return self.energy def setEnergy(self, energy): self.energy = energy def eat(self, prey): prey = prey pField = prey.getField('translation') randX = random.uniform(-2.45, 2.45) randZ = random.uniform(-2.45, 2.45) newPos = [randX,0.05,randZ] pField.setSFVec3f(newPos) self.energy = self.energy + self.consumptionEnergy if self.energy > self.MAX_ENERGY: self.energy = self.MAX_ENERGY def checkEnergyCollision(self, preyName): if preyName: objPos = self.getPosition(self.objName) objPos = np.array(objPos) objPos = np.array(objPos) for i in preyName: self.prey = self.robot.getFromDef(i) preyPos = self.prey.getPosition() preyPos = self.prey.getPosition() preyPos = np.array(preyPos) dist = np.linalg.norm(objPos - preyPos) if dist < 0.4: return self.prey else: return False def getPosition(self, name): thing = self.robot.getFromDef(name) pos = thing.getPosition() return pos def checkObstacle(self): dsValues = [] for i in range(4): dsValues.append(self.ds[i].getValue()) left_obstacle = dsValues[0] < 1000.0 or dsValues[2] < 1000.0 right_obstacle = dsValues[1] < 1000.0 or dsValues[3] < 1000.0 if left_obstacle: return 1 elif right_obstacle: return 2 else: return False def checkRecogniseSource(self): currClosest = [1000,100, 1000] minDist = float('inf') recognisedObj = self.camera.getRecognitionObjects() for obj in recognisedObj: currObj = obj.get_position() distance = math.sqrt(currObj[0]*currObj[0]+currObj[2]*currObj[2]) if distance < minDist: minDist = distance currClosest = currObj if recognisedObj: x = currClosest[0] angle = x *minDist return angle else: return False return False def getMotorDevices(self): return self.md def avoidObstacle(self, value): if value == 1: self.turnRight() elif value == 2: self.turnLeft() def setMaxEnergy(self, energy): self.MAX_ENERGY = energy def setConsumptionEnergy(self, energy): self.consumptionEnergy = energy
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")
# importing the Supervisor module from controller import Robot from controller import Supervisor import FrisPy import numpy as np import euler_to_AA # create the Supervisor instance. supervisor = Supervisor() # get the time step of the current world. timestep = int(supervisor.getBasicTimeStep()) print(timestep) # instantiate object handles for the frisbee frisbee_node = supervisor.getFromDef("frisbee") trans_field = frisbee_node.getField("translation") rotation_field = frisbee_node.getField("rotation") # frisbee simulation time variables frisbee_timestep = 6 N = 1000 scaling_factor = 0.2 ground_offset = 0.25 sim_times = np.linspace(0, N, N + 1) * frisbee_timestep / 1000 print(sim_times) init_conditions = [ 0, -4.2, 1, # x, y, z
- 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() # Maximum time for a match maxTime = 120
"""lab3_controller controller.""" # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot from controller import Supervisor import numpy as np import csv import matplotlib.pyplot as plt # create the Robot instance. robot = Supervisor() segway = robot.getFromDef('robot') # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) motor_R = robot.getDevice('motor_R') motor_L = robot.getDevice('motor_L') lidar_F = robot.getDevice('lidar_F') lidar_R = robot.getDevice('lidar_R') gyro = robot.getDevice('gyro') compass = robot.getDevice('compass') lidar_F.enable(timestep) lidar_R.enable(timestep) gyro.enable(timestep) compass.enable(timestep) motor_R.setPosition(float('+inf')) motor_L.setPosition(float('-inf'))