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