swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
spencerfolk
2023-03-15 15:38:14 -04:00
commit 4d7fca10e4
62 changed files with 5891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
rotorpy/sensors/README.md Normal file
View File

@@ -0,0 +1,11 @@
# Sensor Module
Sensors are implemented as ways to convert the current ground truth vehicle state (and possibly obstacles and/or control commands) to measurements that reflects information that might be available in the real world.
The most straightforward examples of sensors are in `imu.py` and `external_mocap.py` which mimic an inertial measurement unit and external motion capture system (e.g. Vicon), respectively.
The IMU sensor can be arbitrarily placed and oriented with respect to the body axes. Bias and noise intensity can be specified for each sensor axis, in addition to bias drift (diffusion).
The external motion capture sensor provides noisy measurements of pose and twist. The sensor is placed at the center of mass aligned with the body axes. There is also a parameter that enables "artifacts", which are the consequence of numerical differentiation or glitches in the system causing spikes in the measurements.
See each sensor file for more information.

0
rotorpy/sensors/__init__.py Normal file
View File

202
rotorpy/sensors/external_mocap.py Normal file
View File

@@ -0,0 +1,202 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
def hat_map(s):
"""
Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
"""
return np.array([[ 0, -s[2], s[1]],
[ s[2], 0, -s[0]],
[-s[1], s[0], 0]])
class MotionCapture():
"""
The external motion capture is able to provide pose and twist measurements of the vehicle.
Given the current ground truth state of the vehicle, it will output noisy measurements of the
pose and twist. Artifacts can be introduced
"""
def __init__(self, sampling_rate,
mocap_params={'pos_noise_density': 0.0005*np.ones((3,)), # noise density for position
'vel_noise_density': 0.005*np.ones((3,)), # noise density for velocity
'att_noise_density': 0.0005*np.ones((3,)), # noise density for attitude
'rate_noise_density': 0.0005*np.ones((3,)), # noise density for body rates
'vel_artifact_max': 5, # maximum magnitude of the artifact in velocity (m/s)
'vel_artifact_prob': 0.001, # probability that an artifact will occur for a given velocity measurement
'rate_artifact_max': 1, # maximum magnitude of the artifact in body rates (rad/s)
'rate_artifact_prob': 0.0002 # probability that an artifact will occur for a given rate measurement
},
with_artifacts=False):
"""
Parameters:
sampling_rate, Hz, the rate at which this sensor is being sampled. Used for computing the noise.
mocap_params, a dict with keys
pos_noise_density, position noise density, [m/sqrt(Hz)]
vel_noise_density, velocity noise density, [m/s / sqrt(Hz)]
att_noise_density, attitude noise density, [rad / sqrt(Hz)]
rate_noise_density, attitude rate noise density, [rad/s /sqrt(Hz)]
vel_artifact_prob, probability that a spike will occur for a given velocity measurement
vel_artifact_max, the maximum magnitude of the artifact spike. [m/s]
rate_artifact_prob, probability that a spike will occur for a given body rate measurement
rate_artifact_max, the maximum magnitude of hte artifact spike. [rad/s]
"""
self.rate_scale = np.sqrt(sampling_rate/2)
# Noise densities
self.pos_density = mocap_params['pos_noise_density']
self.vel_density = mocap_params['vel_noise_density']
self.att_density = mocap_params['att_noise_density']
self.rate_density = mocap_params['rate_noise_density']
# Artifacts
self.vel_artifact_prob = mocap_params['vel_artifact_prob']
self.vel_artifact_max = mocap_params['vel_artifact_max']
self.rate_artifact_prob = mocap_params['rate_artifact_prob']
self.rate_artifact_max = mocap_params['rate_artifact_max']
self.initialized = True
self.with_artifacts = with_artifacts
def measurement(self, state, with_noise=False, with_artifacts=False):
"""
Computes and returns the sensor measurement at a time step.
Inputs:
state, a dict describing the state with keys
x, position, m, shape=(3,)
v, linear velocity, m/s, shape=(3,)
q, quaternion [i,j,k,w], shape=(4,)
w, angular velocity (in LOCAL frame!), rad/s, shape=(3,)
with_noise, a boolean to indicate if noise is added
with_artifacts, a boolean to indicate if artifacts are added.
Artifacts are added to the velocity and angular rates, and are due
to the numerical differentiation scheme used by motion capture systems.
They will appear as random spikes in the data.
Outputs:
observation, a dictionary with keys
x_m, noisy position measurement, m, shape=(3,)
v_m, noisy linear velocity, m/s, shape=(3,)
q_m, noisy quaternion [i,j,k,w], shape=(4,)
w_m, noisy angular velocity (in LOCAL frame!), rad/s, shape=(3,)
"""
x_measured = copy.deepcopy(state['x']).astype(float)
v_measured = copy.deepcopy(state['v']).astype(float)
q_measured = Rotation.from_quat(copy.deepcopy(state['q']))
w_measured = copy.deepcopy(state['w']).astype(float)
if with_noise:
# Add noise to the measurements based on the provided measurement noise.
x_measured += self.rate_scale * np.random.normal(scale=np.abs(self.pos_density))
v_measured += self.rate_scale * np.random.normal(scale=np.abs(self.vel_density))
w_measured += self.rate_scale * np.random.normal(scale=np.abs(self.rate_density))
# Noise has to be treated differently with quaternions...
# Following https://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf pg 43
# First, let's produce a perturbation vector in R3
delta_phi = self.rate_scale*np.random.normal(scale=np.abs(self.att_density))
# Now convert that to a rotation matrix
delta_rotation = Rotation.from_matrix(np.eye(3) + hat_map(delta_phi))
# Now apply that rotation to the quaternion
q_measured = (q_measured * delta_rotation).as_quat()
else:
q_measured = q_measured.as_quat()
if with_artifacts:
# If including artifacts, first roll the dice on whether or not a spike should occur for each measurement:
vel_spike_bool = np.random.choice([0,1], p=[1-self.vel_artifact_prob, self.vel_artifact_prob])
rate_spike_bool = np.random.choice([0,1], p=[1-self.rate_artifact_prob, self.rate_artifact_prob])
# Choose the axis that the spike will occur on
vel_axis = np.random.choice([0,1,2])
rate_axis = np.random.choice([0,1,2])
# Choose the sign of the spike
vel_sign = np.random.choice([-1,1])
rate_sign = np.random.choice([-1,1])
if vel_spike_bool:
v_measured[vel_axis] += vel_sign*np.random.uniform(low=0, high=self.vel_artifact_max)
if rate_spike_bool:
w_measured[rate_axis] += rate_sign*np.random.uniform(low=0, high=self.rate_artifact_max)
return {'x': x_measured, 'q': q_measured, 'v': v_measured, 'w': w_measured}
if __name__=="__main__":
import matplotlib.pyplot as plt
def merge_dicts(dicts_in):
"""
Concatenates contents of a list of N state dicts into a single dict by
prepending a new dimension of size N. This is more convenient for plotting
and analysis. Requires dicts to have consistent keys and have values that
are numpy arrays.
"""
dict_out = {}
for k in dicts_in[0].keys():
dict_out[k] = []
for d in dicts_in:
dict_out[k].append(d[k])
dict_out[k] = np.array(dict_out[k])
return dict_out
sim_rate = 1/500
mocap_params = {'pos_noise_density': 0.0005*np.ones((3,)), # noise density for position
'vel_noise_density': 0.005*np.ones((3,)), # noise density for velocity
'att_noise_density': 0.0005*np.ones((3,)), # noise density for attitude
'rate_noise_density': 0.0005*np.ones((3,)), # noise density for body rates
'vel_artifact_max': 5, # maximum magnitude of the artifact in velocity (m/s)
'vel_artifact_prob': 0.001, # probability that an artifact will occur for a given velocity measurement
'rate_artifact_max': 1, # maximum magnitude of the artifact in body rates (rad/s)
'rate_artifact_prob': 0.0002 # probability that an artifact will occur for a given rate measurement
}
sensor = MotionCapture(sampling_rate=sim_rate, mocap_params=mocap_params, with_artifacts=True)
measurements = []
state = {'x': np.zeros((3,)), 'v': np.zeros((3,)), 'q': np.array([0,0,0,1]), 'w': np.zeros((3,))}
for i in range(1000):
state = {'x': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)]),
'v': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)]),
'q': np.array([0,0,0,1]),
'w': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)])}
current = sensor.measurement(state, with_noise=True, with_artifacts=True)
measurements.append(current)
measurements = merge_dicts(measurements)
x_m = measurements['x']
v_m = measurements['v']
q_m = measurements['q']
w_m = measurements['w']
q_norm = np.linalg.norm(q_m, axis=1)
(fig, axes) = plt.subplots(nrows=4, ncols=1, sharex=True, num="Measurements")
fig.set_figwidth(9)
fig.set_figheight(9)
axe = axes[0]
axe.plot(x_m[:,0], 'r', markersize=2)
axe.plot(x_m[:,1], 'g', markersize=2)
axe.plot(x_m[:,2], 'b', markersize=2)
axe.set_ylim(bottom=-1.5, top=1.5)
axe = axes[1]
axe.plot(v_m[:,0], 'r', markersize=2)
axe.plot(v_m[:,1], 'g', markersize=2)
axe.plot(v_m[:,2], 'b', markersize=2)
axe.set_ylim(bottom=-1.5, top=1.5)
axe = axes[2]
axe.plot(q_m[:,0], 'r', markersize=2)
axe.plot(q_m[:,1], 'g', markersize=2)
axe.plot(q_m[:,2], 'b', markersize=2)
axe.plot(q_m[:,3], 'm', markersize=2)
axe.plot(q_norm, 'k', markersize=2)
axe = axes[3]
axe.plot(w_m[:,0], 'r', markersize=2)
axe.plot(w_m[:,1], 'g', markersize=2)
axe.plot(w_m[:,2], 'b', markersize=2)
axe.set_ylim(bottom=-1.5, top=1.5)
plt.show()

147
rotorpy/sensors/imu.py Normal file
View File

@@ -0,0 +1,147 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
class Imu:
"""
Simulated IMU measurement given
1) quadrotor's ground truth state and acceleration, and
2) IMU's pose in quadrotor body frame.
CREDIT:
Partial implementation from Yu-Ming Chen and MEAM 620 Sp2022 Teaching Staff
Finishing touches added by Alexander Spinos, checked by Spencer Folk.
"""
def __init__(self, accelerometer_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2
'noise_density': (0.38**2)*np.ones(3,), # m/s^2 / sqrt(Hz)
'random_walk': np.zeros(3,) # m/s^2 * sqrt(Hz)
},
gyroscope_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2
'noise_density': (0.01**2)*np.ones(3,), # rad/s / sqrt(Hz)
'random_walk': np.zeros(3,) # rad/s * sqrt(Hz)
},
R_BS = np.eye(3),
p_BS = np.zeros(3,),
sampling_rate=500,
gravity_vector=np.array([0,0,-9.81])):
"""
Parameters:
R_BS, the rotation matrix of sensor frame S in body frame B
p_BS, the position vector from frame B's origin to frame S's origin, expressed in frame B
accelerometer_params, a dict with keys:
initial_bias, accelerometer contant bias, [ m / s^2 ]
noise_density, accelerometer "white noise", [ m / s^2 / sqrt(Hz) ]
random_walk, accelerometer bias diffusion, [ m / s^2 * sqrt(Hz) ]
gyroscope_params, a dict with keys:
initial_bias, gyro contant bias, [ m / s^2 ]
noise_density, gyro "white noise", [ rad / s / sqrt(Hz) ]
random_walk, gyro bias diffusion, [ rad / s * sqrt(Hz) ]
sampling_rate, the sampling rate of the sensor, Hz (1/s)
gravity_vector, the gravitational vector in world frame (should be ~ [0, 0 , -9.81])
"""
# A few checks
if type(R_BS) != np.ndarray:
raise TypeError("R_BS's type is not numpy.ndarray")
if type(p_BS) != np.ndarray:
raise TypeError("p_BS's type is not numpy.ndarray")
if type(gravity_vector) != np.ndarray:
raise TypeError("gravity_vector's type is not numpy.ndarray")
if R_BS.shape != (3, 3):
raise ValueError("R_BS's size is not (3, 3)")
if p_BS.shape != (3,):
raise ValueError("p_BS's size is not (3,)")
if gravity_vector.shape != (3,):
raise ValueError("gravity_vector's size is not (3,)")
self.R_BS = R_BS
self.p_BS = p_BS
self.rate_scale = np.sqrt(sampling_rate/2)
self.gravity_vector = gravity_vector
self.accel_variance = accelerometer_params['noise_density'].astype('float64')
self.accel_random_walk = accelerometer_params['random_walk'].astype('float64')
self.accel_bias = accelerometer_params['initial_bias'].astype('float64')
self.gyro_variance = gyroscope_params['noise_density'].astype('float64')
self.gyro_random_walk = gyroscope_params['random_walk'].astype('float64')
self.gyro_bias = gyroscope_params['initial_bias'].astype('float64')
def bias_step(self):
"""Simulate bias drift"""
self.accel_bias += np.random.normal(scale=self.accel_random_walk) / self.rate_scale
self.gyro_bias += np.random.normal(scale=self.gyro_random_walk) / self.rate_scale
return
def measurement(self, state, acceleration, with_noise=True):
"""
Computes and returns the IMU measurement at a time step.
Inputs:
state, a dict describing the state with keys
x, position, m, shape=(3,)
v, linear velocity, m/s, shape=(3,)
q, quaternion [i,j,k,w], shape=(4,)
w, angular velocity (in LOCAL frame!), rad/s, shape=(3,)
acceleration, a dict describing the acceleration with keys
vdot, quadrotor's linear acceleration expressed in world frame, m/s^2, shape=(3,)
wdot, quadrotor's angular acceleration expressed in world frame, rad/s^2, shape=(3,)
Outputs:
observation, a dict describing the IMU measurement with keys
accel, simulated accelerometer measurement, m/s^2, shape=(3,)
gyro, simulated gyroscope measurement, rad/s^2, shape=(3,)
"""
q_WB = state['q']
w_WB = state['w']
alpha_WB_W = acceleration['wdot']
a_WB_W = acceleration['vdot']
# Rotation matrix of the body frame B in world frame W
R_WB = Rotation.from_quat(q_WB).as_matrix()
# Sensor position in body frame expressed in world coordinates
p_BS_W = R_WB @ self.p_BS
# Linear acceleration of point S (the imu) expressed in world coordinates W.
a_WS_W = a_WB_W + np.cross(alpha_WB_W, p_BS_W) + np.cross(w_WB, np.cross(w_WB, p_BS_W))
# Rotation from world to imu: R_SW = R_SB * R_BW
R_SW = self.R_BS.T @ R_WB.T
# Rotate to local frame
accelerometer_measurement = R_SW @ (a_WS_W - self.gravity_vector)
gyroscope_measurement = copy.deepcopy(w_WB).astype(float)
# Add the bias drift (default 0)
self.bias_step()
# Add biases and noises
accelerometer_measurement += self.accel_bias
gyroscope_measurement += self.gyro_bias
if with_noise:
accelerometer_measurement += self.rate_scale * np.random.normal(scale=np.abs(self.accel_variance))
gyroscope_measurement += self.rate_scale * np.random.normal(scale=np.abs(self.gyro_variance))
return {'accel': accelerometer_measurement, 'gyro': gyroscope_measurement}
if __name__=="__main__":
# The default
sim_rate = 100
accelerometer_params = {'initial_bias': np.array([0,0,0]), # m/s^2
'noise_density': (0.38**2)*np.ones(3,), # m/s^2 / sqrt(Hz)
'random_walk': np.zeros(3,) # m/s^2 * sqrt(Hz)
}
gyroscope_params = {'initial_bias': np.array([0,0,0]), # m/s^2
'noise_density': (0.01**2)*np.ones(3,), # rad/s / sqrt(Hz)
'random_walk': np.zeros(3,) # rad/s * sqrt(Hz)
}
imu = Imu(accelerometer_params,
gyroscope_params,
p_BS = np.zeros(3,),
R_BS = np.eye(3),
sampling_rate=sim_rate)
print(imu.measurement({'x': np.array([0,0,0]), 'v': np.array([0,0,0]), 'q': np.array([0,0,0,1]), 'w': np.array([0,0,0])},
{'vdot': np.array([0,0,0]), 'wdot': np.array([0,0,0])}))