def test_1d_column(self):
r = RingBuffer(5, dtype=(np.int, 1))
r.append(1)
r.append(2)
r.appendleft(3)
self.assertEqual(np.shape(r), (3, 1))
np.testing.assert_equal(r, np.array([[3], [1], [2]]))
class RingbufferMonitor(Monitor):
def __init__(self,
capacity=100,
key='training_loss',
name='ringbuffer_monitor'):
super().__init__(name=name)
self.capacity = capacity
self.key = key
self.value_buffer = None
self.step_buffer = RingBuffer(capacity=capacity, dtype='float32')
self.previous_session_steps = 0
self.last_step = 0
def start_session(self, num_steps, **kwargs):
self.previous_session_steps += self.last_step
def write(self, data, step, **kwargs):
if self.key in data.keys():
value = data[self.key]
if isinstance(value, Tensor):
dtype = value.data.cpu().numpy().dtype
value = self._to_scalar(value)
else:
dtype = np.array(value).dtype
if self.value_buffer is None:
self.value_buffer = RingBuffer(capacity=self.capacity,
dtype=dtype)
self.value_buffer.append(value)
self.step_buffer.append(step + self.previous_session_steps)
self.last_step = math.ceil(step)
def mean(self, *args, **kwargs):
return np.array(self.value_buffer).mean(*args, **kwargs)
def std(self, *args, **kwargs):
return np.array(self.value_buffer).std(*args, **kwargs)
@property
def movement_index(self):
steps = self.step_buffer
if len(steps) < self.capacity:
# not enough information gathered yet to say...
return None
else:
step_difference = (steps[-1] - steps[0])
midpoint = np.argmax(steps >= steps[0] + (step_difference // 2))
values = np.array(self.value_buffer)
first = values[:midpoint]
last = values[midpoint:]
d = (np.abs(first.mean() - last.mean()) /
(2 * np.sqrt(first.std() * last.std())))
return d
class SensirionSBPlot(QWidget):
def __init__(self, plot_title, color, bufferSize):
super().__init__()
masterLayout = QVBoxLayout()
self.pen = mkPen(color, width=1.25)
layout = QVBoxLayout()
self.group = QGroupBox(plot_title)
self.plot = PlotWidget()
self.plot.getPlotItem().showGrid(x=True, y=True, alpha=1)
if "qdarkstyle" in sys.modules:
self.plot.setBackground((25, 35, 45))
self.buffer = RingBuffer(capacity=bufferSize, dtype=float)
self.group.setLayout(layout)
layout.addWidget(self.plot)
masterLayout.addWidget(self.group)
self.setLayout(masterLayout)
def change_capacity(self, value):
if value > len(self.buffer):
newBuf = RingBuffer(capacity=value, dtype=float)
newBuf.extend(self.buffer)
self.buffer = newBuf
elif value < len(self.buffer):
newBuf = RingBuffer(capacity=value, dtype=float)
newBuf.extend(self.buffer[:-value])
self.buffer = newBuf
def update_plot(self, sample):
self.plot.clear()
self.buffer.append(sample)
self.plot.plot(self.buffer, pen=self.pen, symbolPen=self.pen, symbol='o', symbolSize=5, name="symbol ='o'")
def init_buf():
'''Initialize buffer'''
buf = RingBuffer(p.winsamp, dtype=np.int16)
eps = np.finfo(float).eps
for _ in range(p.winsamp):
buf.append(eps)
return buf
class CausalVelocityFilter():
"""Calculates the derivative of data that's appended to the
internal buffer and allows you to get a moving average of the
derivative with window size defined by the init.
"""
def __init__(self, windowSize):
"""Initializes self.
windowSize defines the width of the moving average buffer.
Too narrow and it doesn't do much filtering, too wide and it
lags behind the acutal derivative
"""
self._derivativeBuffer = RingBuffer(windowSize)
self._previousDatum = 0.0
def get_datum(self):
"""Get the present rate of change of the incomimg data"""
return np.mean(self._derivativeBuffer)
def append(self, datum, timeElapsed):
"""Add a measurement and an elapsed time to the filter's
internal buffer to be able to find the rate of change.
"""
self._derivativeBuffer.append(
(datum - self._previousDatum) / timeElapsed)
self._previousDatum = datum
class Ticker:
def __init__(self, lookback, exchange, symbol, bid, ask, updated_at):
self.exchange = exchange
self.symbol = symbol
self.bid = bid
self.ask = ask
self.updated_at = updated_at
self.bid_lookback = RingBuffer(capacity=lookback, dtype=object)
self.ask_lookback = RingBuffer(capacity=lookback, dtype=object)
def get_bid(self):
return self.bid
def get_ask(self):
return self.ask
def get_time(self):
return self.updated_at
def update(self, data):
self.bid = data['bid']
self.ask = data['ask']
self.updated_at = data['time']
self.bid_lookback.append(self.bid)
self.ask_lookback.append(self.ask)
def main():
window = RingBuffer(capacity=100, dtype=(float, 4))
with open("log.json", "r") as data_file:
for line in data_file:
data = ujson.loads(line)
if data['data']['cpu']['percent'] < 50:
window.append(
[data['data']['cpu']['percent'], data['ts'], 0, 0])
print(np.mean(window, 0)[0])
def test_sizes(self):
r = RingBuffer(5, dtype=(int, 2))
self.assertEqual(r.maxlen, 5)
self.assertEqual(len(r), 0)
self.assertEqual(r.shape, (0, 2))
r.append([0, 0])
self.assertEqual(r.maxlen, 5)
self.assertEqual(len(r), 1)
self.assertEqual(r.shape, (1, 2))
def test_negative_indices_give_recent_data_with_unfull_buffer(self):
r = RingBuffer(10)
for i in range(5):
r.append(i)
for i in reversed(range(-len(r), 0)):
self.assertAlmostEqual(r[i],
r[i + 5],
msg="Fails to index from the back of "
"the filled portion of the buffer "
"given a negative index.")
def test_degenerate(self):
r = RingBuffer(0)
np.testing.assert_equal(r, np.array([]))
# this does not error with deque(maxlen=0), so should not error here
try:
r.append(0)
r.appendleft(0)
except IndexError:
self.fail()
def find_convergence_episode(rewards_list, buffer_size=3):
import numpy as np
from numpy_ringbuffer import RingBuffer
convergence_buffer = RingBuffer(capacity=buffer_size)
res = -1
for index, reward_value in enumerate(rewards_list):
convergence_buffer.append(reward_value)
if (len(np.unique(convergence_buffer)) == 1
and len(convergence_buffer) == 3):
res = index - 1
return res
return res
class CausalIntegralFilter():
def __init__(self, initial_value, initial_time):
self._y = initial_value
self._t_buffer = RingBuffer(2)
self._t_buffer.append(initial_time)
def append_integral_value(self, y):
self._y = y
def append(self, dydt, t):
self._t_buffer.append(t)
self._y += dydt * (self._t_buffer[-1] - self._t_buffer[-2])
def get_datum(self):
return self._y
class KeywordDetector:
def __init__(
self,
checkpoint: Path,
threshold: float = 0.9,
smoothing: int = 5,
timeout: float = 1.0,
):
self.device = torch.device(
"cuda" if torch.cuda.is_available() else "cpu")
self.model, self.config = model_from_checkpoint(checkpoint,
device=self.device)
self.model = self.model.eval().to(self.device)
self.extractor = init_feature_extractor(self.config)
self.threshold = threshold
self.smoothing = smoothing
self.timeout = timeout
self.input_samples = self.config.fft_window_step * (
self.model.input_size - 1)
self.preds_buffer = RingBuffer(self.smoothing, "float32")
self.raw_input_buffer = RingBuffer(capacity=self.input_samples,
dtype="float32")
self.raw_input_buffer.extend(
np.zeros(self.input_samples, dtype="float32"))
self.cooldown_time = 0
def push_audio(self, signal: np.ndarray):
self.raw_input_buffer.extend(signal)
@torch.no_grad()
def predict(self):
chunk = self.raw_input_buffer[-self.input_samples:].copy()
chunk = librosa.util.normalize(chunk)
x = self.extractor(chunk).astype("float32")
x = x.reshape((1, self.model.input_channels, self.model.input_size))
x = torch.as_tensor(x).to(self.device)
pred = self.model.forward(x)
pred = torch.softmax(pred[0], 0).cpu().numpy()[1]
self.preds_buffer.append(pred)
mean = np.mean(self.preds_buffer)
trigger = mean > self.threshold and self.cooldown_time < time()
if trigger:
self.cooldown_time = time() + self.timeout
return mean, trigger
def test_2d(self):
r = RingBuffer(5, dtype=(np.float, 2))
r.append([1, 2])
np.testing.assert_equal(r, np.array([[1, 2]]))
self.assertEqual(len(r), 1)
self.assertEqual(np.shape(r), (1, 2))
r.append([3, 4])
np.testing.assert_equal(r, np.array([[1, 2], [3, 4]]))
self.assertEqual(len(r), 2)
self.assertEqual(np.shape(r), (2, 2))
r.appendleft([5, 6])
np.testing.assert_equal(r, np.array([[5, 6], [1, 2], [3, 4]]))
self.assertEqual(len(r), 3)
self.assertEqual(np.shape(r), (3, 2))
np.testing.assert_equal(r[0], [5, 6])
np.testing.assert_equal(r[0, :], [5, 6])
np.testing.assert_equal(r[:, 0], [5, 1, 3])
class PatientTubingDescriptorCalculator():
def __init__(self, current_time):
self._flow_rate_sample_times = RingBuffer(2)
self._flow_rate_sample_times.append(current_time)
self._tidal_volume_filter = TidalVolume(current_time)
self._peep_filter = PEEP(0.1)
self._pip_filter = PIP(0.1)
self._most_recent_pressure = 0.0
self._most_recent_flow_rate = 0.0
def add_flow_rate_datum(self, datum, current_time):
self._tidal_volume_filter.append(datum, current_time)
self._most_recent_flow_rate = datum
def add_pressure_datum(self, datum):
self._peep_filter.append(datum)
self._pip_filter.append(datum)
self._most_recent_pressure = datum
# def add_tidal_volume_value(self, tidal_volume):
# self._tidal_volume_filter.append_integral_value(tidal_volume)
def _PEEP(self):
return self._peep_filter.get_datum()
def _PIP(self):
return self._pip_filter.get_datum()
def _tidal_volume(self):
return self._tidal_volume_filter.get_datum()
@property
def descriptors(self):
return {
"Inspiratory Pressure": self._most_recent_pressure,
"PEEP": self._PEEP(),
"PIP": self._PIP(),
"Tidal Volume": self._tidal_volume(),
"Flow Rate": self._most_recent_flow_rate
}
class Buffer(object):
""" a class to represent a circular buffer
Parameters
----------
ringBuffer : numpy_ringbuffer RingBuffer object
the circular buffer
windowLength : int
the capacity of the buffer
sensorChannels : int
the number of features of the multivariate timeseries whose
timestep are stored in the buffer
Methods
-------
append(sensorData)
adds a numpy array of shape = (sensorChannels,) to the buffer after its last element
getBufferedData()
returns all the elements in the buffer as a numpy array
"""
def __init__(self, windowLength, sensorChannels):
self.ringBuffer = RingBuffer(capacity=windowLength, dtype=(float, sensorChannels))
self.sensorChannels = sensorChannels
self.windowLength = windowLength
def append(self, sensorData):
"""adds a numpy array of shape = (sensorChannels,) to the buffer after its last element"""
self.ringBuffer.append(sensorData)
def getBufferedData(self):
"""returns all the elements in the buffer as a numpy array
when the buffer is full the numpy array which is returned has
shape = (windowLength, sensorChannels) otherwise shape[0] of the
returned numpy array is equal to the number of elements in the buffer
"""
if not self.ringBuffer: # first time when buffer is empty
return np.zeros((1, self.windowLength, self.sensorChannels))
return np.array(self.ringBuffer)
def test_pops(self):
r = RingBuffer(3)
r.append(1)
r.appendleft(2)
r.append(3)
np.testing.assert_equal(r, np.array([2, 1, 3]))
self.assertEqual(r.pop(), 3)
np.testing.assert_equal(r, np.array([2, 1]))
self.assertEqual(r.popleft(), 2)
np.testing.assert_equal(r, np.array([1]))
# test empty pops
empty = RingBuffer(1)
with self.assertRaisesRegex(IndexError,
"pop from an empty RingBuffer"):
empty.pop()
with self.assertRaisesRegex(IndexError,
"pop from an empty RingBuffer"):
empty.popleft()
class PatientTubingDescriptorCalculator():
def __init__(self, current_time):
self._flow_rate_sample_times = RingBuffer(2)
self._flow_rate_sample_times.append(current_time)
self._tidal_volume_filter = CausalIntegralFilter(0, current_time)
def add_flow_rate_datum(self, datum, current_time):
self._tidal_volume_filter.append(datum, current_time)
def add_pressure_datum(self, datum):
pass
def add_tidal_volume_value(self, tidal_volume):
self._tidal_volume_filter.append_integral_value(tidal_volume)
def _flow_rate(self):
return random.uniform(-200, 200)
def _inspiratory_pressure(self):
return random.uniform(0, 25)
def _PEEP(self):
return random.uniform(2, 5)
def _peak_pressure(self):
return random.uniform(23, 25)
def _tidal_volume(self):
return self._tidal_volume_filter.get_datum()
@property
def descriptors(self):
return {
"Flow Rate": self._flow_rate(),
"Inspiratory Pressure": self._inspiratory_pressure(),
"PEEP": self._PEEP(),
"Peak Pressure": self._peak_pressure(),
"Tidal Volume": self._tidal_volume()
}
def test_append(self):
r = RingBuffer(5)
r.append(1)
np.testing.assert_equal(r, np.array([1]))
self.assertEqual(len(r), 1)
r.append(2)
np.testing.assert_equal(r, np.array([1, 2]))
self.assertEqual(len(r), 2)
r.append(3)
r.append(4)
r.append(5)
np.testing.assert_equal(r, np.array([1, 2, 3, 4, 5]))
self.assertEqual(len(r), 5)
r.append(6)
np.testing.assert_equal(r, np.array([2, 3, 4, 5, 6]))
self.assertEqual(len(r), 5)
self.assertEqual(r[4], 6)
self.assertEqual(r[-1], 6)
class SensDataProc:
def __init__(self, par):
self.ringBuffer = RingBuffer(capacity=10, dtype=np.float)
self.lpt = par
self.motion = False
def dataProcessing(self, data, num):
epoch = data['epoch']
accel = data['value']
self.ringBuffer.append(abs(accel.x) + abs(accel.y) + abs(accel.z))
variance = np.var(np.array(self.ringBuffer))
if variance <= 0.000001:
motionVal = -6
else:
motionVal = np.log10(variance)
#print("[%i, %i]: %0.2f" % (num, epoch, motionVal))
if motionVal > -2 and not self.motion:
if num == 1:
print("[%i, %i]: %x" % (num, epoch, 0x64))
self.lpt.setData(0x64)
if num == 2:
print("[%i, %i]: %x" % (num, epoch, 0x66))
self.lpt.setData(0x66)
self.motion = True
if motionVal < -3 and self.motion:
if num == 1:
print("[%i, %i]: %x" % (num, epoch, 0x65))
self.lpt.setData(0x65)
if num == 2:
print("[%i, %i]: %x" % (num, epoch, 0x67))
self.lpt.setData(0x67)
self.motion = False
def AB_AM_PECE2_interpatT(f, t: float, tvec: Array[float],
xvec: Array[float]) -> Array[float]:
"""Function computing numerical solution with the predictor-corrector
method of order 2 at (potentially) off-step point t.
- **parameters**, **types**, **return** and **return types**::
:param f: function in x' = f(t,x)
:param t: desired time
:param tvec: vector of times where the solution is available
:param xvec: solution at times tvec
:type f: Callable
:type t: np.float
:type tvec: np.array[float]
:type xvec: np.array[float]
:return: Vector x containing solution at desired time
:rtype: np.array[float]
"""
if (tvec[0] > t or tvec[-1] < t):
print(f"t: {t}, tvec[-1]: {tvec[-1]}")
raise ValueError('Error: t must lie in the interval t0, tn.')
i = np.argwhere(tvec <= t)[-1]
h = t - tvec[i].item()
if (h < 1e-6):
return xvec[i, :]
if (i == 0):
return xvec[i, :]
# TODO compute previous function evaluations
fprevAB = RingBuffer(
2,
dtype=((np.float, xvec.shape[1]) if xvec.shape[1] > 1 else np.float))
hprev = RingBuffer(1, dtype=np.float)
# -1 wrt fprevAB size
fprevAB.append(f(tvec[i - 1], xvec[i - 1, :].flatten()))
fprevAB.append(f(tvec[i], xvec[i, :].flatten()))
hprev.append(tvec[i] - tvec[i - 1])
return _PECE_step(f, xvec[i, :].flatten(), tvec[i], h, fprevAB, hprev,
None, None)[0]
def test_no_overwrite(self):
r = RingBuffer(3, allow_overwrite=False)
r.append(1)
r.append(2)
r.appendleft(3)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.appendleft(4)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.extendleft([4])
r.extendleft([])
np.testing.assert_equal(r, np.array([3, 1, 2]))
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.append(4)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.extend([4])
r.extend([])
# works fine if we pop the surplus
r.pop()
r.append(4)
np.testing.assert_equal(r, np.array([3, 1, 4]))
class SideSubgaitController(object):
def __init__(self,
default,
view,
lock_checked=False,
default_checked=False,
subgait=None):
self._lock_checked = lock_checked
self._default_checked = default_checked
self._subgait = subgait
self._default = default
self.view = view
self.settings_history = RingBuffer(capacity=100, dtype=list)
self.settings_redo_list = RingBuffer(capacity=100, dtype=list)
def undo(self):
self.settings_redo_list.append({
'lock_checked': self._lock_checked,
'default_checked': self._default_checked,
'subgait': self._subgait
})
settings = self.settings_history.pop()
self._lock_checked = settings['lock_checked']
self._default_checked = settings['default_checked']
self._subgait = settings['subgait']
self.view.update_widget(self)
def redo(self):
self.save_changed_settings()
settings = self.settings_redo_list.pop()
self._lock_checked = settings['lock_checked']
self._default_checked = settings['default_checked']
self._subgait = settings['subgait']
self.view.update_widget(self)
def save_changed_settings(self):
self.settings_history.append({
'lock_checked': self._lock_checked,
'default_checked': self._default_checked,
'subgait': self._subgait
})
@property
def lock_checked(self):
return self._lock_checked
@lock_checked.setter
def lock_checked(self, lock_checked):
self.save_changed_settings()
self._lock_checked = lock_checked
self.view.update_widget(self)
@property
def default_checked(self):
return self._default_checked
@default_checked.setter
def default_checked(self, default_checked):
self.save_changed_settings()
self._default_checked = default_checked
self.view.update_widget(self)
@property
def subgait(self):
if self.default_checked:
return self._default
else:
return self._subgait
@subgait.setter
def subgait(self, subgait):
self.save_changed_settings()
self._subgait = subgait
self.view.update_widget(self)
@property
def subgait_text(self):
if self._subgait:
return self._subgait.version
else:
return 'Import'
def run(self):
target_play_mean = []
target_play_std = []
self_play_mean = []
self_play_std = []
current_best = 0
max_episodes = self.config['self_play']['episodes']
max_timestep = self.config['self_play']['max_timestep']
max_timestep_alice = self.config['self_play']['max_timestep_alice']
max_timestep_bob = self.config['self_play']['max_timestep_bob']
max_exploration_episodes = self.config['self_play'][
'exploration_episodes']
stop_probability = self.config['self_play'][
'exploration_stop_probability']
tolerance = self.config['self_play']['tolerance']
stop_update = self.config['self_play']['stop_update_freq']
set_update = self.config['self_play']['set_update_freq']
set2_update = self.config['self_play']['set2_update_freq']
mode = self.config['self_play']['mode']
alternate = self.config['self_play']['alternate']
alternate_step = self.config['self_play']['alternate_step']
test_freq = self.config['self_play']['test_freq']
test_episodes = self.config['self_play']['test_episodes']
max_timestep_target = self.config['target_play']['max_timestep']
max_episodes_target = self.config['target_play']['episodes']
max_exploration_episodes_target = self.config['target_play'][
'exploration_episodes']
test_freq_target = self.config['target_play']['test_freq']
test_episodes_target = self.config['target_play']['test_episodes']
max_timestep_strategy = self.config['self_play'][
'max_timestep_strategy']
ma_window_length = self.config['self_play']['ma_window_length']
ma_multiplier = self.config['self_play']['ma_multiplier']
ma_default_value = self.config['self_play']['ma_default_value']
ma_bias = self.config['self_play']['ma_bias']
#increment = 1. / max_episodes
increment = 1. / 100000
t = trange(max_exploration_episodes,
desc='Self play exploration',
leave=True)
for episode in t:
t.set_description("Self play exploration")
t.refresh()
eval(
'self.explore_self_play_{}(max_timestep, tolerance, stop_probability)'
.format(mode))
t = trange(max_episodes, desc='Self play training', leave=True)
train_teacher = True
last_switch = 0
if max_timestep_strategy == "auto":
time_buffer = RingBuffer(capacity=ma_window_length)
for _ in range(ma_window_length):
time_buffer.append(ma_default_value)
max_timestep = int(np.ceil(ma_multiplier * np.mean(time_buffer)))
for episode in t:
t.set_description("Self play training")
t.refresh()
tA, tB = eval(
'self.self_play_{}(max_timestep_alice, max_timestep_bob, episode, tolerance, stop_update, set_update, alternate, train_teacher)'
.format(mode))
if max_timestep_strategy == "auto":
time_buffer.append(tA)
max_timestep = min(
int(np.ceil(ma_multiplier * np.mean(time_buffer) +
ma_bias)), max_timestep_target)
if alternate:
if episode - last_switch >= alternate_step:
train_teacher = not (train_teacher)
last_switch = episode
self.apply_noise_decay()
self.learners.increment_per(increment)
self.teachers.increment_per(increment)
if episode % test_freq == 0:
test_mean, test_std = self.test(test_episodes,
max_timestep_target,
tolerance,
render=False)
self.writer.add_scalars("self_play/{}".format(self.run_id),
{'average reward': np.mean(test_mean)},
episode)
self_play_mean.append(test_mean)
self_play_std.append(test_std)
if test_mean >= current_best:
current_best = test_mean
self.learners.save(self.path +
"/models_{}".format(self.run_id))
return self_play_mean, self_play_std
self.learners.clear_rb()
t = trange(max_exploration_episodes_target,
desc='Target play exploration',
leave=True)
for episode in t:
t.set_description("Target play exploration")
t.refresh()
self.explore_target_play(max_timestep_target, tolerance)
t = trange(max_episodes_target,
desc='Target play training',
leave=True)
for episode in t:
t.set_description("Target play training")
t.refresh()
self.target_play(max_timestep_target, episode, tolerance)
if episode % test_freq == 0:
test_mean, test_std = self.test(test_episodes_target,
max_timestep_target,
tolerance,
render=False)
self.writer.add_scalars("Target_play/{}".format(self.run_id),
{'average reward': np.mean(test_mean)},
episode)
target_play_mean.append(test_mean)
target_play_std.append(test_std)
return target_play_mean, target_play_std
def test_repr(self):
r = RingBuffer(5, dtype=np.int)
for i in range(5):
r.append(i)
self.assertEqual(repr(r), '<RingBuffer of array([0, 1, 2, 3, 4])>')
class Agent:
def __init__(self, state_shape, n_actions, epsilon=None):
self.capacity = 100000
self.state_memory = RingBuffer(capacity=self.capacity,
dtype=(np.uint8, state_shape))
self.next_state_memory = RingBuffer(capacity=self.capacity,
dtype=(np.uint8, state_shape))
self.action_memory = RingBuffer(capacity=self.capacity, dtype=np.uint8)
self.reward_memory = RingBuffer(capacity=self.capacity,
dtype=np.uint16)
self.done_memory = RingBuffer(capacity=self.capacity, dtype=np.bool)
self.n_actions = n_actions
self.state_shape = state_shape
self.gamma = 0.99
if epsilon is None:
self.epsilon = 1.0
self.epsilon_decay = 0.9997
self.epsilon_min = 0.05
self.decay_epsilon = True
else:
self.epsilon = epsilon
self.decay_epsilon = False
self.q = None
def new_model(self):
model = Sequential()
model.add(
Conv2D(16,
kernel_size=(4, 4),
strides=(2, 2),
activation="relu",
input_shape=self.state_shape))
model.add(
Conv2D(32,
kernel_size=(4, 4),
strides=(2, 2),
activation="relu",
input_shape=self.state_shape))
model.add(Flatten())
model.add(Dense(256, activation="relu"))
model.add(Dense(self.n_actions, activation="linear"))
model.compile(loss="mse", optimizer=Adam()) #lr=0.1))
self.model = model
def save_model(self, name):
try:
self.model.save(name)
except KeyboardInterrupt:
self.model.save(name)
raise
def load_model(self, name):
self.model = load_model(name)
#self.model.optimizer.lr.assign(0.001)
def act(self, state):
if random.random() <= self.epsilon:
return random.randrange(self.n_actions)
state = np.moveaxis(state, 0, -1)
Q_values = self.model.predict(np.stack([state]))[0]
return Q_values.argmax(axis=0)
def remember(self, state, action, reward, next_state, done):
state = np.moveaxis(state, 0, -1)
next_state = np.moveaxis(next_state, 0, -1)
self.state_memory.append(state)
self.next_state_memory.append(next_state)
self.action_memory.append(action)
self.reward_memory.append(reward)
self.done_memory.append(done)
def replay(self, batch_size):
memory_size = len(self.state_memory)
if memory_size > batch_size:
indecies = np.random.choice(memory_size, batch_size, replace=False)
states = self.state_memory[indecies]
next_states = self.next_state_memory[indecies]
actions = self.action_memory[indecies]
rewards = self.reward_memory[indecies]
done = self.done_memory[indecies]
self._fit(self.model, self.gamma, states, next_states, actions,
rewards, done)
if self.decay_epsilon == True:
if self.epsilon <= self.epsilon_min:
self.epsilon = 1.0
self.epsilon *= self.epsilon_decay
def _fit(self, model, gamma, states, next_states, actions, rewards, done):
# Predict future
predicted_future_Q_values = model.predict(next_states)
predicted_future_rewards = predicted_future_Q_values.max(axis=1)
# Calculate expected q values
not_done_targets = np.logical_not(done) * np.add(
rewards, np.multiply(predicted_future_rewards, gamma))
done_targets = done * rewards
targets = np.add(not_done_targets, done_targets)
# Set expected q values for the actions in question
target_Q_values = model.predict(states)
self.q = target_Q_values
target_Q_values[range(len(actions)), actions] = targets
model.fit(states, target_Q_values, epochs=1, verbose=0)
class Kalman(Strategy):
"""
Models fairs based on correlated movements between pairs. Weights predictions by volume and
likelihood of cointegration.
"""
def __init__(
self, window_size, movement_hl, trend_hl, cointegration_period, warmup_signals, warmup_data
):
self.window_size = window_size
self.moving_prices = HoltEma(movement_hl, trend_hl, trend_hl)
self.moving_err_from_prev_fair = Emse(trend_hl)
self.cointegration_period = cointegration_period
self.sample_counter = 0
self.r = None
self.r2 = None
# TODO: do some checks for length/pairs of warmup signals/outputs
prices = pd.concat(
[warmup_signals.xs("price", axis=1, level=1), warmup_data.xs("price", axis=1, level=1)],
axis=1,
sort=False,
)
volumes = pd.concat(
[
warmup_signals.xs("volume", axis=1, level=1),
warmup_data.xs("volume", axis=1, level=1),
],
axis=1,
sort=False,
)
self.price_history = RingBuffer(self.window_size, dtype=(np.float64, len(prices.columns)))
self.price_history.extend(prices.values)
for _, p in prices.iloc[-trend_hl * 4 :].iterrows():
self.moving_prices.step(p)
self.moving_volumes = Ema(movement_hl, volumes.mean())
self.moving_variances = TrendEstimator(
Emse(window_size / 2, (prices.diff()[1:] ** 2).mean()), prices.iloc[-1]
)
self.prev_fair = Gaussian(self.moving_prices.value, [1e100 for _ in prices.columns])
self.coint_f = pd.DataFrame(
1, index=warmup_signals.columns.unique(0), columns=prices.columns
)
if len(self.price_history) < self.window_size or not self.moving_prices.ready:
Log.warn("Insufficient warmup data. Price model will warm up (slowly) in real time.")
else:
Log.info("Price model initialized and warm.")
# the fair combination step assumes that all signals are i.i.d. They are not (and obviously not in the case
# of funds). Is this a problem?
def tick(self, frame, signals):
prices = pd.concat([signals.xs("price", level=1), frame.xs("price", level=1)], sort=False)
volumes = pd.concat(
[signals.xs("volume", level=1), frame.xs("volume", level=1)], sort=False
)
input_names = prices.index
signal_names = signals.index.unique(0)
self.price_history.append(prices)
price_history = pd.DataFrame(self.price_history, columns=input_names)
moving_prices = self.moving_prices.step(prices)
moving_volumes = self.moving_volumes.step(volumes)
stddev = np.sqrt(self.moving_variances.step(prices))
if len(self.price_history) < self.window_size or not self.moving_prices.ready:
return Gaussian(pd.Series([]), [])
# calculate p values for pair cointegration
if self.sample_counter == 0:
for i in signal_names:
for j in input_names:
# ignore collinearity warning
with warnings.catch_warnings():
warnings.filterwarnings("ignore")
p = coint(
price_history[i], price_history[j], trend="nc", maxlag=0, autolag=None
)[1]
self.coint_f.loc[i, j] = 1 + p * 20
self.r = price_history.corr().loc[signal_names]
self.r2 = self.r ** 2
self.sample_counter = (self.sample_counter - 1) % self.cointegration_period
correlated_slopes = self.r.mul(stddev, axis=1).div(stddev[signal_names], axis=0)
# ideally use mkt cap instead of volume?
log_volume = np.log1p(moving_volumes)
volume_f = pd.DataFrame(
log_volume[np.newaxis, :] - log_volume[signal_names][:, np.newaxis],
index=signal_names,
columns=input_names,
)
volume_f = volume_f * (volume_f > 0) + 1
delta = signals.xs("price", level=1) - moving_prices[signal_names]
fair_delta_means = correlated_slopes.mul(delta, axis=0)
delta_vars = self.moving_prices.mse
correlated_delta_vars = np.square(correlated_slopes).mul(delta_vars[signal_names], axis=0)
fair_delta_vars = (correlated_delta_vars + (1 - self.r2) * self.coint_f * delta_vars).mul(
volume_f, axis=0
)
fair_deltas = [
Gaussian(fair_delta_means.loc[i], fair_delta_vars.loc[i]) for i in signal_names
]
fair_delta = intersect_with_disagreement(fair_deltas)
absolute_fair = fair_delta + moving_prices
step = prices - (self.prev_fair.mean + self.moving_prices.trend)
step_vars = self.moving_err_from_prev_fair.step(step)
fair_step_means = correlated_slopes.mul(step[signal_names], axis=0)
correlated_step_vars = np.square(correlated_slopes).mul(step_vars[signal_names], axis=0)
fair_step_vars = (correlated_step_vars + (1 - self.r2) * self.coint_f * step_vars).mul(
volume_f, axis=0
)
fair_steps = [Gaussian(fair_step_means.loc[i], fair_step_vars.loc[i]) for i in signal_names]
fair_step = intersect_with_disagreement(fair_steps)
relative_fair = fair_step + self.prev_fair + self.moving_prices.trend
fair = intersect_with_disagreement([absolute_fair, relative_fair])
self.prev_fair = fair
return fair[frame.index.unique(0)]
class BreathAnalyzer:
thresholdMessageMap = {
0: "You're fine!",
10: "Your breath smells...",
20: "You've been drinking!",
50: "Don't drive!",
80: "You're wasted!"
}
raw_min = 33.0
raw_max = 150.0
scaling_exponent = 1.2
slope_threshold = 5
slope_reference_index_ratio = 0.8 # 0 for oldest, 1 for newest
seconds_to_keep_history = 4
std_dev_lock_limit = 0.6
std_dev_adjust_limit = 0.2
thresholds = sorted(thresholdMessageMap.keys(), reverse=True)
minThreshold = thresholds[-1]
def __init__(self,
samples_per_second,
on_threshold_exceeded,
on_reset=None):
arr_size = int(samples_per_second * self.seconds_to_keep_history)
self.gasHistory = CircularArray(arr_size)
self.slope_reference_index = int(arr_size *
self.slope_reference_index_ratio)
self.on_threshold_exceeded = on_threshold_exceeded
self.on_reset = on_reset
self.header_printed = False
self.lock = True
self.called = True
def debug(self, input_val, max_val, std, ref, slope):
if not DEBUG: return
if not self.header_printed:
self.header_printed = True
states = ("Input", "Max.val.", "Std.dev.", "Ref.", "Slope",
"R_min", "R_max", "Lock")
print(("{:>8} " * len(states)).format(*states))
args = (input_val, max_val, std, ref, slope, self.raw_min,
self.raw_max, self.lock)
print(("{: 8.3f} " * len(args) + " " * 20).format(*args), end='\r')
def add_gas_concentration(self, units):
self.gasHistory.append(units)
if self.gasHistory.is_full:
self.check_concentration()
def check_concentration(self):
arr = self.gasHistory
curr = np.mean(arr[-4:])
ref = np.mean(
arr[self.slope_reference_index:self.slope_reference_index + 4])
slope = curr - ref
std = np.std(arr)
max_val = np.max(arr)
self.debug(curr, max_val, std, ref, slope)
if self.lock:
if std < self.std_dev_lock_limit:
self.reset()
elif slope < 0 and not self.called:
self.called = True
self.call_handler(self.scale(max_val))
else:
if slope > self.slope_threshold:
self.lock = True
elif std < self.std_dev_adjust_limit and abs(self.raw_min -
curr) > 1:
self.raw_min = np.round(np.mean(arr))
def scale(self, value):
# adjust range
if self.raw_max < value:
self.raw_max = np.ceil(value)
if self.raw_min > value:
self.raw_min = np.floor(value)
normalized = ((value - self.raw_min) / (self.raw_max - self.raw_min))
return 100 * normalized**self.scaling_exponent # keep between 0 and 100
def reset(self):
self.lock = False
self.called = False
if callable(self.on_reset):
self.on_reset()
def call_handler(self, value):
if callable(self.on_threshold_exceeded):
self.on_threshold_exceeded(value,
self.get_message_for_threshold(value))
def get_message_for_threshold(self, value):
for key in self.thresholds:
if key <= value:
return self.thresholdMessageMap[key]
return ""
class GaitGeneratorController:
def __init__(self, view, robot):
self.view = view
# Default values
self.gait_directory = None
self.current_gait_path = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.robot = robot
empty_subgait_file = os.path.join(
rospkg.RosPack().get_path("march_rqt_gait_generator"),
"resource/empty_gait/empty_subgait/empty_subgait.subgait",
)
self.subgait = ModifiableSubgait.from_file(robot, empty_subgait_file,
self)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
standing = Subgait.from_file(robot, empty_subgait_file)
previous_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view["previous"], default=standing)
next_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view["next"],
default=standing,
)
self.side_subgait_controller = {
"previous": previous_subgait_controller,
"next": next_subgait_controller,
}
self.connect_buttons()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
# Called by __init__
def connect_buttons(self):
self.view.change_gait_directory_button.clicked.connect(
self.change_gait_directory)
self.view.add_inverse_kinematic_setpoints_button.clicked.connect(
self.add_inverse_kinematics_setpoints)
self.view.import_gait_button.clicked.connect(self.import_gait)
self.view.export_gait_button.clicked.connect(self.export_gait)
self.view.side_subgait_view["previous"].import_button.clicked.connect(
lambda: self.import_side_subgait("previous"))
self.view.side_subgait_view[
"previous"].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "previous", "standing"))
self.view.side_subgait_view[
"previous"].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "previous", "lock"))
self.view.side_subgait_view["next"].import_button.clicked.connect(
lambda: self.import_side_subgait("next"))
self.view.side_subgait_view[
"next"].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "next", "standing"))
self.view.side_subgait_view["next"].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "next", "lock"))
self.view.start_button.clicked.connect(self.start_time_slider_thread)
self.view.stop_button.clicked.connect(self.stop_time_slider_thread)
self.view.invert_button.clicked.connect(self.invert_gait)
self.view.undo_button.clicked.connect(self.undo)
self.view.redo_button.clicked.connect(self.redo)
self.view.ctrl_z.activated.connect(self.undo)
self.view.ctrl_shift_z.activated.connect(self.redo)
# Line edits / combo boxes / spin boxes
self.view.gait_type_combo_box.currentTextChanged.connect(
lambda text: self.subgait.set_gait_type(text), )
self.view.gait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_gait_name(text), )
self.view.version_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_version(text), )
self.view.subgait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_subgait_name(text), )
self.view.description_line_edit.textChanged.connect(
lambda text: self.subgait.set_description(text), )
self.view.playback_speed_spin_box.valueChanged.connect(
self.set_playback_speed)
self.view.duration_spin_box.setKeyboardTracking(False)
self.view.duration_spin_box.valueChanged.connect(
self.update_gait_duration)
# Check boxes
self.view.velocity_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
self.view.effort_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
# Disable key inputs when mirroring is off.
self.view.mirror_check_box.stateChanged.connect(lambda state: [
self.view.mirror_key1_line_edit.setEnabled(state),
self.view.mirror_key2_line_edit.setEnabled(state),
])
# Connect TimeSlider to the preview
self.view.time_slider.valueChanged.connect(lambda time: [
self.set_current_time(time / 100.0),
self.view.publish_preview(self.subgait, self.current_time),
self.view.update_time_sliders(self.current_time),
])
# Called by __init__
def connect_plot(self, joint):
joint_widget = self.view.joint_widgets[joint.name]
joint_plot = joint_widget.Plot.getItem(0, 0)
def add_setpoint(joint, time, position, button):
if button == self.view.control_button:
joint.add_interpolated_setpoint(time)
else:
joint.add_setpoint(ModifiableSetpoint(time, position, 0))
# Connect a function to update the model and to update the table.
joint_plot.plot_item.sigPlotChanged.connect(lambda: [
joint.set_setpoints(joint_plot.to_setpoints()),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.add_setpoint.connect(lambda time, position, button: [
add_setpoint(joint, time, position, button),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.remove_setpoint.connect(lambda index: [
joint.remove_setpoint(index),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_widget.Table.itemChanged.connect(lambda: [
joint.save_setpoints(),
self.save_changed_settings({"joints": [joint]}),
joint.set_setpoints(joint_widget.Table.controller.to_setpoints()),
self.view.update_joint_widget(joint, update_table=False),
self.view.publish_preview(self.subgait, self.current_time),
])
# Functions below are connected to buttons, text boxes, joint graphs etc.
def set_playback_speed(self, playback_speed):
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.playback_speed = playback_speed
rospy.loginfo("Changing playbackspeed to " + str(self.playback_speed))
if was_playing:
self.start_time_slider_thread()
def set_current_time(self, current_time):
self.current_time = current_time
def start_time_slider_thread(self):
if self.time_slider_thread is not None:
rospy.logdebug(
"Cannot start another time slider thread as one is already active"
)
return
current = self.view.time_slider.value()
playback_speed = self.playback_speed
max_time = self.view.time_slider.maximum()
self.time_slider_thread = self.view.create_time_slider_thread(
current, playback_speed, max_time)
self.time_slider_thread.update_signal.connect(
self.view.update_main_time_slider)
self.time_slider_thread.start()
def stop_time_slider_thread(self):
if self.time_slider_thread is not None:
self.time_slider_thread.stop()
self.time_slider_thread = None
def update_gait_duration(self, duration):
rescale_setpoints = self.view.scale_setpoints_check_box.isChecked()
if (self.subgait.has_setpoints_after_duration(duration)
and not rescale_setpoints):
if not self.subgait.has_multiple_setpoints_before_duration(
duration):
self.view.message(
title="Could not update gait duration",
msg=
"Not all joints have multiple setpoints before duration " +
str(duration),
)
self.view.set_duration_spinbox(self.subgait.duration)
return
discard_setpoints = self.view.yes_no_question(
title="Gait duration lower than highest time setpoint",
msg="Do you want to discard any setpoints higher than the "
"given duration?",
)
if not discard_setpoints:
self.view.set_duration_spinbox(self.subgait.duration)
return
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.save_changed_settings({"joints": self.subgait.joints})
self.subgait.scale_timestamps_subgait(duration, rescale_setpoints)
self.view.time_slider.setRange(0, 100 * self.subgait.duration)
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.update_joint_widgets(self.subgait.joints)
if was_playing:
self.start_time_slider_thread()
def import_gait(self):
file_name, f = self.view.open_file_dialogue(self.current_gait_path)
if file_name != "":
gait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
gait = None
if gait is None:
rospy.logwarn("Could not load gait %s", file_name)
return
if gait.gait_type is None or gait.gait_type == "":
gait.gait_type = "walk_like"
self.subgait = gait
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
self.current_time = 0
if was_playing:
self.start_time_slider_thread()
# The gait directory of the selected gait is always 3 directories
# behind the .subgait file
# (gait_directory/gait/subgait/version.subgait).
self.current_gait_path = os.path.dirname(os.path.dirname(file_name))
self.gait_directory = os.path.dirname(self.current_gait_path)
rospy.loginfo("Setting gait directory to %s", str(self.gait_directory))
# Display only the actual directory under
# which gaits will be saved for readability
gait_directory_text = "gait directory: " + os.path.basename(
self.gait_directory)
self.view.change_gait_directory_button.setText(gait_directory_text)
# Clear undo and redo history
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
def import_side_subgait(self, side="previous"):
file_name, f = self.view.open_file_dialogue()
if file_name != "":
subgait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
subgait = None
if subgait is None:
rospy.logwarn("Could not load gait %s", file_name)
return
if side == "previous":
self.previous_subgait = subgait
elif side == "next":
self.next_subgait = subgait
def export_gait(self):
if self.view.mirror_check_box.isChecked():
key_1 = self.view.mirror_key1_line_edit.text()
key_2 = self.view.mirror_key2_line_edit.text()
mirror = self.subgait.get_mirror(key_1, key_2)
if mirror:
self.export_to_file(mirror, self.get_gait_directory())
else:
self.view.notify("Could not mirror gait",
"Check the logs for more information.")
return
self.export_to_file(self.subgait, self.get_gait_directory())
def export_to_file(self, subgait, gait_directory):
if gait_directory is None or gait_directory == "":
return
output_file_directory = os.path.join(
gait_directory,
subgait.gait_name.replace(" ", "_"),
subgait.subgait_name.replace(" ", "_"),
)
output_file_path = os.path.join(
output_file_directory,
subgait.version.replace(" ", "_") + ".subgait")
file_exists = os.path.isfile(output_file_path)
if file_exists:
overwrite_file = self.view.yes_no_question(
title="File already exists",
msg="Do you want to overwrite " + str(output_file_path) + "?",
)
if not overwrite_file:
return
rospy.loginfo("Writing gait to " + output_file_path)
if not os.path.isdir(output_file_directory):
os.makedirs(output_file_directory)
with open(output_file_path, "w") as output_file:
subgait_dict = subgait.to_dict()
# remove the version field when writing because it is
# superfluous considering the file name
if "version" in subgait_dict:
del subgait_dict["version"]
output_file.write(yaml.dump(subgait_dict))
self.view.notify("Gait Saved", output_file_path)
# Called by export_gait
def get_gait_directory(self):
if self.gait_directory is None:
self.change_gait_directory()
rospy.loginfo("Selected output directory " + str(self.gait_directory))
return self.gait_directory
def change_gait_directory(self):
previous_gait_directory = self.gait_directory
self.gait_directory = str(self.view.open_directory_dialogue())
# If directory dialogue is canceled, or an invalid directory is selected, leave gait_directory unchanged
if self.gait_directory == "" or self.gait_directory is None:
self.gait_directory = previous_gait_directory
# Valid gait directories are limited to direct subdirectories of march_gait_files (here march_gait_files must
# proceed the final directory in the path) and directories named 'resources' for testing (here resources must be
# the final element of the gait directory path). In any other case the directory is invalid. If the path
# contains but 1 element it must be invalid.
elif (os.path.basename(os.path.dirname(
self.gait_directory)) != "march_gait_files"
and os.path.basename(self.gait_directory) != "resources"):
rospy.logwarn(
"Gait directory path invalid. The gait directory must be a direct subdirectory of"
" march_gait_files or be named resources. Change gait directory cancelled"
)
self.gait_directory = previous_gait_directory
if self.gait_directory is not None:
rospy.loginfo("Setting gait directory to %s",
str(self.gait_directory))
# Display only the actual directory under which gaits will be saved for readability
gait_directory_text = "gait directory: " + os.path.basename(
self.gait_directory)
self.view.change_gait_directory_button.setText(gait_directory_text)
def add_inverse_kinematics_setpoints(self) -> None:
"""Add setpoints to the gait based on a user specified desired foot location."""
self.view.get_inverse_kinematics_setpoints_input()
# Check whether the input is valid
if self.view.inverse_kinematics_pop_up.cancelled:
rospy.loginfo(
"The inputs for the inverse kinematics were cancelled.")
return
if not 0 <= self.view.inverse_kinematics_pop_up.time <= self.subgait.duration:
warning_message = (
"The inverse kinematics setpoints feature has failed."
f"The specified time is invalid. "
f"Should be between 0 and the subgait duration {self.subgait.duration}. "
f"{self.view.inverse_kinematics_pop_up.time} was given.")
rospy.loginfo(warning_message)
self.view.message(
"The inverse kinematics setpoints feature has failed.",
warning_message)
return
# Transform the input from relative to the default position to relative to the exoskeleton
self.transform_inverse_kinematics_setpoints_inputs()
# Calculate the setpoints from the desired foot coordinates and add them to the gait
try:
setpoint_dictionary = self.get_setpoints_from_inverse_kinematics_input(
)
self.add_setpoints_from_dictionary(setpoint_dictionary)
except SubgaitInterpolationError:
traceback.print_exc()
self.view.message(
"The inverse kinematics setpoints feature as failed.",
"A subgait interpolation error occured, see the terminal for more information.",
)
except ValueError:
traceback.print_exc()
self.view.message(
"The inverse kinematics setpoints feature has failed.",
"A ValueError occured, see the terminal for more information",
)
def transform_inverse_kinematics_setpoints_inputs(self) -> None:
"""Transform the input coordinates (relative to a default foot location) to coordinates relative to the exo."""
foot_side = self.view.inverse_kinematics_pop_up.foot_side
[
upper_leg_length,
lower_leg_length,
haa_to_leg_length,
haa_arm,
base,
] = get_lengths_robot_for_inverse_kinematics(foot_side)
self.transform_inverse_kinematics_setpoints_x_coordinate(
haa_to_leg_length)
self.transform_inverse_kinematics_setpoints_y_coordinate(
haa_arm, base, foot_side)
self.transform_inverse_kinematics_setpoints_z_coordinate(
upper_leg_length, lower_leg_length)
def transform_inverse_kinematics_setpoints_x_coordinate(
self, haa_to_leg_length: float) -> None:
"""Add the default x coordinate to the desired x coordinate to transform to exoskeleton coordinate system."""
default_x_position = haa_to_leg_length
self.view.inverse_kinematics_pop_up.position_input.x += default_x_position
def transform_inverse_kinematics_setpoints_y_coordinate(
self, haa_arm: float, base: float, foot_side: float) -> None:
"""Add the default y coordinate to the desired y coordinate to transform to exoskeleton coordinate system."""
hip_to_foot_length_cm = base / 2 + haa_arm
if foot_side == Side.right:
default_y_position = hip_to_foot_length_cm
else:
default_y_position = -hip_to_foot_length_cm
self.view.inverse_kinematics_pop_up.position_input.y += default_y_position
def transform_inverse_kinematics_setpoints_z_coordinate(
self, upper_leg_length: float, lower_leg_length: float) -> None:
"""Transform the z coordinate of the input to the coordinate system of the exoskeleton."""
if self.view.inverse_kinematics_pop_up.z_axis == "From ground upwards":
ground_z_coordinate_cm = upper_leg_length + lower_leg_length
self.view.inverse_kinematics_pop_up.position_input.z = (
ground_z_coordinate_cm -
self.view.inverse_kinematics_pop_up.position_input.z)
def get_setpoints_from_inverse_kinematics_input(self) -> None:
"""Use the inverse kinematics function to translate the desired foot coordinates to setpoints."""
desired_foot_state = Foot(
self.view.inverse_kinematics_pop_up.foot_side,
self.view.inverse_kinematics_pop_up.position_input,
self.view.inverse_kinematics_pop_up.velocity_input,
)
return Foot.get_joint_states_from_foot_state(
desired_foot_state, self.view.inverse_kinematics_pop_up.time)
def add_setpoints_from_dictionary(
self, setpoint_dictionary: Dict[str, any]) -> None:
"""Add setpoints from a dictionary with joints as keys to the current subgait."""
for joint_name in setpoint_dictionary:
time = setpoint_dictionary[joint_name].time
position = setpoint_dictionary[joint_name].position
velocity = setpoint_dictionary[joint_name].velocity
joint = self.subgait.get_joint(joint_name)
# If the current joint already has a setpoint at the specified time, replace it
# Otherwise, add the setpoint to the joint.
setpoint_is_replaced = False
for index, setpoint in enumerate(joint.setpoints):
if setpoint.time == time:
joint.replace_setpoint(index,
setpoint_dictionary[joint_name])
setpoint_is_replaced = True
if not setpoint_is_replaced:
joint.add_setpoint(ModifiableSetpoint(time, position,
velocity))
self.view.update_joint_widget(joint)
self.view.publish_preview(self.subgait, self.current_time)
def invert_gait(self) -> None:
for side, controller in self.side_subgait_controller.items():
controller.lock_checked = False
self.handle_sidepoint_lock(side)
for joint in self.subgait.joints:
joint.invert()
self.view.update_joint_widget(joint)
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits":
self.side_subgait_controller.values(),
})
self.view.publish_preview(self.subgait, self.current_time)
def undo(self):
if not self.settings_changed_history:
return
changed_dict = self.settings_changed_history.pop()
if "joints" in changed_dict:
joints = changed_dict["joints"]
for joint in joints:
joint.undo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if "side_subgaits" in changed_dict:
side_subgait_controllers = changed_dict["side_subgaits"]
for controller in side_subgait_controllers:
controller.undo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_redo_list.append(changed_dict)
def redo(self):
if not self.settings_changed_redo_list:
return
changed_dict = self.settings_changed_redo_list.pop()
if "joints" in changed_dict:
joints = changed_dict["joints"]
for joint in joints:
joint.redo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if "side_subgaits" in changed_dict:
side_subgait_controllers = changed_dict["side_subgaits"]
for controller in side_subgait_controllers:
controller.redo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_history.append(changed_dict)
# Needed for undo and redo.
def save_changed_settings(self, settings):
self.settings_changed_history.append(settings)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
# Functions related to previous/next subgait
def toggle_side_subgait_checkbox(self, value, side, box_type):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller[side]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
if box_type == "lock":
self.side_subgait_controller[side].lock_checked = value
elif box_type == "standing":
self.side_subgait_controller[side].default_checked = value
self.handle_sidepoint_lock(side)
def handle_sidepoint_lock(self, side):
if self.side_subgait_controller[side].lock_checked:
if self.side_subgait_controller[side].subgait is not None:
for joint in self.subgait.joints:
side_subgait_joint = self.side_subgait_controller[
side].subgait.get_joint(joint.name)
if side == "previous":
joint.start_point = side_subgait_joint.setpoints[-1]
elif side == "next":
joint.end_point = side_subgait_joint.setpoints[0]
else:
for joint in self.subgait.joints:
if side == "previous":
joint.start_point = joint.setpoints[0]
elif side == "next":
joint.end_point = joint.setpoints[-1]
else:
for joint in self.subgait.joints:
if side == "previous":
joint.start_point = None
elif side == "next":
joint.end_point = None
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
@property
def previous_subgait(self):
return self.side_subgait_controller["previous"].subgait
@previous_subgait.setter
def previous_subgait(self, new_subgait):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller["previous"]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller["previous"].subgait = new_subgait
self.handle_sidepoint_lock("previous")
@property
def next_subgait(self):
return self.side_subgait_controller["next"].subgait
@next_subgait.setter
def next_subgait(self, new_subgait):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller["next"]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller["next"].subgait = new_subgait
self.handle_sidepoint_lock("next")
image = child_pipe.recv()
child_pipe.send("STOP")
clean_pipe(child_pipe)
plt.imshow(image[:, :, 8],
'coolwarm') # show the frame to choose the pixel
print("Select ROI (signature 2)")
coords_s2 = plt.ginput() # select coordinates
plt.close()
# Save signatures
child_pipe.send("START")
frame_counter = 0
while frame_counter < buffer_length:
frame = child_pipe.recv()
array.append(frame[int(coords_s1[0][1]), int(coords_s1[0][0]), :])
frame_counter += 1
print("Bit stream captured")
child_pipe.send("STOP")
clean_pipe(child_pipe)
np.save("captures/signature1.npy", array)
child_pipe.send("START")
frame_counter = 0
while frame_counter < buffer_length:
frame = child_pipe.recv()
array.append(frame[int(coords_s2[0][1]), int(coords_s2[0][0]), :])
frame_counter += 1
print("Bit stream captured")
child_pipe.send("STOP")
clean_pipe(child_pipe)
