def svg1():
outdir = get_comptests_output_dir()
control_points = [
SE2Transform.from_SE2(geo.SE2_from_translation_angle([0, 0], 0)),
SE2Transform.from_SE2(geo.SE2_from_translation_angle([1, 0], 0)),
SE2Transform.from_SE2(geo.SE2_from_translation_angle([2, -1], np.deg2rad(-90))),
]
width = 0.3
ls = LaneSegment(width, control_points)
area = RectangularArea((-1, -2), (3, 2))
draw_static(ls, outdir, area=area)
def get_flattened_measurement_graph(po, include_root_to_self=False):
G = get_meausurements_graph(po)
G2 = nx.DiGraph()
root_name = ()
for name in G.nodes():
if name == root_name:
continue
path = nx.shortest_path(G, (), name)
transforms = []
for i in range(len(path) - 1):
a = path[i]
b = path[i + 1]
edges = G.get_edge_data(a, b)
k = list(edges)[0]
v = edges[k]
sr = v['attr_dict']['sr'].transform
transforms.append(sr)
from duckietown_world import TransformSequence
if any(isinstance(_, Sequence) for _ in transforms):
res = VariableTransformSequence(transforms)
else:
res = TransformSequence(transforms)
G2.add_edge(root_name, name, transform_sequence=res)
if include_root_to_self:
from duckietown_world import SE2Transform
transform_sequence = SE2Transform.identity()
G2.add_edge(root_name, root_name, transform_sequence=transform_sequence)
return G2
def center_point1():
outdir = get_comptests_output_dir()
templates = load_tile_types()
for k, v in templates.items():
if isinstance(v, LaneSegment):
area = RectangularArea((-2, -2), (3, 3))
dest = os.path.join(outdir, k)
N = len(v.control_points)
betas = list(np.linspace(-2, N + 1, 20))
betas.extend(range(N))
betas = sorted(betas)
transforms = []
for timestamp in betas:
beta = timestamp
p = v.center_point(beta)
# print('%s: %s' % (beta, geo.SE2.friendly(p)))
transform = SE2Transform.from_SE2(p)
transforms.append(transform)
c = SampledSequence[SE2Transform](betas, transforms)
v.set_object("a", PlacedObject(), ground_truth=c)
draw_static(v, dest, area=area)
def __init__(self, children=None, spatial_relations=None):
children = children or {}
spatial_relations = spatial_relations or {}
self.children = children
for k, v in list(spatial_relations.items()):
from .transforms import Transform
if isinstance(v, Transform):
if k in self.children:
sr = GroundTruth(a=(), b=(k, ), transform=v)
spatial_relations[k] = sr
else:
msg = 'What is the "%s" referring to?' % k
raise ValueError(msg)
self.spatial_relations = spatial_relations
if not spatial_relations:
for child in self.children:
from duckietown_world import SE2Transform
sr = GroundTruth(a=(),
b=(child, ),
transform=SE2Transform.identity())
self.spatial_relations[child] = sr
def __post_init__(self):
from .transforms import Transform
for k, v in list(self.spatial_relations.items()):
if isinstance(v, Transform):
if k in self.children:
b: Tuple[str, ...] = (k,)
sr = GroundTruth(a=root, b=b, transform=v)
self.spatial_relations[k] = sr
else:
msg = f'What is the "{k}" referring to?'
raise ValueError(msg)
if not self.spatial_relations:
for child in self.children:
from duckietown_world import SE2Transform
sr = GroundTruth(a=root, b=(child,), transform=SE2Transform.identity())
self.spatial_relations[child] = sr
def test_pwm1():
parameters = get_DB18_nominal(delay=0)
# initial configuration
init_pose = np.array([0, 0.8])
init_vel = np.array([0, 0])
q0 = geo.SE2_from_R2(init_pose)
v0 = geo.se2_from_linear_angular(init_vel, 0)
tries = {
"straight_50": (PWMCommands(+0.5, 0.5)),
"straight_max": (PWMCommands(+1.0, +1.0)),
"straight_over_max": (PWMCommands(+1.5, +1.5)),
"pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)),
"pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)),
"slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)),
"faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)),
# 'slight-right': (PWMCommands(-0.1, 0.1)),
}
dt = 0.03
t_max = 10
map_data_yaml = """
tiles:
- [floor/W,floor/W, floor/W, floor/W, floor/W]
- [straight/W , straight/W , straight/W, straight/W, straight/W]
- [floor/W,floor/W, floor/W, floor/W, floor/W]
tile_size: 0.61
"""
map_data = yaml.load(map_data_yaml)
root = construct_map(map_data)
timeseries = {}
for id_try, commands in tries.items():
seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands)
ground_truth = seq.transform_values(
lambda t: SE2Transform.from_SE2(t[0]))
poses = seq.transform_values(lambda t: t[0])
velocities = get_velocities_from_sequence(poses)
linear = velocities.transform_values(linear_from_se2)
angular = velocities.transform_values(angular_from_se2)
# print(linear.values)
# print(angular.values)
root.set_object(id_try, DB18(), ground_truth=ground_truth)
sequences = {}
sequences["motor_left"] = seq.transform_values(
lambda _: commands.motor_left)
sequences["motor_right"] = seq.transform_values(
lambda _: commands.motor_right)
plots = TimeseriesPlot(f"{id_try} - PWM commands", "pwm_commands",
sequences)
timeseries[f"{id_try} - commands"] = plots
sequences = {}
sequences["linear_velocity"] = linear
sequences["angular_velocity"] = angular
plots = TimeseriesPlot(f"{id_try} - Velocities", "velocities",
sequences)
timeseries[f"{id_try} - velocities"] = plots
outdir = os.path.join(get_comptests_output_dir(), "together")
draw_static(root, outdir, timeseries=timeseries)
def evaluate(self, agent, outdir, episodes=None):
"""
Evaluates the agent on the map inicialised in __init__
:param agent: Agent to be evaluated, passed to self._collect_trajectory(agent,...)
:param outdir: Directory for logged outputs (trajectory plots + numeric data)
:param episodes: Number of evaluation episodes, if None, it is determined based on self.start_poses
"""
if not os.path.exists(outdir):
os.makedirs(outdir)
if episodes is None:
episodes = len(self.start_poses.get(self.map_name, []))
totals = {}
for i in range(episodes):
episode_path, episode_orientations, episode_timestamps = self._collect_trajectory(
agent, i)
logger.info("Episode {}/{} sampling completed".format(
i + 1, episodes))
if len(episode_timestamps) <= 1:
continue
episode_path = np.stack(episode_path)
episode_orientations = np.stack(episode_orientations)
# Convert them to SampledSequences
transforms_sequence = []
for j in range(len(episode_path)):
transforms_sequence.append(
SE2Transform(episode_path[j], episode_orientations[j]))
transforms_sequence = SampledSequence.from_iterator(
enumerate(transforms_sequence))
transforms_sequence.timestamps = episode_timestamps
_outdir = outdir
if outdir is not None and episodes > 1:
_outdir = os.path.join(outdir, "Trajectory_{}".format(i + 1))
evaluated = self._eval_poses_sequence(transforms_sequence,
outdir=_outdir)
logger.info("Episode {}/{} plotting completed".format(
i + 1, episodes))
totals = self._extract_total_episode_eval_metrics(
evaluated, totals, display_outputs=True)
# Calculate the median total metrics
median_totals = {}
mean_totals = {}
stdev_totals = {}
for key, value in totals.items():
median_totals[key] = np.median(value)
mean_totals[key] = np.mean(value)
stdev_totals[key] = np.std(value)
# Save results to file
with open(os.path.join(outdir, "total_metrics.json"),
"w") as json_file:
json.dump(
{
'median_totals': median_totals,
'mean_totals': mean_totals,
'stdev_totals': stdev_totals,
'episode_totals': totals
},
json_file,
indent=2)
logger.info("\nMedian total metrics: \n {}".format(
pretty_print(median_totals)))
logger.info("\nMean total metrics: \n {}".format(
pretty_print(mean_totals)))
logger.info("\nStandard deviation of total metrics: \n {}".format(
pretty_print(stdev_totals)))
def interpret_map_data(data):
tiles = data['tiles']
assert len(tiles) > 0
assert len(tiles[0]) > 0
# Create the grid
A = len(tiles)
B = len(tiles[0])
tm = TileMap(H=B, W=A)
templates = load_tile_types()
for a, row in enumerate(tiles):
if len(row) != B:
msg = "each row of tiles must have the same length"
raise ValueError(msg)
# For each tile in this row
for b, tile in enumerate(row):
tile = tile.strip()
if tile == 'empty':
continue
if '/' in tile:
kind, orient = tile.split('/')
kind = kind.strip()
orient = orient.strip()
# angle = ['S', 'E', 'N', 'W'].index(orient)
drivable = True
elif '4' in tile:
kind = '4way'
# angle = 2
orient = 'N'
drivable = True
else:
kind = tile
# angle = 0
orient = 'S'
drivable = False
tile = Tile(kind=kind, drivable=drivable)
if kind in templates:
tile.set_object(kind,
templates[kind],
ground_truth=SE2Transform.identity())
else:
pass
# msg = 'Could not find %r in %s' % (kind, templates)
# logger.debug(msg)
tm.add_tile(b, (A - 1) - a, orient, tile)
# if drivable:
# tile['curves'] = self._get_curve(i, j)
# self.drivable_tiles.append(tile)
#
# self._load_objects(self.map_data)
#
# # Get the starting tile from the map, if specified
# self.start_tile = None
# if 'start_tile' in self.map_data:
# coords = self.map_data['start_tile']
# self.start_tile = self._get_tile(*coords)
# # Arrays for checking collisions with N static objects
# # (Dynamic objects done separately)
# # (N x 2): Object position used in calculating reward
# self.collidable_centers = []
#
# # (N x 2 x 4): 4 corners - (x, z) - for object's boundbox
# self.collidable_corners = []
#
# # (N x 2 x 2): two 2D norms for each object (1 per axis of boundbox)
# self.collidable_norms = []
#
# # (N): Safety radius for object used in calculating reward
# self.collidable_safety_radii = []
# For each object
for obj_idx, desc in enumerate(data.get('objects', [])):
kind = desc['kind']
pos = desc['pos']
rotate = desc['rotate']
transform = SE2Transform([float(pos[0]), float(tm.W - pos[1])], rotate)
# x, z = pos[0:2]
#
# i = int(np.floor(x))
# j = int(np.floor(z))
# dx = x - i
# dy = z - j
#
# z = pos[2] if len(pos) == 3 else 0.0
# optional = desc.get('optional', False)
# height = desc.get('height', None)
# pos = ROAD_TILE_SIZE * np.array((x, y, z))
# Load the mesh
# mesh = ObjMesh.get(kind)
# TODO
# if 'height' in desc:
# scale = desc['height'] / mesh.max_coords[1]
# else:
# scale = desc['scale']
# assert not ('height' in desc and 'scale' in desc), "cannot specify both height and scale"
# static = desc.get('static', True)
# obj_desc = {
# 'kind': kind,
# # 'mesh': mesh,
# 'pos': pos,
# 'scale': scale,
# 'y_rot': rotate,
# 'optional': optional,
# 'static': static,
# }
if kind == "trafficlight":
status = Constant("off")
obj = TrafficLight(status)
else:
kind2klass = {
'duckie': Duckie,
'cone': Cone,
'barrier': Barrier,
'building': Building,
'duckiebot': DB18,
'sign_left_T_intersect': SignLeftTIntersect,
'sign_right_T_intersect': SignRightTIntersect,
'sign_T_intersect': SignTIntersect,
'sign_4_way_intersect': Sign4WayIntersect,
'sign_t_light_ahead': SingTLightAhead,
'sign_stop': SignStop,
'tree': Tree,
'house': House,
'bus': Bus,
'truck': Truck,
}
if kind in kind2klass:
klass = kind2klass[kind]
obj = klass()
else:
logger.debug('Do not know special kind %s' % kind)
obj = GenericObject(kind=kind)
obj_name = 'ob%02d-%s' % (obj_idx, kind)
# tile = tm[(i, j)]
# transform = TileRelativeTransform([dx, dy], z, rotate)
tm.set_object(obj_name, obj, ground_truth=transform)
# obj = None
# if static:
# if kind == "trafficlight":
# obj = TrafficLightObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT)
# else:
# obj = WorldObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT)
# else:
# obj = DuckieObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT, ROAD_TILE_SIZE)
#
# self.objects.append(obj)
# Compute collision detection information
# angle = rotate * (math.pi / 180)
# Find drivable tiles object could intersect with
return tm
slot = int(slot)
rotation = rotation or 0
rotation = int(rotation)
# bug in the map file
# rotation = rotation + 90
sign = dict(kind=kind,
tag={
'~TagInstance':
dict(tag_id=int(tag_ID),
family='36h11',
size=APRIL_TAG_SIZE)
},
attach=dict(tile=[x, y], slot=slot))
sign['pose'] = SE2Transform([0, 0], np.deg2rad(rotation)).as_json_dict()
objects['tag%02d' % i] = sign
in_file = open("AprilTag position duckietown.csv", "rb")
reader = csv.reader(in_file)
data = list(reader)[1:]
CM = 0.01
origin = -5 * CM, -15.5 * CM
# Tag ID,Quadrant,x location,Y-location,Roation
# FAMILY = 'default'
# SIZE = 0.035
for entry in data:
tag_ID, quadrant, x, y, rotation = entry
x = float(x) + origin[0]
def get_SE2transform(x: PlatformDynamics) -> SE2Transform:
check_isinstance(x, PlatformDynamics)
q, v = x.TSE2_from_state()
return SE2Transform.from_SE2(q)