def draw_fn(state):
self.visualizer.begin_draw_cycle(
'debug_grasp')
self.visualizer.draw_poses(
'debug_grasp', np.eye(4), 1.0, 0.01, [
gm.subs(eef_pose, state),
gm.subs(self._grasp_poses[p],
state)
])
self.visualizer.render('debug_grasp')
def debug_draw(vis, state, cmd):
vis.begin_draw_cycle('debug_vecs')
s_object_cp = gm.subs(object_cp, state)
s_neutral_tangent = gm.subs(neutral_tangent, state)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(contact_grad, state),
r=0,
b=0)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(active_tangent, state),
r=0,
b=1)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(neutral_tangent, state),
r=1,
g=1,
b=0)
if target_pos is not None:
vis.draw_sphere('debug_vecs',
gm.subs(target_pos, state),
0.01,
r=0,
b=1)
# print(f'{gm.norm(gm.subs(contact_normal, state))}')
# vis.draw_vector('debug_vecs', s_object_cp, s_ortho_vel_vec, r=1, b=0)
vis.render('debug_vecs')
def stop_motion(self, subs):
r, p, y = rpy_from_matrix(gm.subs(self.fk_frame, subs))
subs[self.input_rot_offset.rr] = r
subs[self.input_rot_offset.rp] = p
subs[self.input_rot_offset.ry] = y
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = 0
self.current_lin_vel = gm.vector3(0,0,0)
self.current_rpy = gm.vector3(0,0,0)
subs[self.input_lin_vel.x] = 0
subs[self.input_lin_vel.y] = 0
subs[self.input_lin_vel.z] = 0
subs[self.input_rot_goal.rr] = self.current_rpy[0]
subs[self.input_rot_goal.rp] = self.current_rpy[1]
subs[self.input_rot_goal.ry] = self.current_rpy[2]
def draw_shelves(shelf, params, world, visualizer, steps=4, spacing=1):
state = params.copy()
visualizer.begin_draw_cycle('world')
panel_cos = gm.dot_product(gm.z_of(shelf.links['panel_top'].pose),
-gm.z_of(shelf.links['panel_bottom'].pose))
for x, p in enumerate(np.linspace(0, 1.84, steps)):
state[shelf.joints['hinge'].position] = p
state[x_loc_sym] = (x - steps / 2) * spacing
cos = gm.subs(panel_cos, state)
print('Angle at {:>8.2f}: {:> 8.2f}'.format(
np.rad2deg(p), np.rad2deg(np.arccos(cos.flatten()[0]))))
world.update_world(state)
visualizer.draw_world('world', world)
visualizer.render('world')
def process_input(self, subs, lx, ly, lz, ax, ay, az, oy, scale=1.0, command_type='global'):
now = rospy.Time.now()
if self.last_command is None:
rotation_matrix = gm.subs(self.fk_frame, subs)
if command_type == 'relative':
iframe = gm.rotation3_rpy(0,0, oy)
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = oy
elif command_type == 'global':
iframe = gm.eye(4)
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = 0
else:
iframe = rotation_matrix
if self.symmetry == 'xz' and rotation_matrix[2,2] < 0:
iframe = gm.rotation3_rpy(math.pi, 0, 0) * iframe
r, p, y = rpy_from_matrix(iframe)
subs[self.input_iframe.rr] = r
subs[self.input_iframe.rp] = p
subs[self.input_iframe.ry] = y
r, p, y = rpy_from_matrix(iframe.T * rotation_matrix)
subs[self.input_rot_offset.rr] = r
subs[self.input_rot_offset.rp] = p
subs[self.input_rot_offset.ry] = y
self.last_command = now
self.command_type = command_type
self.current_rpy = gm.vector3(ax, ay, az)
self.current_lin_vel = gm.vector3(lx, ly, lz) * self.vel_limit * scale
subs[self.input_lin_vel.x] = self.current_lin_vel[0]
subs[self.input_lin_vel.y] = self.current_lin_vel[1]
subs[self.input_lin_vel.z] = self.current_lin_vel[2]
subs[self.input_rot_goal.rr] = self.current_rpy[0]
subs[self.input_rot_goal.rp] = self.current_rpy[1]
subs[self.input_rot_goal.ry] = self.current_rpy[2]
'shoulder_lift_joint': 1.4,
'torso_lift_joint' : 0.2}
start_pose = {gm.Position(Path(f'{robot_path}/{n}')): v for n, v in start_pose.items()}
else:
joint_symbols, \
robot_controlled_symbols, \
start_pose, \
eef, \
blacklist = generic_setup(km, robot_path, rospy.get_param('~eef', 'gripper_link'))
collision_world = km.get_active_geometry(gm.free_symbols(handle.pose).union(gm.free_symbols(eef.pose)))
# Init step
grasp_in_handle = gm.dot(gm.translation3(0.04, 0, 0), gm.rotation3_rpy(math.pi * 0.5, 0, math.pi))
goal_pose = gm.dot(handle.pose, grasp_in_handle)
goal_0_pose = gm.subs(goal_pose, {s: 0 for s in gm.free_symbols(goal_pose)})
start_state = {s: 0 for s in gm.free_symbols(collision_world)}
start_state.update(start_pose)
ik_err, robot_start_state = ik_solve_one_shot(km, eef.pose, start_state, goal_0_pose)
start_state.update({s: 0 for s in gm.free_symbols(handle.pose)})
start_state.update(robot_start_state)
collision_world.update_world(start_state)
vis.begin_draw_cycle('ik_solution')
vis.draw_world('ik_solution', collision_world)
vis.render('ik_solution')
print(f'IK error: {ik_err}')
def behavior_update(self):
state_count = 0
while not rospy.is_shutdown() and not self._kys:
loop_start = rospy.Time.now()
with self._state_lock:
if self._robot_state_update_count <= state_count:
rospy.sleep(0.01)
continue
state_count = self._robot_state_update_count
if self.controller is not None:
now = rospy.Time.now()
with self._state_lock:
deltaT = 0.05 if self._last_controller_update is None else (
now - self._last_controller_update).to_sec()
try:
command = self.controller.get_cmd(self._state,
deltaT=deltaT)
except Exception as e:
print(traceback.format_exc())
rospy.signal_shutdown('die lol')
self.robot_command_processor.send_command(command)
self._last_controller_update = now
# Lets not confuse the tracker
if self._phase != 'opening' and self._last_external_cmd_msg is not None:
# Ensure that velocities are assumed to be 0 when not operating anything
self._last_external_cmd_msg.value = [0] * len(
self._last_external_cmd_msg.value)
self.pub_external_command.publish(self._last_external_cmd_msg)
self._last_external_cmd_msg = None
if self._phase == 'idle':
# if there is no controller, instantiate idle
# check for new target
# -> open gripper
# instantiate 6d controller
# switch to "grasping"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
with self._state_lock:
for s, p in self._target_map.items():
if s in self._state and self._state[
s] < self._var_upper_bound[
s] * self.monitoring_threshold: # Some thing in the scene is closed
self._current_target = p
print(f'New target is {self._current_target}')
self.gripper_wrapper.sync_set_gripper_position(
0.07)
self._last_controller_update = None
print('Gripper is open. Proceeding to grasp...')
draw_fn = None
eef_pose = self.km.get_data(self.eef_path).pose
if self.visualizer is not None:
self.visualizer.begin_draw_cycle('grasp pose')
self.visualizer.draw_poses(
'grasp pose', np.eye(4), 0.2, 0.01, [
gm.subs(self._grasp_poses[p],
self._state)
])
self.visualizer.render('grasp pose')
def draw_fn(state):
self.visualizer.begin_draw_cycle(
'debug_grasp')
self.visualizer.draw_poses(
'debug_grasp', np.eye(4), 1.0, 0.01, [
gm.subs(eef_pose, state),
gm.subs(self._grasp_poses[p],
state)
])
self.visualizer.render('debug_grasp')
# print('\n'.join(f'{s}: {v}' for s, v in state.items()))
self.controller = SixDPoseController(
self.km, eef_pose, self._grasp_poses[p],
self.controlled_symbols, self.weights, draw_fn)
self._phase = 'grasping'
print(f'Now entering {self._phase} state')
break
else:
print(
'None of the monitored symbols are closed:\n {}'.
format('\n '.join(
f'{self._state[s]} < {self._var_upper_bound[s]}'
for s in self._target_map.keys()
if s in self._state)))
elif self._phase == 'grasping':
# check if grasp is acheived
# -> close gripper
# instantiate cascading controller
# switch to "opening"
# if there is no more command but the goal error is too great -> "homing"
if self.controller.equilibrium_reached(0.03, -0.03):
if self.controller.current_error() > 0.01:
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
else:
print('Closing gripper...')
self.gripper_wrapper.sync_set_gripper_position(0, 80)
print('Generating opening controller')
eef = self.km.get_data(self.eef_path)
obj = self.km.get_data(self._current_target)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
static_object_pose = gm.subs(obj.pose, self._state)
offset_pose = np_inverse_frame(static_object_pose).dot(
static_eef_pose)
print(offset_pose)
goal_pose = gm.dot(obj.pose, gm.Matrix(offset_pose))
goal_pos_error = gm.norm(
gm.pos_of(eef.pose) - gm.pos_of(goal_pose))
goal_rot_error = gm.norm(eef.pose[:3, :3] -
goal_pose[:3, :3])
target_symbols = self._target_body_map[
self._current_target]
lead_goal_constraints = {
f'open_{s}': SC(self._var_upper_bound[s] - s, 1000,
1, s)
for s in target_symbols
if s in self._var_upper_bound
}
for n, c in lead_goal_constraints.items():
print(f'{n}: {c}')
follower_goal_constraints = {
'keep position':
SC(-goal_pos_error, -goal_pos_error, 10,
goal_pos_error),
'keep rotation':
SC(-goal_rot_error, -goal_rot_error, 1,
goal_rot_error)
}
blacklist = {
gm.Velocity(Path('pr2/torso_lift_joint'))
}.union({
gm.DiffSymbol(s)
for s in gm.free_symbols(obj.pose)
if 'location' in str(s)
})
# blacklist = {gm.DiffSymbol(s) for s in gm.free_symbols(obj.pose) if 'location' in str(s)}
self.controller = CascadingQP(
self.km,
lead_goal_constraints,
follower_goal_constraints,
f_gen_follower_cvs=gen_dv_cvs,
controls_blacklist=blacklist,
t_follower=GQPB,
visualizer=None # self.visualizer
)
print(self.controller)
# def debug_draw(vis, state, cmd):
# vis.begin_draw_cycle('lol')
# vis.draw_poses('lol', np.eye(4), 0.2, 0.01,
# [gm.subs(goal_pose, state),
# gm.subs(eef.pose, state)])
# vis.render('lol')
# self.controller.follower_qp._cb_draw = debug_draw
# self.gripper_wrapper.sync_set_gripper_position(0.07)
# rospy.signal_shutdown('die lol')
# return
self._last_controller_update = None
self._phase = 'opening'
print(f'Now entering {self._phase} state')
elif self._phase == 'opening':
# Wait for monitored symbols to be in open position
# -> open gripper
# generate 6d retraction goal: -10cm in tool frame
# spawn 6d controller
# switch to "retracting"
# self.visualizer.begin_draw_cycle('world state')
# with self._state_lock:
# self._world.update_world(self._state)
# self.visualizer.draw_world('world state', self._world, b=0)
# self.visualizer.render('world state')
external_command = {
s: v
for s, v in command.items()
if s not in self.controlled_symbols
}
ext_msg = ValueMapMsg()
ext_msg.header.stamp = now
ext_msg.symbol, ext_msg.value = zip(
*[(str(s), v) for s, v in external_command.items()])
self.pub_external_command.publish(ext_msg)
self._last_external_cmd_msg = ext_msg
with self._state_lock:
current_external_error = {
s: self._state[s]
for s in self._target_body_map[self._current_target]
}
print('Current error state:\n {}'.format('\n '.join(
[f'{s}: {v}' for s, v in current_external_error.items()])))
gripper_err = self.gripper_wrapper.get_latest_error()
# if gripper_err < 0.0005: # Grasped object is thinner than 5mm aka we lost it
# self.controller = self._idle_controller
# self._phase = 'homing'
# print(f'Grasped object seems to have slipped ({gripper_err}). Returning to home...')
# return
if min([
v >=
self._var_upper_bound[s] * self.acceptance_threshold
for s, v in current_external_error.items()
]):
print('Target fulfilled. Setting it to None')
self._current_target = None
eef = self.km.get_data(self.eef_path)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
goal_pose = static_eef_pose.dot(
np.array([[1, 0, 0, -0.12], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
self.controller = SixDPoseController(
self.km, eef.pose, goal_pose, self.controlled_symbols,
self.weights)
self.gripper_wrapper.sync_set_gripper_position(0.07)
self._last_controller_update = None
self._phase = 'retracting'
print(f'Now entering {self._phase} state')
else:
remainder = (1 / 3) - (rospy.Time.now() -
loop_start).to_sec()
if remainder > 0:
rospy.sleep(remainder)
elif self._phase == 'retracting':
# Wait for retraction to complete (Currently just skipping)
# -> spawn idle controller
# switch to "homing"
if self.controller.equilibrium_reached(0.035, -0.035):
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
elif self._phase == 'homing':
# Wait for idle controller to have somewhat low error
# -> switch to "idle"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
continue
if self.controller.equilibrium_reached(0.1, -0.1):
self._phase = 'idle'
print(f'Now entering {self._phase} state')
else:
raise Exception(f'Unknown state "{self._phase}')
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)
# x: 0.8333333333333333
# y: -0.1333333333333333
for x_coord in [0.8]: #np.linspace(0.5, 1.0, 4):
for y_coord in [-0.1]: #np.linspace(0.2, -0.8, 4):
print(f'Config is ({x_coord}, {y_coord})')
ik_goal_start = {s: 0 for s in world.free_symbols}
ik_goal_start[door_x] = x_coord
ik_goal_start[door_y] = y_coord
err_ik, q_ik_goal = ik_solve_one_shot(km, eef.pose, ik_goal_start,
goal_pose)
world.update_world(q_ik_goal)
visualizer.begin_draw_cycle('pre_open_world')
visualizer.draw_poses('pre_open_world', gm.eye(4), 0.2, 0.01, [
gm.subs(goal_pose, {s: 0
for s in gm.free_symbols(goal_pose)}),
gm.subs(eef.pose, q_ik_goal)
])
visualizer.draw_world('pre_open_world', world, g=0.6, b=0.6)
visualizer.render('pre_open_world')
# Build Door opening problem
grasp_err_rot = gm.norm(gm.rot_of(goal_pose - eef.pose).elements())
grasp_err_lin = gm.norm(gm.pos_of(goal_pose - eef.pose))
active_symbols = {s for s in gm.free_symbols(eef.pose) if gm.get_symbol_type(s) == gm.TYPE_POSITION}\
.union({door_position, handle_position})
controlled_symbols = {gm.DiffSymbol(s) for s in active_symbols}
controlled_values, constraints = generate_controlled_values(
E = gm.dot(d_in_w, gm.point3(0, 0, l * 0.75))
lock_bound = gm.alg_not(
gm.alg_and(gm.less_than(b, 0.3), gm.less_than(1.99, a)))
# PLOT OF MOVEMENT
As = []
Bs = []
Cs = []
Ds = []
Es = []
for x in np.linspace(0, l, 20):
q = {a: x}
As.append(np.take(gm.subs(A, q).flatten(), (0, 2)))
Bs.append(np.take(gm.subs(B, q).flatten(), (0, 2)))
Cs.append(np.take(gm.subs(C, q).flatten(), (0, 2)))
Ds.append(np.take(gm.subs(D, q).flatten(), (0, 2)))
Es.append(np.take(gm.subs(E, q).flatten(), (0, 2)))
As = np.vstack(As)
Bs = np.vstack(Bs)
Cs = np.vstack(Cs)
Ds = np.vstack(Ds)
Es = np.vstack(Es)
print(Bs)
# exit()
data = [As, Bs, Cs, Ds, Es]
visualizer = ROSBPBVisualizer('/tracker_vis', 'world')
last_n_dof = 0
total_start = Time.now()
for group in groups:
for path, alias in group:
tracker.track(path, alias)
constraints = tracker.km_client.get_constraints_by_symbols(
tracker.joints)
constraints = {
c.expr: [
float(subs(c.lower, {c.expr: 0})),
float(subs(c.upper, {c.expr: 0}))
]
for k, c in constraints.items() if cm.is_symbol(c.expr)
and not is_symbolic(subs(c.lower, {c.expr: 0}))
and not is_symbolic(subs(c.upper, {c.expr: 0}))
}
joint_array = [
s for _, s in sorted([(str(s), s) for s in tracker.joints])
]
if len(joint_array) == last_n_dof:
continue
last_n_dof = len(joint_array)
def lock_explorer(km, state, goals, generated_constraints):
final_goals = goals.copy()
for n, goal in goals.items():
symbols = goal.expr.free_symbols
goal_sign = sign(subs(goal.lower, state)) + sign(
subs(goal.upper, state))
if goal_sign == 0: # Constraint is satisfied
continue
goal_expr = goal.expr
if type(goal_expr) != GC:
goal_expr = GC(goal_expr)
goal_expr.do_full_diff()
diff_value = {
s: subs(g, state)
for s, g in goal_expr.gradients.items()
}
diff_sign = {s: sign(g) * goal_sign for s, g in diff_value.items()}
diff_symbols = set(diff_sign.keys())
diff_constraints = km.get_constraints_by_symbols(diff_symbols)
# Constraints constraining the DoF listed by symbol
blocking_constraints = {s: {} for s in diff_symbols}
# Iterate over constraints which directly constrain a symbol -> type(c.expr) == Symbol
for n, c in {
n: c
for n, c in diff_constraints.items() if c.expr in diff_symbols
}.items():
s = c.expr
c_upper = subs(c.upper, state)
c_lower = subs(c.lower, state)
sign_u = sign(c_upper)
sign_l = sign(c_lower)
# Check if constraint is blocking the DoF from moving in the desired direction
if diff_sign[s] > 0 and sign_u <= 0:
blocking_constraints[s][n] = c
elif diff_sign[s] < 0 and sign_l >= 0:
blocking_constraints[s][n] = c
new_goals = {}
# If all symbols are blocked from going in the desired direction
if min([len(cd) for cd in blocking_constraints.values()]) > 0:
for s, cd in blocking_constraints.items():
for n, c in cd.items():
u_const_name = 'unlock {} upper bound'.format(n)
l_const_name = 'unlock {} lower bound'.format(n)
if diff_sign[s] > 0 and type(
c.upper
) in symbolic_types and u_const_name not in generated_constraints:
new_goals[u_const_name] = SoftConstraint(
less_than(c.upper, 0), 1000, goal.weight, c.upper)
generated_constraints.add(u_const_name)
elif diff_sign[s] < 0 and type(
c.lower
) in symbolic_types and l_const_name not in generated_constraints:
new_goals[l_const_name] = SoftConstraint(
-1000, -greater_than(c.lower, 0), goal.weight,
c.lower)
generated_constraints.add(l_const_name)
final_goals.update(
lock_explorer(km, state, new_goals, generated_constraints))
return final_goals
