def __init__(self,
km,
lead_goal_constraints,
follower_goal_constraints,
t_leader=TQPB,
t_follower=TQPB,
f_gen_lead_cvs=None,
f_gen_follower_cvs=None,
visualizer=None,
controls_blacklist=set(),
transition_overrides=None):
lead_symbols = union(
gm.free_symbols(c.expr) for c in lead_goal_constraints.values())
follower_symbols = union(
gm.free_symbols(c.expr)
for c in follower_goal_constraints.values())
self.lead_symbols = lead_symbols
self.follower_symbols = follower_symbols
self.lead_controlled_symbols = {
s
for s in union(
gm.get_diff_symbols(c.expr)
for c in lead_goal_constraints.values())
if s not in controls_blacklist
}
# Only update the symbols that are unique to the follower
self.follower_controlled_symbols = {
s
for s in union(
gm.get_diff_symbols(c.expr)
for c in follower_goal_constraints.values()) if
s not in controls_blacklist and gm.IntSymbol(s) not in lead_symbols
}
f_gen_lead_cvs = self.gen_controlled_values if f_gen_lead_cvs is None else f_gen_lead_cvs
lead_cvs, \
lead_constraints = f_gen_lead_cvs(km,
km.get_constraints_by_symbols(self.lead_controlled_symbols.union({gm.IntSymbol(s) for s in self.lead_controlled_symbols})),
self.lead_controlled_symbols)
f_gen_follower_cvs = self.gen_controlled_values if f_gen_follower_cvs is None else f_gen_follower_cvs
follower_cvs, \
follower_constraints = f_gen_follower_cvs(km,
km.get_constraints_by_symbols(self.follower_controlled_symbols.union({gm.IntSymbol(s) for s in self.follower_controlled_symbols})),
self.follower_controlled_symbols)
if issubclass(t_leader, GQPB):
lead_world = km.get_active_geometry(lead_symbols)
self.lead_qp = t_leader(lead_world,
lead_constraints,
lead_goal_constraints,
lead_cvs,
visualizer=visualizer)
else:
self.lead_qp = t_leader(lead_constraints, lead_goal_constraints,
lead_cvs)
self.sym_dt = gm.Symbol('dT')
self.lead_o_symbols, \
self.lead_t_function, \
self.lead_o_controls = generate_transition_function(self.sym_dt, lead_symbols, transition_overrides)
self.follower_o_symbols, \
self.follower_t_function, \
self.follower_o_controls = generate_transition_function(self.sym_dt,
{gm.IntSymbol(s) for s in self.follower_controlled_symbols},
transition_overrides)
self.follower_o_bounds = list(self.follower_controlled_symbols)
follower_ctrl_bounds = [
sum([[c.lower, c.upper]
for c in km.get_constraints_by_symbols({s}).values()], [])
for s in self.follower_o_bounds
]
max_bounds = max(len(row) for row in follower_ctrl_bounds)
for s, row in zip(self.follower_o_bounds, follower_ctrl_bounds):
row.extend([1e3] * (max_bounds - len(row)))
print(f'{s}: {row}')
follower_ctrl_bounds = gm.Matrix(follower_ctrl_bounds).T
self.follower_ctrl_bounds_params = list(
gm.free_symbols(follower_ctrl_bounds))
self.follower_ctrl_bounds_f = gm.speed_up(
follower_ctrl_bounds, self.follower_ctrl_bounds_params)
self.follower_delta_map = {
gm.IntSymbol(s): s
for s in self.follower_controlled_symbols
}
if issubclass(t_follower, GQPB):
follower_world = km.get_active_geometry(follower_symbols)
self.follower_qp = t_follower(follower_world,
follower_constraints,
follower_goal_constraints,
follower_cvs,
visualizer=visualizer)
else:
self.follower_qp = t_follower(follower_constraints,
follower_goal_constraints,
follower_cvs)
def __init__(self,
observations,
constraints,
Q=None,
transition_rules=None,
trim_threshold=None):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
constraints (dict): A dict of named constraints that govern the
configuration space of the estimated quantities
Q (matrix, optional): Process noise of the estimated quantities.
Note: Quantities are expected to be ordered alphabetically
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.ordered_vars = [
s for _, s in sorted((str(s), s) for s in state_vars)
]
self.Q = Q if Q is not None else np.zeros(
(len(self.ordered_vars), len(self.ordered_vars)))
st_fn = {}
for s in self.ordered_vars:
st_fn[s] = gm.wrap_expr(s + gm.DiffSymbol(s) * EKFModel.DT_SYM)
if transition_rules is not None:
varset = set(self.ordered_vars).union(
{gm.DiffSymbol(s)
for s in self.ordered_vars}).union({EKFModel.DT_SYM})
for s, r in transition_rules.items():
if s in st_fn:
if len(gm.free_symbols(r).difference(varset)) == 0:
st_fn[s] = gm.wrap_expr(r)
else:
print(
f'Dropping rule "{s}: {r}". Symbols missing from state: {gm.free_symbols(r).difference(varset)}'
)
control_vars = union([gm.free_symbols(r) for r in st_fn.values()]) \
.difference(self.ordered_vars) \
.difference({EKFModel.DT_SYM})
self.ordered_controls = [
s for _, s in sorted((str(s), s) for s in control_vars)
]
# State as column vector n * 1
temp_g_fn = gm.Matrix(
[gm.extract_expr(st_fn[s]) for s in self.ordered_vars])
self.g_fn = gm.speed_up(temp_g_fn, [EKFModel.DT_SYM] +
self.ordered_vars + self.ordered_controls)
temp_g_prime_fn = gm.Matrix([[
gm.extract_expr(st_fn[s][d]) if d in st_fn[s] else 0
for d in self.ordered_controls
] for s in self.ordered_vars])
self.g_prime_fn = gm.speed_up(temp_g_prime_fn,
[EKFModel.DT_SYM] + self.ordered_vars +
self.ordered_controls)
self.obs_labels = []
self.takers = []
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(
sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(
sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []),
o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
self.obs_labels.append('{}_{}_{}'.format(
o_label, *coords))
flat_obs.append(gm.wrap_expr(c))
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers.append((o_label, indices))
else:
self.obs_labels.append(o_label)
flat_obs.append(gm.wrap_expr(o))
self.takers.append((o_label, [0]))
temp_h_fn = gm.Matrix([gm.extract_expr(o) for o in flat_obs])
self.h_fn = gm.speed_up(temp_h_fn, self.ordered_vars)
temp_h_prime_fn = gm.Matrix([[
gm.extract_expr(o[d]) if d in o else 0
for d in self.ordered_controls
] for o in flat_obs])
self.h_prime_fn = gm.speed_up(temp_h_prime_fn, self.ordered_vars)
state_constraints = {}
for n, c in constraints.items():
if gm.is_symbol(c.expr):
s = gm.free_symbols(c.expr).pop()
fs = gm.free_symbols(c.lower).union(gm.free_symbols(c.upper))
if len(fs.difference({s})) == 0:
state_constraints[s] = (float(gm.subs(c.lower, {s: 0})),
float(gm.subs(c.upper, {s: 0})))
self.state_bounds = np.array([
state_constraints[s]
if s in state_constraints else [-np.pi, np.pi]
for s in self.ordered_vars
])
self.R = None # np.zeros((len(self.obs_labels), len(self.obs_labels)))
self.trim_threshold = trim_threshold
def main(create_figure=False,
vis_mode=False,
log_csv=True,
min_n_dof=1,
samples=300,
n_observations=25,
noise_lin=0.2,
noise_ang=30,
noise_steps=5):
wait_duration = rospy.Duration(0.1)
vis = ROSBPBVisualizer('ekf_vis', 'world') if vis_mode != 'none' else None
km = GeometryModel()
with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
'r') as urdf_file:
urdf_kitchen_str = urdf_file.read()
kitchen_model = urdf_filler(
URDF.from_xml_string(hacky_urdf_parser_fix(urdf_kitchen_str)))
load_urdf(km, Path('kitchen'), kitchen_model)
km.clean_structure()
km.dispatch_events()
kitchen = km.get_data('kitchen')
tracking_pools = []
for name, link in kitchen.links.items():
symbols = gm.free_symbols(link.pose)
if len(symbols) == 0:
continue
for x in range(len(tracking_pools)):
syms, l = tracking_pools[x]
if len(symbols.intersection(syms)
) != 0: # BAD ALGORITHM, DOES NOT CORRECTLY IDENTIFY SETS
tracking_pools[x] = (syms.union(symbols),
l + [(name, link.pose)])
break
else:
tracking_pools.append((symbols, [(name, link.pose)]))
# tracking_pools = [tracking_pools[7]]
# print('Identified {} tracking pools:\n{}'.format(len(tracking_pools), tracking_pools))
all_ekfs = [
EKFModel(dict(poses), km.get_constraints_by_symbols(symbols))
for symbols, poses in tracking_pools
] # np.eye(len(symbols)) * 0.001
print('Created {} EKF models'.format(len(all_ekfs)))
print('\n'.join(str(e) for e in all_ekfs))
# Sanity constraint
min_n_dof = min(min_n_dof, len(all_ekfs))
iteration_times = []
for u in range(min_n_dof, len(all_ekfs) + 1):
if rospy.is_shutdown():
break
ekfs = all_ekfs[:u]
observed_poses = {}
for ekf in ekfs:
for link_name, _ in ekf.takers:
observed_poses[link_name] = kitchen.links[link_name].pose
names, poses = zip(*sorted(observed_poses.items()))
state_symbols = union([gm.free_symbols(p) for p in poses])
ordered_state_vars = [
s for _, s in sorted((str(s), s) for s in state_symbols)
]
state_constraints = {}
for n, c in km.get_constraints_by_symbols(state_symbols).items():
if gm.is_symbol(c.expr):
s = gm.free_symbols(c.expr).pop()
fs = gm.free_symbols(c.lower).union(gm.free_symbols(c.upper))
if len(fs.difference({s})) == 0:
state_constraints[s] = (float(gm.subs(c.lower, {s: 0})),
float(gm.subs(c.upper, {s: 0})))
state_bounds = np.array([
state_constraints[s]
if s in state_constraints else [-np.pi * 0.5, np.pi * 0.5]
for s in ordered_state_vars
])
state_fn = gm.speed_up(gm.vstack(*poses), ordered_state_vars)
subworld = km.get_active_geometry(state_symbols)
# Generate observation noise
print('Generating R matrices...')
n_cov_obs = 400
full_log = []
dof_iters = []
# EXPERIMENT
for lin_std, ang_std in [(noise_lin, noise_ang * (np.pi / 180.0))]:
# zip(np.linspace(0, noise_lin, noise_steps),
# np.linspace(0, noise_ang * (np.pi / 180.0), noise_steps)):
if rospy.is_shutdown():
break
# INITIALIZE SENSOR MODEL
training_obs = []
state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
observations = state_fn.call2(state)
for _ in range(n_cov_obs):
noisy_obs = {}
for x, noise in enumerate([
t.dot(r) for t, r in zip(
random_normal_translation(len(poses), 0, lin_std),
random_rot_normal(len(poses), 0, ang_std))
]):
noisy_obs[names[x]] = observations[x * 4:x * 4 +
4, :4].dot(noise)
training_obs.append(noisy_obs)
for ekf in ekfs:
ekf.generate_R(training_obs)
# ekf.set_R(np.eye(len(ekf.ordered_vars)) * 0.1)
# Generate figure
gridsize = (4, samples)
plot_size = (4, 4)
fig = plt.figure(figsize=(gridsize[1] * plot_size[0], gridsize[0] *
plot_size[1])) if create_figure else None
gt_states = []
states = [[] for x in range(samples)]
variances = [[] for x in range(samples)]
e_obs = [[] for x in range(samples)]
print('Starting iterations')
for k in tqdm(range(samples)):
if rospy.is_shutdown():
break
state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
gt_states.append(state)
observations = state_fn.call2(state).copy()
gt_obs_d = {
n: observations[x * 4:x * 4 + 4, :4]
for x, n in enumerate(names)
}
subworld.update_world(dict(zip(ordered_state_vars, state)))
if vis_mode == 'iter' or vis_mode == 'io':
vis.begin_draw_cycle('gt', 'noise', 'estimate', 't_n',
't0')
vis.draw_world('gt', subworld, g=0, b=0)
vis.render('gt')
estimates = []
for ekf in ekfs:
particle = ekf.spawn_particle()
estimates.append(particle)
initial_state = dict(
sum([[(s, v) for s, v in zip(ekf.ordered_vars, e.state)]
for e, ekf in zip(estimates, ekfs)], []))
initial_state = np.array(
[initial_state[s] for s in ordered_state_vars])
if initial_state.min() < state_bounds.T[0].min(
) or initial_state.max() > state_bounds.T[1].max():
raise Exception(
'Estimate initialization is out of bounds: {}'.format(
np.vstack([initial_state, state_bounds.T]).T))
initial_delta = state - initial_state
for y in range(n_observations):
# Add noise to observations
noisy_obs = {}
for x, noise in enumerate([
t.dot(r) for t, r in zip(
random_normal_translation(
len(poses), 0, lin_std),
random_rot_normal(len(poses), 0, ang_std))
]):
noisy_obs[names[x]] = observations[x * 4:x * 4 +
4, :4].dot(noise)
if vis_mode in {'iter', 'iter-trail'} or (vis_mode == 'io'
and y == 0):
for n, t in noisy_obs.items():
subworld.named_objects[Path(
('kitchen', 'links', n))].np_transform = t
if vis_mode != 'iter-trail':
vis.begin_draw_cycle('noise')
vis.draw_world('noise', subworld, r=0, g=0, a=0.1)
vis.render('noise')
start_time = time()
for estimate, ekf in zip(estimates, ekfs):
if y > 0:
control = np.zeros(len(ekf.ordered_controls))
estimate.state, estimate.cov = ekf.predict(
estimate.state.flatten(), estimate.cov,
control)
obs_vector = ekf.gen_obs_vector(noisy_obs)
estimate.state, estimate.cov = ekf.update(
estimate.state, estimate.cov,
ekf.gen_obs_vector(noisy_obs))
if vis_mode in {'iter', 'iter-trail'}:
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
else:
obs_vector = ekf.gen_obs_vector(noisy_obs)
for _ in range(1):
h_prime = ekf.h_prime_fn.call2(estimate.state)
obs_delta = obs_vector.reshape(
(len(obs_vector), 1)) - ekf.h_fn.call2(
estimate.state)
estimate.state += (h_prime.T.dot(obs_delta) *
0.1).reshape(
estimate.state.shape)
if vis_mode in {'iter', 'io'}:
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
if vis_mode != 'none' and y == 0:
vis.draw_world('t0', subworld, b=0, a=1)
vis.render('t0')
elif vis_mode in {'iter', 'iter-trail'}:
if vis_mode != 'iter-trail':
vis.begin_draw_cycle('t_n')
vis.draw_world('t_n', subworld, b=0, a=1)
vis.render('t_n')
if log_csv or fig is not None:
e_state_d = dict(
sum([[(s, v)
for s, v in zip(ekf.ordered_vars, e.state)]
for e, ekf in zip(estimates, ekfs)], []))
covs = dict(
sum([[(s, v) for s, v in zip(
ekf.ordered_vars, np.sqrt(np.trace(e.cov)))]
for e, ekf in zip(estimates, ekfs)], []))
e_state = np.hstack([
e_state_d[s] for s in ordered_state_vars
]).reshape((len(e_state_d), ))
if log_csv:
full_log.append(
np.hstack(
([lin_std,
ang_std], state, e_state.flatten(),
np.array([
covs[s] for s in ordered_state_vars
]))))
if fig is not None:
e_obs[k].append(
np.array([
np.abs(
ekf.gen_obs_vector(gt_obs_d) -
ekf.h_fn.call2(e.state)).max()
for e, ekf in zip(estimates, ekfs)
]))
states[k].append(e_state)
variances[k].append(
np.array([covs[s]
for s in ordered_state_vars]))
else:
if vis_mode == 'io':
for estimate, ekf in zip(estimates, ekfs):
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
vis.draw_world('t_n', subworld, r=0, b=0, a=1)
vis.render('t_n')
dof_iters.append(time() - start_time)
if fig is not None:
axes = [
plt.subplot2grid(gridsize, (y, 0), colspan=1, rowspan=1)
for y in range(gridsize[0])
]
axes = np.array(
sum([[
plt.subplot2grid(gridsize, (y, x),
colspan=1,
rowspan=1,
sharey=axes[y])
for y in range(gridsize[0])
] for x in range(1, gridsize[1])], axes)).reshape(
(gridsize[1], gridsize[0]))
for x, (gt_s, state, variance, obs_delta,
(ax_s, ax_d, ax_o, ax_v)) in enumerate(
zip(gt_states, states, variances, e_obs, axes)):
for y in gt_s:
ax_s.axhline(y, xmin=0.97, xmax=1.02)
ax_s.set_title('State; Sample: {}'.format(x))
ax_d.set_title('Delta from GT; Sample: {}'.format(x))
ax_o.set_title('Max Delta in Obs; Sample: {}'.format(x))
ax_v.set_title('Standard Deviation; Sample: {}'.format(x))
ax_s.plot(state)
ax_d.plot(gt_s - np.vstack(state))
ax_o.plot(obs_delta)
ax_v.plot(variance)
ax_s.grid(True)
ax_d.grid(True)
ax_o.grid(True)
ax_v.grid(True)
fig.tight_layout()
plt.savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker_{}_{}.png'
.format(lin_std, ang_std)))
iteration_times.append(dof_iters)
if log_csv:
df = pd.DataFrame(
columns=['lin_std', 'ang_std'] +
['gt_{}'.format(x) for x in range(len(state_symbols))] +
['ft_{}'.format(x) for x in range(len(state_symbols))] +
['var_{}'.format(x) for x in range(len(state_symbols))],
data=full_log)
df.to_csv(res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker.csv'
),
index=False)
df = pd.DataFrame(
columns=[str(x) for x in range(1,
len(iteration_times) + 1)],
data=np.vstack(iteration_times).T)
df.to_csv(res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker_performance.csv'
),
index=False)
def get_controls(self):
return union([e.command_vars for e in self.estimators])
def __init__(self, km, observations, transition_rules=None, max_iterations=20, num_samples=7):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
km (ArticulationModel): Articulation model to query for constraints
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.num_samples = num_samples
self.ordered_vars, \
self.transition_fn, \
self.transition_args = generate_transition_function(QPStateModel.DT_SYM,
state_vars,
transition_rules)
self.command_vars = {s for s in self.transition_args
if s not in state_vars and str(s) != str(QPStateModel.DT_SYM)}
obs_constraints = {}
obs_switch_vars = {}
# State as column vector n * 1
self.switch_vars = {}
self._obs_state = {}
self.obs_vars = {}
self.takers = {}
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
obs_switch_var = gm.Symbol(f'{o_label}_observed')
self.switch_vars[o_label] = obs_switch_var
if o_label not in obs_constraints:
obs_constraints[o_label] = {}
if o_label not in self.obs_vars:
self.obs_vars[o_label] = []
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
obs_symbol = gm.Symbol('{}_{}_{}'.format(o_label, *coords))
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - (1 - obs_switch_var) * 1e3,
-obs_error + (1 - obs_switch_var) * 1e3, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers[o_label] = indices
else:
obs_symbol = gm.Symbol(f'{o_label}_value')
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - obs_switch_var * 1e9,
-obs_error + obs_switch_var * 1e9, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
self.takers[o_label] = [0]
state_constraints = km.get_constraints_by_symbols(state_vars)
cvs, hard_constraints = generate_controlled_values(state_constraints,
{gm.DiffSymbol(s) for s in state_vars
if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN})
flat_obs_constraints = dict(sum([list(oc.items()) for oc in obs_constraints.values()], []))
self.qp = TQPB(hard_constraints, flat_obs_constraints, cvs)
st_bound_vars, st_bounds, st_unbounded = static_var_bounds(km, state_vars)
self._state = {s: 0 for s in st_unbounded} # np.random.uniform(-1.0, 1.0) for s in st_unbounded}
for vb, (lb, ub) in zip(st_bound_vars, st_bounds):
self._state[vb] = np.random.uniform(lb, ub)
self._state_buffer = []
self._state.update({s: 0 for s in self.transition_args})
self._obs_state = {s: 0 for s in sum(self.obs_vars.values(), [])}
self._obs_count = 0
self._stamp_last_integration = None
self._max_iterations = 10
self._current_error = 1e9