def __init__(self, km,
actuated_pose,
goal_pose,
controlled_symbols,
weight_override=None,
draw_fn=None):
eef = actuated_pose
goal_pos_error = gm.norm(gm.pos_of(eef) - gm.pos_of(goal_pose))
self.eef_rot = eef[:3, :3]
self.goal_rot = goal_pose[:3, :3]
self.rot_error_matrix = self.eef_rot - self.goal_rot
goal_rot_error = gm.norm(self.rot_error_matrix)
rot_align_scale = gm.exp(-3 * goal_rot_error)
goal_constraints = {'align position': SC(-goal_pos_error * rot_align_scale,
-goal_pos_error * rot_align_scale, 10, goal_pos_error),
'align rotation': SC(-goal_rot_error, -goal_rot_error, 1, goal_rot_error)}
self.goal_rot_error = goal_rot_error
constraints = km.get_constraints_by_symbols(gm.free_symbols(eef).union(controlled_symbols))
cvs, constraints = generate_controlled_values(constraints, controlled_symbols, weights=weight_override)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=1.02)
self.draw_fn = draw_fn
self.qp = TQPB(constraints,
goal_constraints,
cvs)
def _build_ext_symbol_map(self, km, ext_paths):
# total_ext_symbols = set()
# Map of path to set of symbols
self._target_body_map = {}
self._grasp_poses = {}
self._var_upper_bound = {}
for path, grasp_pose in ext_paths.items():
data = km.get_data(path)
if not isinstance(data, Frame):
print(f'Given external path "{path}" is not a frame.')
continue
self._grasp_poses[path] = gm.dot(data.pose, grasp_pose)
symbols = {
s
for s in gm.free_symbols(data.pose) if 'location' not in str(s)
}
static_bounded_vars, \
static_bounds, \
_ = static_var_bounds(km, symbols)
print(static_bounds.shape)
if len(static_bounds) == 0:
constraints = km.get_constraints_by_symbols(symbols)
raise Exception(
'None of {} seem to have static bounds. Constraints are:\n '
.format(
', '.join(str(s) for s in symbols),
'\n '.join(f'{n} : {c}'
for n, c in constraints.items())))
self._var_upper_bound.update(
zip(static_bounded_vars, static_bounds[:, 1]))
# total_ext_symbols.update(gm.free_symbols(data.pose))
self._target_body_map[path] = set(static_bounded_vars)
list_sets = list(self._target_body_map.values())
for x, s in enumerate(list_sets):
# Only keep symbols unique to one target
s.difference_update(list_sets[:x] + list_sets[x + 1:])
for p, symbols in self._target_body_map.items():
for s in symbols:
self._target_map[s] = p
print('Monitored symbols are:\n {}'.format('\n '.join(
str(s) for s in self._target_map.keys())))
def __init__(self, km, controlled_symbols, resting_pose, camera_path=None):
tucking_constraints = {}
if resting_pose is not None:
tucking_constraints = {
f'tuck {s}': SC(p - s, p - s, 1, s)
for s, p in resting_pose.items()
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
# tucking_constraints.update(self.taxi_constraints)
self.use_camera = camera_path is not None
if camera_path is not None:
self._poi_pos = gm.Symbol('poi')
poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3(
0, self._poi_pos * 2.0, 0)
camera = km.get_data(camera_path)
cam_to_poi = poi - gm.pos_of(camera.pose)
lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose),
cam_to_poi) / gm.norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= gm.free_symbols(c.expr)
joint_symbols = {
s
for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN
}
controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols}
hard_constraints = km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
km, controlled_values)
self.qp = TQPB(hard_constraints, tucking_constraints,
controlled_values)
self._start = Time.now()
def _build_ext_symbol_map(self, km, ext_paths):
# total_ext_symbols = set()
# Map of path to set of symbols
self._target_body_map = {}
for path in ext_paths:
data = km.get_data(path)
if not isinstance(data, RigidBody):
print(f'Given external path "{path}" is not a rigid body.')
continue
# total_ext_symbols.update(gm.free_symbols(data.pose))
self._target_body_map[path] = {s for s in gm.free_symbols(data.pose) if 'location' not in str(s)}
list_sets = list(self._target_body_map.values())
for x, s in enumerate(list_sets):
# Only keep symbols unique to one target
s.difference_update(list_sets[:x] + list_sets[x + 1:])
for p, symbols in self._target_body_map.items():
for s in symbols:
self._target_map[s] = p
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
'shoulder_pan_joint' : 0.3,
'elbow_flex_joint' : 1.72,
'forearm_roll_joint' : -1.2,
'upperarm_roll_joint': -1.57,
'wrist_flex_joint' : 1.66,
'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)
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 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,
km,
robot_command_processor,
gripper_wrapper,
robot_prefix,
eef_path,
ext_paths_and_poses,
controlled_symbols,
cam_path=None,
weights=None,
resting_pose=None,
visualizer=None,
monitoring_threshold=0.7,
acceptance_threshold=0.8,
control_mode='vel'):
self.km = km
self.robot_command_processor = robot_command_processor
self.gripper_wrapper = gripper_wrapper
self.control_mode = control_mode
self.robot_prefix = robot_prefix
self.eef_path = eef_path
self.cam_path = cam_path
self.visualizer = visualizer
if weights is None:
cvs, _ = gen_dv_cvs(km, {}, controlled_symbols)
self.weights = {c.symbol: c.weight_id for c in cvs.values()}
else:
self.weights = weights
self.controlled_symbols = controlled_symbols
self.controller = None
self.monitoring_threshold = monitoring_threshold
self.acceptance_threshold = acceptance_threshold
self._phase = 'homing'
self._state_lock = RLock()
self._state = {}
self._robot_state_update_count = 0
self._target_map = {}
self._target_body_map = {}
self._last_controller_update = None
self._current_target = None
self._last_external_cmd_msg = None
self._idle_controller = IdleController(km, self.controlled_symbols,
resting_pose, None)
self._build_ext_symbol_map(km, ext_paths_and_poses)
self._world = km.get_active_geometry(
gm.free_symbols(km.get_data(self.eef_path).pose).union(
self._target_map.keys()))
self.pub_external_command = rospy.Publisher('~external_command',
ValueMapMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_external_js = rospy.Subscriber('~external_js',
ValueMapMsg,
callback=self.cb_external_js,
queue_size=1)
self.robot_command_processor.register_state_cb(self.cb_robot_js)
self._kys = False
self._behavior_thread = threading.Thread(target=self.behavior_update)
self._behavior_thread.start()
def generate_push_closing(km,
grounding_state,
controlled_symbols,
eef_pose,
obj_pose,
eef_path,
obj_path,
nav_method='cross',
cp_offset=0,
static_symbols=set()):
# CONTACT GEOMETRY
robot_cp, object_cp, contact_normal = contact_geometry(
eef_pose, obj_pose, eef_path, obj_path)
object_cp = object_cp - contact_normal * cp_offset
geom_distance = gm.dot_product(contact_normal, robot_cp - object_cp)
coll_world = km.get_active_geometry(gm.free_symbols(geom_distance))
# GEOMETRY NAVIGATION LOGIC
# This is exploiting task knowledge which makes this function inflexible.
contact_grad = sum([
sign(-grounding_state[s]) *
gm.vector3(gm.diff(object_cp[0], s), gm.diff(object_cp[1], s),
gm.diff(object_cp[2], s))
for s in gm.free_symbols(obj_pose) if s not in static_symbols
], gm.vector3(0, 0, 0))
neutral_tangent = gm.cross(contact_grad, contact_normal)
active_tangent = gm.cross(neutral_tangent, contact_normal)
contact_constraints, in_contact = generate_contact_model(
robot_cp, controlled_symbols, object_cp, contact_normal,
gm.free_symbols(obj_pose))
target_pos = None
if nav_method == 'linear':
geom_distance = gm.norm(object_cp + active_tangent * geom_distance +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cubic':
dist_scaling = 2**(-0.5 * ((geom_distance - 0.2) / (0.2 * 0.2))**2)
geom_distance = gm.norm(object_cp + active_tangent * dist_scaling -
robot_cp)
elif nav_method == 'cross':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cross_deep':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad *
-gm.dot_product(contact_normal, contact_grad) -
robot_cp)
elif nav_method == 'none' or nav_method is None:
pass
elif nav_method == 'proj':
obj_cp_dist = gm.dot_product(contact_normal,
object_cp - gm.pos_of(obj_pose))
target_pos = gm.pos_of(
obj_pose
) + contact_normal * obj_cp_dist - contact_normal * 0.02 # Drive into the surface
geom_distance = gm.norm(robot_cp - target_pos)
contact_relative_pos = gm.dot(gm.rot_of(obj_pose),
robot_cp - gm.pos_of(obj_pose))
contact_relative_vel = gm.vector3(
sum([gm.diff(contact_relative_pos[0], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[1], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[2], s) for s in controlled_symbols],
0))
# PUSH CONSTRAINT GENERATION
constraints = km.get_constraints_by_symbols(
gm.free_symbols(geom_distance).union(controlled_symbols))
constraints.update(contact_constraints)
# for x, n in enumerate('xyz'):
# constraints[f'zero tangent vel_{n}'] = Constraint((1 - in_contact) * -1e3,
# (1 - in_contact) * 1e3, contact_relative_pos[x] * (1.0 + 0.1 * x))
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')
return constraints, geom_distance, coll_world, PushingInternals(
robot_cp, contact_normal, contact_relative_pos, contact_relative_vel,
debug_draw)
angle_hinge = gm.Symbol('angle_top')
x_hinge_in_parent, z_hinge_in_parent = [
gm.Symbol(f'hinge_in_parent_{x}') for x in 'xz'
]
x_child_in_hinge, z_child_in_hinge = [
gm.Symbol(f'child_in_hinge_{x}') for x in 'xz'
]
fwd_kinematic_hinge = gm.dot(
gm.translation3(x_hinge_in_parent, 0, z_hinge_in_parent),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), angle_hinge),
gm.translation3(x_child_in_hinge, 0, z_child_in_hinge))
# we don't care about the location in y
fwd_kinematic_hinge_residual_tf = gm.speed_up(
gm.dot(gm.diag(1, 0, 1, 1), fwd_kinematic_hinge),
gm.free_symbols(fwd_kinematic_hinge))
def compute_y_hinge_axis_residual(x, angle_tf):
"""Computes the residual for an estimate of the hinge locations on the nobilia shelf
Args:
x (np.ndarray): [xh, zh, xc, zc] x and z locations of hinge in parent and child in hinge
angle_1_tf (TYPE): list of tuples (a, tf) where a is the angle of the top panel relative to the parent
"""
param_dict = {
str(x_hinge_in_parent): x[0],
str(z_hinge_in_parent): x[1],
str(x_child_in_hinge): x[2],
str(z_child_in_hinge): x[3]
}
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
def __init__(self,
km,
actuated_object_path,
target_object_path,
controlled_symbols,
start_state,
camera_path=None,
navigation_method='cross',
visualizer=None,
weight_override=None,
collision_avoidance_paths=None):
print(f'{actuated_object_path}\n{target_object_path}')
actuated_object = km.get_data(actuated_object_path)
target_object = km.get_data(target_object_path)
all_controlled_symbols = controlled_symbols.union({
gm.DiffSymbol(j)
for j in gm.free_symbols(target_object.pose)
if 'location' not in str(j)
})
static_symbols = {
s
for s in gm.free_symbols(target_object.pose)
if 'location' in str(s)
}
# Generate push problem
constraints, \
geom_distance, \
coll_world, \
self.p_internals = generate_push_closing(km,
start_state,
all_controlled_symbols,
actuated_object.pose,
target_object.pose,
actuated_object_path,
target_object_path,
navigation_method,
static_symbols=static_symbols)
start_state.update({s: 0.0 for s in gm.free_symbols(coll_world)})
weight_override = {} if weight_override is None else weight_override
controlled_values, \
constraints = generate_controlled_values(constraints,
all_controlled_symbols)
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.1)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.00)
}
for s, w in weight_override.items():
for cv in controlled_values.values():
if cv.symbol is s:
cv.weight_id = w
break
# CAMERA STUFF
if camera_path is not None:
camera = km.get_data(camera_path)
cam_pos = gm.pos_of(camera.pose)
cam_to_obj = gm.pos_of(target_object.pose) - cam_pos
cam_forward = gm.dot(camera.pose, gm.vector3(1, 0, 0))
look_goal = 1 - (gm.dot_product(cam_to_obj, cam_forward) /
gm.norm(cam_to_obj))
goal_constraints['look_at_obj'] = SC(-look_goal, -look_goal, 1,
look_goal)
# GOAL CONSTAINT GENERATION
# 'avoid_collisions': SC.from_constraint(closest_distance_constraint_world(eef_pose, eef_path[:-1], 0.03), 100)
# }
if collision_avoidance_paths is not None:
for p in collision_avoidance_paths:
obj = km.get_data(p)
goal_constraints[f'avoid_collision {p}'] = SC.from_constraint(
closest_distance_constraint(actuated_object.pose, obj.pose,
actuated_object_path, p, 0.01),
100)
goal_constraints.update({
f'open_object_{x}': PIDC(s, s, 1)
for x, s in enumerate(gm.free_symbols(target_object.pose))
})
self.look_goal = look_goal if camera_path is not None else None
self.in_contact = gm.less_than(geom_distance, 0.01)
self.controlled_values = controlled_values
self.geom_distance = geom_distance
self.qpb = GQPB(coll_world,
constraints,
goal_constraints,
controlled_values,
visualizer=visualizer)
self.qpb._cb_draw = self.p_internals.f_debug_draw
# c_avoidance_constraints = {f'collision avoidance {name}': SC.from_constraint(closest_distance_constraint_world(gm.get_data(Path(name) + ('pose',)),
# Path(name),
# 0.03), 100) for name in active_robot_world.names}
print('\n'.join([str(s) for s in robot_controlled_symbols]))
n_iter = []
total_dur = []
for part_path in [Path(f'kitchen/links/{p}') for p in kitchen_parts]:
# for part_path in [Path(f'nobilia/links/handle')]:
print(f'Planning trajectory for "{part_path}"')
printed_exprs = {}
obj = km.get_data(part_path)
start_state = {s: 0.7 for s in gm.free_symbols(obj.pose)}
active_parts_to_avoid = [
p for p in km.get_active_geometry_raw(gm.free_symbols(
obj.pose)).keys() if p != part_path
]
weights = None
if isinstance(base_joint, DiffDriveJoint):
weights = {}
weights[base_joint.l_wheel_vel] = 0.001
weights[base_joint.r_wheel_vel] = 0.001
if args.vis_plan == 'during':
pushing_controller = PushingController(
km,
def __init__(self,
km,
paths_observables,
transition_rules=None,
num_samples=10):
"""Initializes the tracker to estimate variables based
on the given paths. The paths are assumed to point to
6D poses.
Args:
km (ArticulationModel): Articulation model to used
paths_observables (list): List of paths to poses that will be observed
noise_estimation_observations (int): Number of observations to collect before initializing the EKFs
estimate_init_steps (int): Number of steps used to initialize the estimate of an ekf
"""
poses = {p: km.get_data(p) for p in paths_observables}
poses = {
str(p): pose.pose if isinstance(pose, Frame) else pose
for p, pose in poses.items()
}
# obs_features = {p: generate_6d_feature(pose) for p, pose in poses.items()}
obs_features = {p: pose for p, pose in poses.items()}
# Identify tracking pools. All poses within one pool share at least one DoF
tracking_pools = [(gm.free_symbols(feature), [(p, feature)])
for p, feature in obs_features.items()
if len(gm.free_symbols(feature)) != 0]
final_pools = []
while len(tracking_pools) > 0:
syms, feature_list = tracking_pools[0]
initial_syms_size = len(syms)
y = 1
while y < len(tracking_pools):
o_syms, o_feature_list = tracking_pools[y]
if len(syms.intersection(o_syms)) != 0:
syms = syms.union(o_syms)
feature_list += o_feature_list
del tracking_pools[y]
else:
y += 1
# If the symbol set has not been enlarged,
# there is no more overlap with other pools
if len(syms) == initial_syms_size:
final_pools.append((syms, feature_list))
tracking_pools = tracking_pools[1:]
else:
tracking_pools[0] = (syms, feature_list)
tracking_pools = final_pools
self.estimators = [
QPStateModel(km,
dict(features),
transition_rules=transition_rules,
num_samples=num_samples)
for symbols, features in tracking_pools
]
print(f'Generated {len(self.estimators)} Estimators:\n '
'\n '.join(str(e) for e in self.estimators))
self.observation_names = [str(p) for p in paths_observables]
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)
km = GeometryModel()
vis = ROSBPBVisualizer('kineverse/nobilia/vis', base_frame='world')
origin_pose = gm.translation3(x_loc_sym, 0, 0)
debug = create_nobilia_shelf(km, Path('nobilia'), origin_pose)
km.clean_structure()
km.dispatch_events()
shelf = km.get_data('nobilia')
shelf_pos = shelf.joints['hinge'].position
world = km.get_active_geometry({shelf_pos
}.union(gm.free_symbols(origin_pose)))
params = debug.tuning_params
with dpg.window(label='Parameter settings'):
for s, p in debug.tuning_params.items():
def update_param(sender, value, symbol):
params[symbol] = value
draw_shelves(shelf, params, world, vis, steps=5)
slider = dpg.add_slider_float(label=f'{s}',
id=f'{s}',
callback=update_param,
user_data=s,
min_value=p - 0.1,
door_x, door_y = [gm.Position(f'localization_{x}') for x in 'xy']
create_door(km,
Path('door'),
0.5,
0.35,
to_world_tf=gm.translation3(door_x, door_y, 0.1))
km.clean_structure()
km.dispatch_events()
door = km.get_data('door')
handle = door.links['handle']
iiwa = km.get_data('iiwa')
symbols = gm.free_symbols(door.links['handle'].pose).union(
gm.free_symbols(iiwa.links['link_7'].pose))
door_position = door.joints['hinge'].position
handle_position = door.joints['handle'].position
# Fun for visuals
world = km.get_active_geometry(symbols)
symbols = world.free_symbols
eef = iiwa.links['wsg_50_tool_frame']
goal_pose = gm.dot(handle.pose, gm.translation3(-0.08, 0.1, 0),
gm.rotation3_rpy(0, math.pi * 0.5, 0))
ik_solver = IKSolver(km, eef.pose, visualizer)
