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