def cb_controls(self, map_message): self.last_control = { gm.Symbol(str_symbol): v for str_symbol, v in zip(map_message.symbol, map_message.value) if str_symbol in self.str_controls } self.tracker.process_control(self.last_control)
def cb_external_js(self, value_msg): state_update = { gm.Symbol(s): v for s, v in zip(value_msg.symbol, value_msg.value) } with self._state_lock: self._state.update(state_update)
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 __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)
world_frame='world', wheel_radius=0.12 * 0.5, wheel_distance=0.3748, wheel_vel_limit=17.4) else: insert_omni_base(km, robot_path, robot_urdf.get_root(), 'world') km.clean_structure() km.dispatch_events() robot = km.get_data(robot_path) nobilia = km.get_data(model_name) handle = nobilia.links[rospy.get_param('~handle', 'handle')] integration_rules = None sym_dt = gm.Symbol('dT') if robot_name == 'pr2': joint_symbols, \ robot_controlled_symbols, \ start_pose, \ eef, \ blacklist = pr2_setup(km, robot_path) elif robot_name == 'fetch': joint_symbols, \ robot_controlled_symbols, \ start_pose, \ eef, \ blacklist = generic_setup(km, robot_path, rospy.get_param('~eef', 'gripper_link')) if rospy.get_param('~use_base', False): base_joint = robot.joints['to_world']
def create_nobilia_shelf(km, prefix, origin_pose=gm.eye(4), parent_path=Path('world')): km.apply_operation( f'create {prefix}', ExecFunction(prefix, MarkedArticulatedObject, str(prefix))) shelf_height = 0.72 shelf_width = 0.6 shelf_body_depth = 0.35 wall_width = 0.016 l_prefix = prefix + ('links', ) geom_body_wall_l = Box( l_prefix + ('body', ), gm.translation3(0, 0.5 * (shelf_width - wall_width), 0), gm.vector3(shelf_body_depth, wall_width, shelf_height)) geom_body_wall_r = Box( l_prefix + ('body', ), gm.translation3(0, -0.5 * (shelf_width - wall_width), 0), gm.vector3(shelf_body_depth, wall_width, shelf_height)) geom_body_ceiling = Box( l_prefix + ('body', ), gm.translation3(0, 0, 0.5 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width)) geom_body_floor = Box( l_prefix + ('body', ), gm.translation3(0, 0, -0.5 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width)) geom_body_shelf_1 = Box( l_prefix + ('body', ), gm.translation3(0.02, 0, -0.2 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width, wall_width)) geom_body_shelf_2 = Box( l_prefix + ('body', ), gm.translation3(0.02, 0, 0.2 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width, wall_width)) geom_body_back = Box( l_prefix + ('body', ), gm.translation3(0.5 * (shelf_body_depth - 0.005), 0, 0), gm.vector3(0.005, shelf_width - 2 * wall_width, shelf_height - 2 * wall_width)) shelf_geom = [ geom_body_wall_l, geom_body_wall_r, geom_body_ceiling, geom_body_floor, geom_body_back, geom_body_shelf_1, geom_body_shelf_2 ] rb_body = RigidBody(parent_path, origin_pose, geometry=dict(enumerate(shelf_geom)), collision=dict(enumerate(shelf_geom))) geom_panel_top = Box(l_prefix + ('panel_top', ), gm.eye(4), gm.vector3(0.357, 0.595, wall_width)) geom_panel_bottom = Box(l_prefix + ('panel_bottom', ), gm.eye(4), gm.vector3(0.357, 0.595, wall_width)) handle_width = 0.16 handle_depth = 0.05 handle_diameter = 0.012 geom_handle_r = Box( l_prefix + ('handle', ), gm.translation3(0.5 * handle_depth, 0.5 * (handle_width - handle_diameter), 0), gm.vector3(handle_depth, handle_diameter, handle_diameter)) geom_handle_l = Box( l_prefix + ('handle', ), gm.translation3(0.5 * handle_depth, -0.5 * (handle_width - handle_diameter), 0), gm.vector3(handle_depth, handle_diameter, handle_diameter)) geom_handle_bar = Box( l_prefix + ('handle', ), gm.translation3(handle_depth - 0.5 * handle_diameter, 0, 0), gm.vector3(handle_diameter, handle_width - handle_diameter, handle_diameter)) handle_geom = [geom_handle_l, geom_handle_r, geom_handle_bar] # Sketch of mechanism # # T ---- a # ---- \ Z # b ..... V \ # | ...... d # B | ------ # c ------ # L # # Diagonal V is virtual # # # Angles: # a -> alpha (given) # b -> gamma_1 + gamma_2 = gamma # c -> don't care # d -> delta_1 + delta_2 = delta # opening_position = gm.Position(prefix + ('door', )) # Calibration results # # Solution top hinge: cost = 0.03709980624159568 [ 0.08762252 -0.01433833 0.2858676 0.00871125] # Solution bottom hinge: cost = 0.025004236048128934 [ 0.1072496 -0.01232362 0.27271013 0.00489996] # Added 180 deg rotation due to -x being the forward facing side in this model top_hinge_in_body_marker = gm.translation3(0.08762252 - 0.015, 0, -0.01433833) top_panel_marker_in_top_hinge = gm.translation3(0.2858676 - 0.003, -wall_width + 0.0025, 0.00871125 - 0.003) front_hinge_in_top_panel_maker = gm.translation3(0.1072496 - 0.02, 0, -0.01232362 + 0.007) bottom_panel_marker_in_front_hinge = gm.translation3( 0.27271013, 0, 0.00489996) # Top hinge - Data taken from observation body_marker_in_body = gm.dot( gm.rotation3_axis_angle(gm.vector3(0, 0, 1), math.pi), gm.translation3(0.5 * shelf_body_depth - 0.062, -0.5 * shelf_width + 0.078, 0.5 * shelf_height)) top_panel_marker_in_top_panel = gm.translation3( geom_panel_top.scale[0] * 0.5 - 0.062, -geom_panel_top.scale[1] * 0.5 + 0.062, geom_panel_top.scale[2] * 0.5) bottom_panel_marker_in_bottom_panel = gm.translation3( geom_panel_bottom.scale[0] * 0.5 - 0.062, -geom_panel_bottom.scale[1] * 0.5 + 0.062, geom_panel_bottom.scale[2] * 0.5) top_hinge_in_body = gm.dot(body_marker_in_body, top_hinge_in_body_marker) top_panel_in_top_hinge = gm.dot( top_panel_marker_in_top_hinge, gm.inverse_frame(top_panel_marker_in_top_panel)) front_hinge_in_top_panel = gm.dot(top_panel_marker_in_top_panel, front_hinge_in_top_panel_maker) bottom_panel_in_front_hinge = gm.dot( bottom_panel_marker_in_front_hinge, gm.inverse_frame(bottom_panel_marker_in_bottom_panel)) # Point a in body reference frame point_a = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(top_hinge_in_body)) point_d = gm.point3(-shelf_body_depth * 0.5 + 0.09, 0, shelf_height * 0.5 - 0.192) # point_d = gm.point3(-shelf_body_depth * 0.5 + gm.Symbol('point_d_x'), 0, shelf_height * 0.5 - gm.Symbol('point_d_z')) # Zero alpha along the vertical axis vec_a_to_d = gm.dot(point_d - point_a) alpha = gm.atan2(vec_a_to_d[0], -vec_a_to_d[2]) + opening_position top_panel_in_body = gm.dot( top_hinge_in_body, # Translation hinge to body frame gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position + 0.5 * math.pi), # Hinge around y top_panel_in_top_hinge) front_hinge_in_body = gm.dot(top_panel_in_body, front_hinge_in_top_panel) # Point b in top panel reference frame point_b_in_top_hinge = gm.pos_of( gm.dot(gm.diag(1, 0, 1, 1), front_hinge_in_top_panel, top_panel_in_top_hinge)) point_b = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(front_hinge_in_body)) # Hinge lift arm in body reference frame point_c_in_bottom_panel = gm.dot( gm.diag(1, 0, 1, 1), bottom_panel_marker_in_bottom_panel, gm.point3(-0.094, -0.034, -0.072), # gm.point3(-gm.Symbol('point_c_x'), -0.034, -gm.Symbol('point_c_z')) ) point_c_in_front_hinge = gm.dot( gm.diag(1, 0, 1, 1), gm.dot(bottom_panel_in_front_hinge, point_c_in_bottom_panel)) length_z = gm.norm(point_a - point_d) vec_a_to_b = point_b - point_a length_t = gm.norm(vec_a_to_b) length_b = gm.norm(point_c_in_front_hinge[:3]) # length_l = gm.Symbol('length_l') # 0.34 length_l = 0.372 vec_b_to_d = point_d - point_b length_v = gm.norm(vec_b_to_d) gamma_1 = inner_triangle_angle(length_t, length_v, length_z) gamma_2 = inner_triangle_angle(length_b, length_v, length_l) top_panel_offset_angle = gm.atan2(point_b_in_top_hinge[2], point_b_in_top_hinge[0]) bottom_offset_angle = gm.atan2(point_c_in_front_hinge[2], point_c_in_front_hinge[0]) gamma = gamma_1 + gamma_2 rb_panel_top = RigidBody(l_prefix + ('body', ), gm.dot(rb_body.pose, top_panel_in_body), top_panel_in_body, geometry={0: geom_panel_top}, collision={0: geom_panel_top}) # old offset: 0.5 * geom_panel_top.scale[2] + 0.03 tf_bottom_panel = gm.dot( front_hinge_in_top_panel, gm.rotation3_axis_angle( gm.vector3(0, 1, 0), math.pi + bottom_offset_angle - top_panel_offset_angle), gm.rotation3_axis_angle(gm.vector3(0, -1, 0), gamma), bottom_panel_in_front_hinge) rb_panel_bottom = RigidBody(l_prefix + ('panel_top', ), gm.dot(rb_panel_top.pose, tf_bottom_panel), tf_bottom_panel, geometry={0: geom_panel_bottom}, collision={0: geom_panel_bottom}) handle_transform = gm.dot( gm.translation3(geom_panel_bottom.scale[0] * 0.5 - 0.08, 0, 0.5 * wall_width), gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -math.pi * 0.5)) rb_handle = RigidBody(l_prefix + ('panel_bottom', ), gm.dot(rb_panel_bottom.pose, handle_transform), handle_transform, geometry={x: g for x, g in enumerate(handle_geom)}, collision={x: g for x, g in enumerate(handle_geom)}) # Only debugging point_c = gm.dot(rb_panel_bottom.pose, point_c_in_bottom_panel) vec_b_to_c = point_c - point_b km.apply_operation(f'create {prefix}/links/body', CreateValue(rb_panel_top.parent, rb_body)) km.apply_operation(f'create {prefix}/links/panel_top', CreateValue(rb_panel_bottom.parent, rb_panel_top)) km.apply_operation( f'create {prefix}/links/panel_bottom', CreateValue(l_prefix + ('panel_bottom', ), rb_panel_bottom)) km.apply_operation(f'create {prefix}/links/handle', CreateValue(l_prefix + ('handle', ), rb_handle)) km.apply_operation( f'create {prefix}/joints/hinge', ExecFunction( prefix + Path('joints/hinge'), RevoluteJoint, CPath(rb_panel_top.parent), CPath(rb_panel_bottom.parent), opening_position, gm.vector3(0, 1, 0), gm.eye(4), 0, 1.84, **{ f'{opening_position}': Constraint(0 - opening_position, 1.84 - opening_position, opening_position), f'{gm.DiffSymbol(opening_position)}': Constraint(-0.25, 0.25, gm.DiffSymbol(opening_position)) })) m_prefix = prefix + ('markers', ) km.apply_operation( f'create {prefix}/markers/body', ExecFunction(m_prefix + ('body', ), Frame, CPath(l_prefix + ('body', )), gm.dot(rb_body.pose, body_marker_in_body), body_marker_in_body)) km.apply_operation( f'create {prefix}/markers/top_panel', ExecFunction(m_prefix + ('top_panel', ), Frame, CPath(l_prefix + ('panel_top', )), gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel), top_panel_marker_in_top_panel)) km.apply_operation( f'create {prefix}/markers/bottom_panel', ExecFunction( m_prefix + ('bottom_panel', ), Frame, CPath(l_prefix + ('panel_bottom', )), gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel), bottom_panel_marker_in_bottom_panel)) return NobiliaDebug( [ top_hinge_in_body, gm.dot( top_hinge_in_body, gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position + 0.5 * math.pi), top_panel_in_top_hinge, front_hinge_in_top_panel), body_marker_in_body, gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel), gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel) ], [(point_a, vec_a_to_d), (point_a, vec_a_to_b), (point_b, vec_b_to_d), (point_b, vec_b_to_c)], { 'gamma_1': gamma_1, 'gamma_1 check_dot': gamma_1 - gm.acos( gm.dot_product(-vec_a_to_b / gm.norm(vec_a_to_b), vec_b_to_d / gm.norm(vec_b_to_d))), 'gamma_1 check_cos': gamma_1 - inner_triangle_angle( gm.norm(vec_a_to_b), gm.norm(vec_b_to_d), gm.norm(vec_a_to_d)), 'gamma_2': gamma_2, 'gamma_2 check_dot': gamma_2 - gm.acos( gm.dot_product(vec_b_to_c / gm.norm(vec_b_to_c), vec_b_to_d / gm.norm(vec_b_to_d))), 'length_v': length_v, 'length_b': length_b, 'length_l': length_l, 'position': opening_position, 'alpha': alpha, 'dist c d': gm.norm(point_d - point_c) }, { gm.Symbol('point_c_x'): 0.094, gm.Symbol('point_c_z'): 0.072, gm.Symbol('point_d_x'): 0.09, gm.Symbol('point_d_z'): 0.192, gm.Symbol('length_l'): 0.372 })
class EKFModel(object): DT_SYM = gm.Symbol('dt') 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 gen_control_vector(self, control_dict): """Generates a control vector from a given dict mapping symbols to values Args: control_dict (dict): Map symbol -> float Returns: np.ndarray: Vectorized constrol """ return np.array([control_dict[s] for s in self.ordered_controls]) def gen_obs_vector(self, obs_dict): """Generates a flat vector of observations from a dictionary of them. Args: obs_dict (dict): Dict of observations. Names need to match those given to __init__ Returns: np.ndarray: Flat vector of observations """ return np.hstack([np.take(obs_dict[l], i) for l, i in self.takers]) def pandas_R(self): """Returns the measurement covariance matrix as pandas frame. Returns: pd.DataFrame: Measurement covariance as DataFrame if matrix is stored. None otherwise. """ if self.R is None: return None return pd.DataFrame(data=self.R, index=self.obs_labels, columns=self.obs_labels) @profile def predict(self, state_t, Sigma_t, control, dt=0.05): """Predicts the next state and covariance from current state, covariance, and control signal. Args: state_t (np.ndarray): Current state vector Sigma_t (np.ndarray): Current covariance control (nd.ndarray): Current control vector dt (float, optional): Size of the prediction step Returns: (np.ndarray, np.ndarray): predicted_state, predicted_covariance """ params = np.hstack(([dt], state_t, control)) F_t = self.g_prime_fn.call2(params) # print(f'state_t: {state_t}\nSigma_t:\n{Sigma_t}\ncontrol: {control}\nparams: {params}\nF_t: {F_t}') return self.g_fn.call2(params), F_t.dot(Sigma_t.dot(F_t.T)) + self.Q @profile def update(self, state_t, Sigma_t, obs_t): """Performs the kalman update. Args: state_t (np.ndarray): Current state Sigma_t (np.ndarray): Current covariance obs_t (np.ndarray): Current observation Returns: (np.ndarray, np.ndarray): Updated state, updated covariance Raises: Exception: Will raise an exception if the measurement covariance R is not set """ if self.R is None: raise Exception('No noise model set for EKF model.') H_t = self.h_prime_fn.call2(state_t) h_t = self.h_fn.call2(state_t) # H_indices, H_t = reorder_H(H_t, np.diag(Sigma_t), h_t, obs_t) if self.trim_threshold is not None: idx_obs, H_t = trim_singularities(H_t, self.trim_threshold) else: idx_obs = range(H_t.shape[0]) # These indices actually refer to the reordered H_t, not the original one # idx_obs = [H_indices[x] for x in idx_obs] # if not hasattr(self, 'lol'): # pd.DataFrame(data=H_t).to_csv('H_t.csv') # np.savetxt('R.csv', self.R) # self.lol = True # After reducing the observation, R will not necessarily match anymore reduced_R = np.take(np.take(self.R, idx_obs, axis=0), idx_obs, axis=1) S_t = H_t.dot(Sigma_t.dot(H_t.T)) + reduced_R # print(f'H_t: {H_t}\nS_t:\n{S_t}') if np.linalg.det(S_t) != 0.0: K_t = Sigma_t.dot(H_t.T.dot(np.linalg.inv(S_t))) y_t = np.take(obs_t - h_t.flatten(), idx_obs).reshape( (len(idx_obs), 1)) state_t = (state_t + K_t.dot(y_t)).flatten() Sigma_t = (np.eye(Sigma_t.shape[0]) - K_t.dot(H_t)).dot(Sigma_t) return state_t, Sigma_t else: print('Determinant of S_t is 0') return state_t, Sigma_t def set_R(self, R): """Set the measurement covariance matrix R Args: R (np.ndarray): Measurement covariance matrix """ self.R = R @profile def generate_R(self, noisy_observations): """Generates the covariance matrix from a set of noisy observations. Once generated, the matrix will be stored in the model. Args: noisy_observations ([dict]): List of observation dictionaries. """ obs = np.vstack( [self.gen_obs_vector(obs) for obs in noisy_observations]) cov = np.cov(obs.T) self.set_R(cov) def spawn_particle(self): """Spawns a particle initialized to be in the center of the configuration space. The covariance is initialized as variance of the observed quantities. Returns: Particle: A particle modeling an estimate of the state of this model """ return Particle( (self.state_bounds.T[0] + self.state_bounds.T[1]) * 0.5, np.diag(self.state_bounds.T[1] - self.state_bounds.T[0])**2) def __str__(self): return 'EKF estimating:\n {}\nFrom:\n {}\nWith controls:\n {}'.format( '\n '.join(str(s) for s in self.ordered_vars), '\n '.join(l for l in self.obs_labels), '\n '.join(str(c) for c in self.ordered_controls))
import math import rospy import numpy as np import kineverse.gradients.gradient_math as gm import dearpygui.dearpygui as dpg import signal from kineverse.model.geometry_model import GeometryModel, Path from kineverse.visualization.bpb_visualizer import ROSBPBVisualizer from kineverse_experiment_world.nobilia_shelf import create_nobilia_shelf x_loc_sym = gm.Symbol('location_x') 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)
data = timeline[-1] if x == 0: return getattr(data, value) p_data = timeline[x - 1] segment_length = (data.stamp - p_data.stamp).to_sec() if segment_length < 1e-4: return getattr(data, value) fac = (stamp - p_data.stamp).to_sec() / segment_length return (1 - fac) * getattr(p_data, value) + fac * getattr(data, value) 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 __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
class QPStateModel(object): DT_SYM = gm.Symbol('dt') 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 _integrate_state(self): now = Time.now() if self._stamp_last_integration is not None: dt = (now - self._stamp_last_integration).to_sec() self._state[QPStateModel.DT_SYM] = dt new_state = self.transition_fn.call2([self._state[x] for x in self.transition_args]).flatten() for x, (s, v) in enumerate(zip(self.ordered_vars, new_state)): delta = v - self._state[s] self._state[s] = v for state in self._state_buffer: state[x] += delta self._stamp_last_integration = now def set_command(self, command): self._integrate_state() for s in self.command_vars: if s in command: self._state[s] = command[s] @profile def update(self, observation): self._integrate_state() for o_label, o_vars in self.obs_vars.items(): if o_label in observation: self._obs_state[self.switch_vars[o_label]] = 1 sub_obs = np.take(observation[o_label], self.takers[o_label]) self._obs_state.update({s: v for s, v in zip(o_vars, sub_obs)}) else: self._obs_state[self.switch_vars[o_label]] = 0 self._obs_state.update(self._state) self._obs_state[QPStateModel.DT_SYM] = 0.5 for x in range(self._max_iterations): cmd = self.qp.get_cmd(self._obs_state, deltaT=0.5) self._obs_state.update(cmd) new_state = self.transition_fn.call2([self._obs_state[s] for s in self.transition_args]) self._obs_state.update({s: v for s, v in zip(self.ordered_vars, new_state)}) if self.qp.equilibrium_reached(): break # CMA update self._obs_count += 1 # cma_n = np.array([self._state[s] for s in self.ordered_vars]) x_n_1 = np.array([self._obs_state[s] for s in self.ordered_vars]).flatten() self._state_buffer.append(x_n_1) if len(self._state_buffer) > self.num_samples: self._state_buffer = self._state_buffer[-self.num_samples:] state_mean = np.mean(self._state_buffer, axis=0) # print(f'cma_n: {cma_n}\nx_n_1: {x_n_1}') # cma_n_1 = cma_n + (x_n_1 - cma_n) / self._obs_count if self._obs_count > 1 else x_n_1 self._state.update({s: v for s, v in zip(self.ordered_vars, state_mean)}) return self.qp.latest_error def state(self): self._integrate_state() return {s: self._state[s] for s in self.ordered_vars} @property def latest_error(self): return self.qp.latest_error def __str__(self): return 'QP estimating:\n {}\nFrom:\n {}\nWith controls:\n {}'.format( '\n '.join(str(s) for s in self.ordered_vars), '\n '.join(l for l in sorted(str(s) for s in sum(self.obs_vars.values(), []))), '\n '.join(c for c in sorted(str(c) for c in self.command_vars)))