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