def __init__(self, filename):
self.messages = load_messages(filename)
# messages with gitsha1 are transmitted before the simulation loop
for i, msg in enumerate(self.messages):
if msg.HasField('gitsha1'):
start_index = i + 1
else:
break
self.records = get_records_from_messages(self.messages)[start_index:]
self.t = get_time_vector(self.records)
self.states = self.records.state[:, 1:] # skip yaw angle
self.colors = np.roll(sns.color_palette('Paired', 12), 2, axis=0)
steer_encoder_counts_per_rev = 152000
encoder_count = self.records.sensors.steer_encoder_count
encoder_angle = encoder_count / steer_encoder_counts_per_rev * 2*np.pi
encoder_angle[np.where(encoder_angle > np.pi)[0]] -= 2*np.pi
self.measured_steer_angle = encoder_angle
self.kollmorgen_command_torque = convert_kollmorgen_command_dac(
self.records.actuators.kollmorgen_command_torque)
self.kollmorgen_applied_torque = convert_kollmorgen_applied_adc(
self.records.sensors.kollmorgen_actual_torque)
self.kistler_sensor_torque = convert_kistler_torque_adc(
self.records.sensors.kistler_measured_torque)
def __init__(self, filename):
self.messages = load_messages(filename)
# messages with gitsha1 are transmitted before the simulation loop
for i, msg in enumerate(self.messages):
if msg.HasField('gitsha1'):
start_index = i + 1
else:
break
self.records = get_records_from_messages(self.messages)[start_index:]
self.t = get_time_vector(self.records)
self.states = self.records.state[:, 1:] # skip yaw angle
self.colors = np.roll(sns.color_palette('Paired', 12), 2, axis=0)
steer_encoder_counts_per_rev = 152000
encoder_count = self.records.sensors.steer_encoder_count
encoder_angle = encoder_count / steer_encoder_counts_per_rev * 2 * np.pi
encoder_angle[np.where(encoder_angle > np.pi)[0]] -= 2 * np.pi
self.measured_steer_angle = encoder_angle
self.kollmorgen_command_torque = convert_kollmorgen_command_dac(
self.records.actuators.kollmorgen_command_torque)
self.kollmorgen_applied_torque = convert_kollmorgen_applied_adc(
self.records.sensors.kollmorgen_actual_torque)
self.kistler_sensor_torque = convert_kistler_torque_adc(
self.records.sensors.kistler_measured_torque)
def estimate_inertia(records):
"""We assume no stiffness or damping for the equation of motion is:
T = I*alpha
where T is the sum of torques applied
I is the moment of inertia
alpha is the angular acceleration
"""
t = get_time_vector(records)
torque = (
((records.sensors.kollmorgen_actual_torque).astype(float) - 2**11) /
2**11 * KOLLMORGEN_MAX_TORQUE) # N-m
angle = np.unwrap(((records.sensors.steer_encoder_count).astype(float) *
2 * np.pi / ENCODER_COUNT_PER_REV)) # radians
dt = np.diff(t).mean()
angle_d = sg(angle, 11, 3, deriv=1, delta=dt, mode='nearest')
angle_dd = sg(angle, 11, 3, deriv=2, delta=dt, mode='nearest')
color = sns.color_palette('Paired', 10)
fig, ax = plt.subplots()
ax.plot(t, angle, label='angle', color=color[0])
ax.plot(t, angle_d, label='angular rate', color=color[2])
ax.plot(t, angle_dd, label='angular accel', color=color[4])
ax.plot(t, torque, label='torque', color=color[6])
ret = np.linalg.lstsq(np.reshape(angle_dd, (-1, 1)),
np.reshape(torque, (-1, 1)))
inertia = np.squeeze(ret[0])
ax.plot(t, inertia * angle_dd, label='estimated torque', color=color[7])
ax.legend()
plt.show()
return inertia
if __name__ == '__main__':
import sys
import matplotlib.pyplot as plt
from plot_sim import plot_states
sim_time = 3 # time for our Whipple simulation or upper limit [s]
start_index = 50 # number of records to skip to allow some time for state
# estimate to converge
if len(sys.argv) > 1:
from load_sim import (load_messages, get_records_from_messages,
get_time_vector)
messages = load_messages(sys.argv[1])
records = get_records_from_messages(messages)
t = get_time_vector(records[start_index:])
t -= t[0] # redefine zero time for simulator data
m = messages[0]
v = m.model.v
dt = m.model.dt
if t[-1] < sim_time:
sim_time = t[-1]
n = int(sim_time / dt)
states = records[start_index:].state[:, 1:]
fig2, ax2 = plot_states(t[:n], states[:n, :], to_degrees=True)
fig2.suptitle('Phobos simulator')
x0 = np.array(messages[start_index].state.x[1:]).reshape((4, 1))
else:
v = 5
file_dir = os.path.dirname(os.path.realpath(__file__))
sys.path.append(os.path.abspath(os.path.join(file_dir, os.pardir)))
from plot_sim import plot_states
from load_sim import load_messages, get_records_from_messages, get_time_vector
from simulate_whipple_benchmark import simulate
if __name__ == '__main__':
messages = load_messages(os.path.join(file_dir, 'whipple.pb'))
start_index = 50 # ignore some samples to give Kalman state estimate
# time to converge
sim_time = 3
stride = 10 # don't plot all the points
records = get_records_from_messages(messages)[start_index::stride]
t = get_time_vector(records)
t -= t[0] # redefine zero time for simulator data
# plot in degrees because understanding radians is harder
states = records.state[:, 1:]
fig1, ax1 = plot_states(t, states, second_yaxis=True, to_degrees=True)
m = messages[0]
v = m.model.v
dt = m.model.dt * stride
n = int(sim_time / dt)
x0 = np.array(messages[start_index].state.x[1:]).reshape((4, 1))
x = simulate(v, x0, dt, n)
t = np.array(range(n + 1)) * dt
fig2, ax2 = plot_states(t,
np.hstack((x0, x)).T,
if __name__ == '__main__':
import sys
import matplotlib.pyplot as plt
from plot_sim import plot_states
sim_time = 3 # time for our Whipple simulation or upper limit [s]
start_index = 50 # number of records to skip to allow some time for state
# estimate to converge
if len(sys.argv) > 1:
from load_sim import (load_messages, get_records_from_messages,
get_time_vector)
messages = load_messages(sys.argv[1])
records = get_records_from_messages(messages)
t = get_time_vector(records[start_index:])
t -= t[0] # redefine zero time for simulator data
m = messages[0]
v = m.model.v
dt = m.model.dt
if t[-1] < sim_time:
sim_time = t[-1]
n = int(sim_time/dt)
states = records[start_index:].state[:, 1:]
fig2, ax2 = plot_states(t[:n], states[:n, :], to_degrees=True)
fig2.suptitle('Phobos simulator')
x0 = np.array(messages[start_index].state.x[1:]).reshape((4, 1))
else:
v = 5