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