def add_noise(self, data: list):
"""Add noise to the data
Args:
data (list): The data to add noise to
"""
def rand(element):
return np.random.normal(getattr(self._mean, element, 0),
getattr(self._sigma, element, 0))
eta_a = WAVector([rand('x'), rand('y'), rand('z')])
eta_b = WAVector()
if self._tau_drift > 0 and self._bias_drift > 0:
def rand():
return np.random.normal(
0.0,
self._drift_bias *
np.sqrt(1.0 /
(self._update_rate * self._tau_drift))) # noqa
eta_b = WAVector([rand(), rand(), rand()])
self._bias += eta_b
data += eta_a + self._bias
def _load_properties(self, p: dict):
"""Private function that loads propeties and sets them to class variables
Args:
p (dict): Propeties dictionary
"""
self._mean = WAVector(p['Mean'])
self._sigma = WAVector(p['Standard Deviation'])
def visualize(self, assets, *args, **kwargs):
if not isinstance(assets, list):
assets = [assets]
patches = []
bodies = []
for i, asset in enumerate(assets):
if isinstance(asset, WAPath):
points = asset.get_points()
self._plotter.plot(points[:, 0], points[:, 1], *args,
**asset.get_vis_properties(), **kwargs)
elif isinstance(asset, WATrack):
self.visualize(asset.left, *args, **kwargs)
self.visualize(asset.right, *args, **kwargs)
self.visualize(asset.center, *args, **kwargs)
elif isinstance(asset, WABody):
if not hasattr(asset, 'size') or not hasattr(
asset, 'position'):
raise AttributeError(
"Body must have 'size', and 'position' fields")
if hasattr(asset, 'updates') and asset.updates:
# bodies.append(_MatplotlibBody(asset))
print(
"WARNING: updateable WABody's are currently not supported for the WAMatplotlibVisualization. This will come in a future release."
)
continue
position = asset.position
yaw = 0 if not hasattr(asset, 'yaw') else asset.yaw
size = asset.size / 2
color = WAVector(
[1, 0, 0]) if not hasattr(asset, 'color') else asset.color
# Can't really see white
edgecolor = color
if color == WAVector([1, 1, 1]):
edgecolor = WAVector([0, 0, 0])
outline = np.array(
[[-size.y, size.y, size.y, -size.y, -size.y],
[size.x, size.x, -size.x, -size.x, size.x],
[1, 1, 1, 1, 1]])
outline = _transform(outline, position.x, position.y, yaw)
patches.append(
Polygon(outline[:2, :].T,
facecolor=color,
edgecolor=edgecolor,
alpha=0.4))
if len(patches):
self._plotter.plot(
collections.PatchCollection(patches, match_original=True))
if len(bodies):
self._plotter.plot(_MatplotlibBodyList(bodies))
def _load_properties(self, p: dict):
"""Private function that loads propeties and sets them to class variables
Args:
p (dict): Propeties dictionary
"""
self._update_rate = p['Update Rate']
self._mean = WAVector(p['Mean'])
self._sigma = WAVector(p['Standard Deviation'])
self._bias_drift = p['Bias Drift']
self._tau_drift = p['Tau Drift']
self._bias = WAVector()
def ChVector_to_WAVector(vector: chrono.ChVectorD):
"""Converts a ChVector to a WAVector
Args:
vector (ChVector): The vector to convert
"""
return WAVector([vector.x, vector.y, vector.z])
def load_environment_from_json(environment: 'WAEnvironment', filename: str):
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Environment')
_check_field(j, 'Template')
_check_field(j, 'World', field_type=dict, optional=True)
_check_field(j, 'Objects', field_type=list, optional=True)
if 'World' in j:
w = j['World']
if 'Objects' in j:
objects = j['Objects']
for o in objects:
_check_field(o, 'Size', field_type=list)
_check_field(o, 'Position', field_type=list, optional=True)
_check_field(o, 'Color', field_type=list, optional=True)
kwargs = {}
kwargs['size'] = WAVector(o['Size'])
kwargs['position'] = WAVector(o['Position'])
if 'Color' in o:
kwargs['color'] = WAVector(o['Color'])
if 'Texture' in o:
kwargs['texture'] = o['Texture']
if 'Name' in o:
kwargs['name'] = o['Name']
environment.add_asset(WABody(**kwargs))
if 'Track' in j:
t = j['Track']
# Validate json
_check_field(t, 'Track Input File', field_type=str)
track_file = t['Track Input File']
track = create_track_from_json(get_wa_data_file(track_file),
environment)
environment.add_asset(track.center)
def calc_closest_point(self, pos: WAVector, return_idx: bool = False) -> (WAVector, int):
dist = cdist(self._points, [pos])
idx, = np.argmin(dist, axis=0)
pos = WAVector([self._x[idx], self._y[idx], self._z[idx]])
if return_idx:
return pos, idx
return pos
def update_position_of_sphere(sphere: chrono.ChBodyEasySphere, pos: WAVector):
"""Update the position of a sphere being visualized in irrlicht
Args:
sphere (chrono.ChBodyEasySphere): the sphere to change the position of
pos (WAVector): the new position
"""
if not isinstance(pos, WAVector):
pos = WAVector(pos)
sphere.SetPos(chrono.ChVectorD(pos.x, pos.y, 0))
def add_noise(self, data: list):
"""Add noise to the data
Args:
data (list): The data to add noise to
"""
def rand(element):
return np.random.normal(getattr(self._mean, element, 0),
getattr(self._sigma, element, 0))
noise = WAVector([rand('x'), rand('y'), rand('z')])
data += noise
def inside_boundaries(self, point: WAVector) -> bool:
"""Check whether the passed point is within the track boundaries
Implementation is explained `here <https://stackoverflow.com/a/33155594>`_.
Args:
point (WAVector): point to check whether it's inside the track boundaries
Returns:
bool: is the point inside the boundaries?
"""
if not isinstance(point, WAVector):
raise TypeError(
f'WATrack.inside_boundary: Expects a WAVector, not a {type(point)}.'
)
closest_point, idx = self.center.calc_closest_point(point, True)
A, B, C = point, WAVector(self.left.get_points()[idx]), WAVector(
self.right.get_points()[idx]) # noqa
a, b, c = (B - C).length, (C - A).length, (A - B).length
return a**2 + b**2 >= c**2 and a**2 + c**2 >= b**2
def _load_properties(self, p: dict):
"""Private function that loads properties and sets them to class variables
Args:
p (dict): Properties dictionary
"""
self._update_rate = p['Update Rate']
self._reference = WAVector(p['GPS Reference'])
self._noise_model = WANoNoiseModel()
if 'Noise Model' in p:
n = p['Noise Model']
if n['Noise Type'] == 'Normal Drift':
self._noise_model = WANormalDriftNoiseModel(n)
elif n['Noise Type'] == 'Normal':
self._noise_model = WANormalNoiseModel(n)
else:
raise TypeError(
f"{p['Noise Type']} is not an implemented model type")
self._pos = WAVector()
def __init__(self,
system: 'WASystem',
filename: str,
vehicle: 'WAVehicle' = None,
body: 'WABody' = None):
super().__init__(vehicle, body)
if body is not None:
raise NotImplementedError(
"Setting 'body' is currently not supported. Please pass a vehicle instead"
)
self._vehicle = vehicle
self._pos_dtdt = None
self._rot = None
self._rot_dt = None
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j, 'Template', value='IMU')
_check_field(j, 'Properties', field_type=dict)
p = j['Properties']
_check_field(p, 'Update Rate', field_type=int)
_check_field(p, 'Noise Model', field_type=dict, optional=True)
if 'Noise Model' in p:
_check_field(p['Noise Model'],
'Noise Type',
allowed_values=["Normal", "Normal Drift"])
self._load_properties(p)
self._acc = WAVector()
self._omega = WAVector()
self._orientation = WAVector()
def __init__(self,
system: 'WAChronoSystem',
vehicle_inputs: 'WAVehicleInputs',
env: 'WAEnvironment',
filename: str,
init_loc: WAVector = WAVector([0, 0, 0.5]),
init_rot: WAQuaternion = WAQuaternion([1, 0, 0, 0]),
init_speed: float = 0.0):
super().__init__(
system, vehicle_inputs,
get_wa_data_file("vehicles/GoKart/GoKart_KinematicBicycle.json"))
# Get the filenames
vehicle_file, powertrain_file, tire_file = read_vehicle_model_file(
filename)
# Create the vehicle
vehicle = veh.WheeledVehicle(system._system, vehicle_file)
# Initialize the vehicle
init_loc = WAVector_to_ChVector(init_loc)
init_rot = WAQuaternion_to_ChQuaternion(init_rot)
vehicle.Initialize(chrono.ChCoordsysD(init_loc, init_rot), init_speed)
# Set the visualization components for the vehicle
vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_NONE)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_NONE)
vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
# Create the powertrain
# Assumes a SimplePowertrain
powertrain = veh.SimplePowertrain(powertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles():
tireL = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
self._vehicle = vehicle
self._terrain = env._terrain
def cartesian_to_gps(coords: WAVector, ref: WAVector):
"""Convert a point from cartesian to gps
Args:
coords(WAVector): The coordinate to convert
ref(WAVector): The "origin" or reference point
Returns:
WAVector: The coordinate in the form of[longitude, latitude, altitude]
"""
lat = (coords.y / WA_EARTH_RADIUS) * 180.0 / WA_PI + ref.y
lon = (coords.x / (WA_EARTH_RADIUS * np.cos(lat * WA_PI / 180.0))
) * 180.0 / WA_PI + ref.x # noqa
alt = coords.z + ref.z
lon = lon + 360.0 if lon < -180.0 else lon - 360.0 if lon > 180.0 else lon
return WAVector([lon, lat, alt])
def gps_to_cartesian(coords: WAVector, ref: WAVector):
"""Convert a gps coordinate to cartesian given some reference
Args:
coords(WAVector): The coordinate to convert
ref(WAVector): The "origin" or reference point
Returns:
WAVector: The x, y, z point in cartesian
"""
lon = coords.x
lat = coords.y
alt = coords.z
x = ((lon - ref.x) * WA_PI / 180.0) * (
WA_EARTH_RADIUS * np.cos(lat * WA_PI / 180.0)) # noqa
y = ((lat - ref.y) * WA_PI / 180.0) * WA_EARTH_RADIUS
z = alt - ref.z
return WAVector([x, y, z])
def _load_properties(self, p: dict):
"""Private function that loads properties and sets them to class variables
Args:
p (dict): Properties dictionary
"""
self._update_rate = p['Update Rate']
self._axle = p['Axle']
self._noise_model = WANoNoiseModel()
if 'Noise Model' in p:
n = p['Noise Model']
if n['Noise Type'] == 'Normal Drift':
self._noise_model = WANormalDriftNoiseModel(n)
elif n['Noise Type'] == 'Normal':
self._noise_model = WANormalNoiseModel(n)
else:
raise TypeError(
f"{p['Noise Type']} is not an implemented model type")
self._vel = WAVector()
self._angular_speed = 0
self._tire_radius = self._vehicle.get_tire_radius(self._axle)
def test_length(self):
"""Tests the length method of a WAVector"""
v = WAVector([10, -4, 1])
self.assertEqual(v.length, (10**2 + (-4)**2 + 1**2)**(1 / 2))
def test_rot(self):
"""Tests rotation of a vector by a quaternion"""
q = WAQuaternion.from_z_rotation(WA_PI) # 180 degree rotation
v = WAVector([1, 0, 0])
self.assertTrue(np.allclose(q * v, WAVector([-1, 0, 0])))
def test_add(self):
"""Tests simple addition of two WAVectors"""
v1 = WAVector()
v2 = WAVector([1, 1, 1])
self.assertEqual(v1 + v2, v2)
def test_add2(self):
"""Tests simple addition of two WAVectors"""
v1 = WAVector([91, 44, -10])
v2 = WAVector([12, -111, 0])
self.assertEqual(v1 + v2, WAVector([103, -67, -10]))
def test_cross(self):
"""Tests cross product of two WAVectors"""
v1 = WAVector([1, 4, 5])
v2 = WAVector([6, 9, 10])
self.assertEqual(v1.cross(v2), WAVector([-5, 20, -15]))
def test_sub(self):
"""Tests simple subtraction of two WAVectors"""
v1 = WAVector([11, 5, 10])
v2 = WAVector([1, 1, 1])
self.assertEqual(v1 - v2, WAVector([10, 4, 9]))
def get_pos_dtdt(self) -> WAVector:
angle = np.tan(np.interp(self._steering, [-1, 1], [self._min_steering, self._max_steering])) # noqa
angle = angle if angle != 0 else 1e-3
tr = self._L / angle
tr = tr if tr != 0 else 1e-3
return WAVector([self._acc, self._v ** 2 / tr, WA_GRAVITY])
def get_pos_dt(self) -> WAVector:
return WAVector([self._v * np.cos(self._yaw), self._v * np.sin(self._yaw), 0.0])
def get_pos(self) -> WAVector:
return WAVector([self._x, self._y, 0.0])
def __init__(self, system: 'WASystem', vehicle_inputs: 'WAVehicleInputs', filename: str, init_pos: WAVector = WAVector(), init_rot: WAQuaternion = WAQuaternion.from_z_rotation(0), init_pos_dt: WAVector = WAVector()):
super().__init__(system, vehicle_inputs, filename)
# Simple state variables
self._x = init_pos.x
self._y = init_pos.y
self._yaw = init_rot.to_euler_yaw()
self._v = init_pos_dt.length
self._acc = 0.0
self._steering = 0
self._throttle = 0
self._braking = 0
# Wheel angular velocity
self._omega = 0.0
self._omega_dot = 0.0
self._yaw_dt = 0.0
self._yaw_dtdt = 0.0
self._last_yaw = 0.0
properties = load_properties_from_json(filename, "Vehicle Properties")
self._initialize(properties)
def test_sub2(self):
"""Tests subtracting a constant to a WAVector"""
v = WAVector([1, 1, 1])
n = 5
self.assertEqual(v - n, WAVector([-4, -4, -4]))
def test_add3(self):
"""Tests adding a constant to a WAVector"""
v = WAVector([1, 1, 1])
n = 5
self.assertEqual(v + n, WAVector([6, 6, 6]))
def test_dot(self):
"""Tests dot product of two WAVectors"""
v1 = WAVector([1, 4, 5])
v2 = WAVector([6, 9, 10])
self.assertEqual(v1.dot(v2), 92)
def test_mul(self):
"""Tests multiplying a vector by a constant"""
v = WAVector([1, 2, 3])
n = 5
self.assertEqual(v * n, WAVector([5, 10, 15]))
