From 47ba0db68d283c9ea83a825a336b6b55bf0e7320 Mon Sep 17 00:00:00 2001 From: Jordan Souvenir Date: Fri, 31 Jul 2026 20:26:52 -0400 Subject: [PATCH] Add GraphSLAM data collection loop and full linearize/reduce/solve pipeline Adds a graphSLAM Webots controller (graphSLAM.py) that teleports the robot through rotate/move cycles while collecting synchronized odometry, landmark range-bearing, and ground-truth logs. Data collection helpers (get_odometry, get_landmark_row, rotate_step, move_forward_step) live in graphSLAM_data_collection.py, including a slew-rate limiter on rotate_step that fixes a spurious odometry velocity spike at the start of each rotation. navigation_tools.py holds shared geometry helpers split out of my_robot.py. Implements the full offline GraphSLAM pipeline in graph_slam_calculations.py: - graphSLAM_init: dead-reckons an initial pose trajectory and seeds landmark estimates from first sightings - graphSLAM_linearize: builds the information matrix/vector for one Gauss-Newton correction from motion and measurement edges - graphSLAM_reduce: eliminates landmark variables via Schur complement - graphSLAM_solve: solves the reduced system and back-substitutes landmark corrections - graphSLAM_run: iterates linearize/reduce/solve to convergence - Accuracy/diagnostic helpers (graphSLAM_accuracy, print_graphSLAM_accuracy, print_landmark_triangulation_check, print_landmark_observation_spread, print_worst_pose_errors) for comparing results against ground truth Each stage was verified against a synthetic scenario with known ground truth: linearize/reduce/solve match a full-system solve to floating-point precision, and graphSLAM_run converges landmark/pose estimates toward true positions. Known limitation: on real Webots runs, GraphSLAM currently underperforms dead reckoning (~-31% on the current path/cycle configuration). Root cause diagnosed as insufficient landmark observability -- stretches of the path where fewer than two landmarks are simultaneously visible, or where the visible landmark(s) are seen across a narrow angular window, leave individual poses underdetermined even though the overall landmark map converges well. Not an implementation bug: linearize/reduce/solve were verified independently, and the odometry-spike issue found during investigation was fixed separately. Remaining fix requires path/rotation schedule or camera FOV changes, not optimization changes. Co-Authored-By: Claude Sonnet 5 --- .../robot_lib/graphSLAM_data_collection.py | 146 +++++ .../robot_lib/graph_slam_calculations.py | 544 ++++++++++++++++++ realm_tools/robot_lib/my_robot.py | 455 ++++++++++++++- realm_tools/robot_lib/navigation_tools.py | 85 +++ simulation/controllers/graphSLAM/graphSLAM.py | 95 +++ 5 files changed, 1323 insertions(+), 2 deletions(-) create mode 100644 realm_tools/robot_lib/graphSLAM_data_collection.py create mode 100644 realm_tools/robot_lib/graph_slam_calculations.py create mode 100644 realm_tools/robot_lib/navigation_tools.py create mode 100644 simulation/controllers/graphSLAM/graphSLAM.py diff --git a/realm_tools/robot_lib/graphSLAM_data_collection.py b/realm_tools/robot_lib/graphSLAM_data_collection.py new file mode 100644 index 0000000..f1258c8 --- /dev/null +++ b/realm_tools/robot_lib/graphSLAM_data_collection.py @@ -0,0 +1,146 @@ +import numpy as np +import math +from realm_tools.robot_lib.my_robot import MyRobot + + +""" +This function gets the row in a 2d array where each index corresponds to +each landmark and the data stored is the distance to each landmark +""" +def get_landmark_row(robot, n_landmarks=8): + row = np.full((n_landmarks, 2), np.nan, dtype=np.float32) # <-- shape fix + + for obj in robot.camera.getRecognitionObjects(): + colors = obj.getColors() # returns a flat list, e.g. [r, g, b] per color + if not colors: + continue + color_key = tuple(colors[:3]) + rel_x, rel_y, _ = obj.getPosition() + landmark_index = get_landmark_index(color_key) + dist = math.hypot(rel_x, rel_y) + bearing = math.atan2(rel_y, rel_x) + row[landmark_index] = (dist, bearing) + return row + + +LANDMARK_COLOR_MAP = { + (1.0, 0.0, 0.0): 0, + (0.0, 1.0, 0.0): 1, + (0.0, 0.0, 1.0): 2, + (1.0, 1.0, 0.0): 3, + (0.0, 1.0, 1.0): 4, + (1.0, 0.5, 0.0): 5, + (0.5, 0.0, 0.5): 6, + (0.0, 0.5, 0.5): 7, +} + +def get_landmark_index(color_key): + return LANDMARK_COLOR_MAP[color_key] + +def get_odometry(robot, prev_encoder_readings, dt): + """ + Compute linear and angular velocity from encoder deltas since the last call. + + Parameters + ---------- + robot : Robot + prev_encoder_readings : tuple/list (left, right) — encoder values from the previous timestep + dt : float — elapsed time since the previous timestep (seconds) + + Returns + ------- + v : float — linear velocity (m/s) + omega : float — angular velocity (rad/s) + current_encoder_readings : tuple (left, right) — pass this in as prev_encoder_readings next call + """ + current_encoder_readings = robot.get_encoder_readings() + + delta_left = current_encoder_readings[0] - prev_encoder_readings[0] + delta_right = current_encoder_readings[1] - prev_encoder_readings[1] + + delta_s_left = delta_left * robot.wheel_radius + delta_s_right = delta_right * robot.wheel_radius + + delta_s = (delta_s_left + delta_s_right) / 2.0 + delta_theta = (delta_s_right - delta_s_left) / robot.axel_length + + v = delta_s / dt + omega = delta_theta / dt + + return v, omega, current_encoder_readings + +def rotate_step(self, degrees=90, Kp=1, Ki=0, Kd=0, margin_error=0.01, max_accel=None): + """ + Generator version of rotate(). Does ONE PID update per call to next(). + Caller is responsible for calling robot.experiment_supervisor.step(timestep) + between each next() call. + + max_accel slew-rate limits the commanded velocity so it ramps up over + a few steps instead of jumping straight to the PID's (often saturated) + output on the very first step. Without this, a large initial heading + error commands near-max velocity immediately, but the wheels can't + actually reach that speed in one ~32ms timestep -- the encoder-derived + odometry for that step ends up reflecting the abrupt commanded jump + rather than the robot's actual smooth motion, showing up as a spurious + velocity spike right at the move -> rotate transition. Defaults to + max_motor_velocity / 10 (full speed reached after ~10 steps). + + Yields True while still rotating, then yields False once and stops + (StopIteration) when the rotation is complete. + """ + I = 0.0 + prev_error = 0.0 + dt = 0.032 + if max_accel is None: + max_accel = self.max_motor_velocity / 10.0 + + setpoint = (self.get_compass_reading() + degrees) % 360 + applied_velocity = 0.0 + + while True: + current_heading = self.get_compass_reading() + error = (setpoint - current_heading + 180) % 360 - 180 + P = Kp * error + I = Ki * (I + error * dt) + D = Kd * ((error - prev_error) / dt) + + desired_velocity = abs(self.sat(P + I + D)) + + if -margin_error <= error <= margin_error: + self.stop() + return # done — generator ends here + + #ramp applied_velocity toward desired_velocity instead of jumping + #straight to it, so the commanded velocity changes smoothly step + #to step + velocity_step = max(-max_accel, min(max_accel, desired_velocity - applied_velocity)) + applied_velocity += velocity_step + out_signal = applied_velocity + + if error < 0: + self.set_right_motor_velocity(-out_signal) + self.set_left_motor_velocity(out_signal) + elif error > 0: + self.set_right_motor_velocity(out_signal) + self.set_left_motor_velocity(-out_signal) + + prev_error = error + yield # give control back to the caller after one PID step + +def move_forward_step(self, distance, Kp=20, margin_error=0.01): + """ + Generator version of move_forward(). Does ONE control update per call to next(). + Caller is responsible for calling robot.experiment_supervisor.step(timestep) + between each next() call. + """ + starting_encoder_position = self.get_encoder_readings() + + while True: + error = distance - self.calculate_wheel_distance_traveled(starting_encoder_position) + + if error <= margin_error: + self.stop() + return # done — generator ends here + + self.go_forward(velocity=self.sat(Kp * error)) + yield # give control back to the caller after one control step \ No newline at end of file diff --git a/realm_tools/robot_lib/graph_slam_calculations.py b/realm_tools/robot_lib/graph_slam_calculations.py new file mode 100644 index 0000000..4f61331 --- /dev/null +++ b/realm_tools/robot_lib/graph_slam_calculations.py @@ -0,0 +1,544 @@ +import math +import numpy as np + + +def normalize_angle(angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + + +def pose_slice(t): + """Index range in the (horizontal) stacked state vector for pose x_t.""" + return slice(3 * t, 3 * t + 3) + + +def landmark_slice(j, n_poses): + """Index range in the stacked state vector for landmark m_j.""" + start = 3 * n_poses + 2 * j + return slice(start, start + 2) + + +def graphSLAM_init(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0)): + """ + Builds the initial linearization point: dead-reckons pose estimates + from odometry_log, then seeds each landmark's (x, y) estimate from its + first non-NaN observation. + + odometry_log[0] is the null edge captured before the loop started + (near-zero motion into x0) and is skipped as a real motion edge. + + Parameters + ---------- + odometry_log : list of (v, omega, dt) + landmark_log : list of (n_landmarks, 2) arrays of (range, bearing), + NaN where a landmark wasn't visible that timestep + x0 : initial pose, (x, y, theta) + + Returns + ------- + mu_poses : (T, 3) ndarray of [x, y, theta] pose estimates + mu_landmarks : (n_landmarks, 2) ndarray of [x, y] landmark estimates, + NaN rows for landmarks never observed + """ + + timesteps = len(odometry_log) + #creates an array corresponding to the poses at each timestep + mu_poses = np.zeros((timesteps, 3)) + #index first pose as (x=0,y=0,theta=0) + mu_poses[0] = x0 + + #given the linear and angular velocity at time t + #compute the belief pose + for t in range(1, timesteps): + v, omega, dt = odometry_log[t] + x, y, theta = mu_poses[t - 1] + + #poses are estimated from velocity times time (distance) + #then multiplied by cos(theta) sin and (theta) + #as these are decomposed into components + #omega *dt gives angular displacement which is added to theta and normalized + mu_poses[t] = [ + x + v * dt * math.cos(theta), + y + v * dt * math.sin(theta), + normalize_angle(theta + omega * dt), + ] + + + n_landmarks = landmark_log[0].shape[0] + #mu_landmarks stores position of landmark when it is first seen + #the position is calculated by turning the relative position into a + #global position + mu_landmarks = np.full((n_landmarks, 2), np.nan, dtype=np.float64) + for t, row in enumerate(landmark_log): + for j in range(n_landmarks): + if np.isnan(mu_landmarks[j, 0]) and not np.isnan(row[j, 0]): + r, phi = row[j] + x, y, theta = mu_poses[t] + bearing = theta + phi + mu_landmarks[j] = [ + x + r * math.cos(bearing), + y + r * math.sin(bearing), + ] + + return mu_poses, mu_landmarks + + +def graphSLAM_linearize(mu_poses, mu_landmarks, odometry_log, landmark_log, + R=None, Q=None, anchor_information=1e9): + """ + One Gauss-Newton linearization pass. Builds the information matrix + Omega and information vector xi such that solving + + Omega @ delta = xi + + gives the CORRECTION delta to apply on top of the current estimate: + + mu_poses_new = mu_poses + delta[:3*T].reshape(T, 3) + mu_landmarks_new = mu_landmarks + delta[3*T:].reshape(M, 2) + + State vector layout: [x_0, x_1, ..., x_{T-1}, m_0, m_1, ..., m_{M-1}], + each x_t = (x, y, theta), each m_j = (x, y). + + Parameters + ---------- + mu_poses : (T, 3) current pose estimates + mu_landmarks : (M, 2) current landmark estimates (NaN row = unobserved) + odometry_log : list of (v, ang_v, dt), same length as mu_poses + landmark_log : list of (M, 2) arrays of (range, bearing), same length + as mu_poses + R : (3, 3) motion noise covariance + Q : (2, 2) measurement noise covariance + anchor_information : information (not covariance) pinning x_0 in place + + Returns + ------- + Omega : (3T + 2M, 3T + 2M) ndarray + xi : (3T + 2M) ndarray + """ + num_timesteps = len(mu_poses) + num_landmarks = len(mu_landmarks) + total_dimensions = 3 * num_timesteps + 2 * num_landmarks + + #noise estimates + if R is None: + R = np.diag([0.05 ** 2, 0.05 ** 2, math.radians(2) ** 2]) + if Q is None: + Q = np.diag([0.05 ** 2, math.radians(3) ** 2]) + + R_inv = np.linalg.inv(R) + Q_inv = np.linalg.inv(Q) + + #information matrix and vector + Omega = np.zeros((total_dimensions, total_dimensions)) + xi = np.zeros(total_dimensions) + + # Anchor x_0: heavily penalize moving it, so delta_x0 stays ~0 and the + # whole trajectory/map is pinned to an absolute frame. + x0_idx = pose_slice(0) #returns slice object of pose at time 0 + Omega[x0_idx, x0_idx] += anchor_information * np.eye(3) + + + # Motion edges: x_{t-1} -> x_t via odometry_log[t] + for t in range(1, num_timesteps): + v, ang_v, dt = odometry_log[t] + x, y, theta = mu_poses[t - 1] + + predicted_pose = np.array([ + x + v * dt * math.cos(theta), + y + v * dt * math.sin(theta), + normalize_angle(theta + ang_v * dt), + ]) + + motion_jacobian = np.array([ + [1, 0, -v * dt * math.sin(theta)], + [0, 1, v * dt * math.cos(theta)], + [0, 0, 1], + ]) + + # residual = mu_poses[t] - predicted_pose; + # d(residual)/d[prev_pose, curr_pose] = [-motion_jacobian, I] + + residual_jacobian = np.hstack([-motion_jacobian, np.eye(3)]) + + residual = mu_poses[t] - predicted_pose + residual[2] = normalize_angle(residual[2]) + + + idx = np.r_[np.arange(3 * (t - 1), 3 * (t - 1) + 3), + np.arange(3 * t, 3 * t + 3)] + + Omega[np.ix_(idx, idx)] += residual_jacobian.T @ R_inv @ residual_jacobian + xi[idx] += -residual_jacobian.T @ R_inv @ residual + + # Measurement edges: x_t -> m_j via landmark_log[t][j] + for t, row in enumerate(landmark_log): + x, y, theta = mu_poses[t] + for j in range(num_landmarks): + r_obs, phi_obs = row[j] + if np.isnan(r_obs) or np.isnan(mu_landmarks[j, 0]): + continue + + mx, my = mu_landmarks[j] + dx, dy = mx - x, my - y + q = dx ** 2 + dy ** 2 + r_hat = math.sqrt(q) + phi_hat = normalize_angle(math.atan2(dy, dx) - theta) + + # H = d(h)/d[x, y, theta, mx, my], h = [r_hat, phi_hat] + H = np.array([ + [-dx / r_hat, -dy / r_hat, 0, dx / r_hat, dy / r_hat], + [dy / q, -dx / q, -1, -dy / q, dx / q], + ]) + + # residual r = z_obs - h(x); d(r)/d[...] = -H + residual = np.array([ + r_obs - r_hat, + normalize_angle(phi_obs - phi_hat), + ]) + + pose_idx = np.arange(3 * t, 3 * t + 3) + lm_idx = np.arange(3 * num_timesteps + 2 * j, 3 * num_timesteps + 2 * j + 2) + idx = np.r_[pose_idx, lm_idx] + + Omega[np.ix_(idx, idx)] += H.T @ Q_inv @ H + xi[idx] += H.T @ Q_inv @ residual + + return Omega, xi + + +def graphSLAM_reduce(Omega, xi, num_timesteps, num_landmarks): + """ + Eliminates the landmark variables from the full system via the Schur + complement, leaving a pose-only system: + + Omega_reduced = Omega_xx - sum_j Omega_xm_j @ inv(Omega_mm_j) @ Omega_mx_j + xi_reduced = xi_x - sum_j Omega_xm_j @ inv(Omega_mm_j) @ xi_m_j + + This works landmark-by-landmark (rather than inverting one big + Omega_mm block) because landmarks never connect to each other in the + graph -- only to the poses that observed them -- so Omega_mm is + block-diagonal and each landmark's 2x2 block can be eliminated on its + own. + + Landmarks that were never observed have an all-zero Omega_mm block + (no measurement edges ever touched them in linearize) and are skipped, + since that block can't be inverted and carries no information anyway. + + Parameters + ---------- + Omega : (3T + 2M, 3T + 2M) ndarray, from graphSLAM_linearize + xi : (3T + 2M,) ndarray, from graphSLAM_linearize + num_timesteps : T + num_landmarks : M + + Returns + ------- + Omega_reduced : (3T, 3T) ndarray, pose-only information matrix + xi_reduced : (3T,) ndarray, pose-only information vector + """ + pose_dim = 3 * num_timesteps + + Omega_reduced = Omega[:pose_dim, :pose_dim].copy() + xi_reduced = xi[:pose_dim].copy() + + for j in range(num_landmarks): + lm_idx = landmark_slice(j, num_timesteps) + + omega_mm_j = Omega[lm_idx, lm_idx] + if np.allclose(omega_mm_j, 0): + #never observed -- no information to eliminate + continue + + omega_mm_j_inv = np.linalg.inv(omega_mm_j) + + omega_xm_j = Omega[:pose_dim, lm_idx] #(3T, 2), sparse: only nonzero + #for poses that saw landmark j + xi_m_j = xi[lm_idx] #(2,) + + Omega_reduced -= omega_xm_j @ omega_mm_j_inv @ omega_xm_j.T + xi_reduced -= omega_xm_j @ omega_mm_j_inv @ xi_m_j + + return Omega_reduced, xi_reduced + + +def graphSLAM_solve(Omega, xi, Omega_reduced, xi_reduced, num_timesteps, num_landmarks): + """ + Solves the reduced pose-only system for the pose correction, then + back-substitutes each landmark's own block from the FULL (unreduced) + Omega/xi -- together with the now-known pose correction -- to recover + that landmark's correction: + + delta_m_j = inv(Omega_mm_j) @ (xi_m_j - Omega_mx_j @ delta_poses) + + Landmarks that were never observed (skipped during reduce, since their + Omega_mm block can't be inverted) get a correction of exactly 0 -- + there's no evidence to move them. + + Parameters + ---------- + Omega, xi : full system from graphSLAM_linearize + Omega_reduced, xi_reduced : pose-only system from graphSLAM_reduce + num_timesteps : T + num_landmarks : M + + Returns + ------- + delta_poses : (T, 3) ndarray, pose corrections + delta_landmarks : (M, 2) ndarray, landmark corrections + """ + pose_dim = 3 * num_timesteps + + delta_poses_flat = np.linalg.solve(Omega_reduced, xi_reduced) + delta_poses = delta_poses_flat.reshape(num_timesteps, 3) + + delta_landmarks = np.zeros((num_landmarks, 2)) + for j in range(num_landmarks): + lm_idx = landmark_slice(j, num_timesteps) + + omega_mm_j = Omega[lm_idx, lm_idx] + if np.allclose(omega_mm_j, 0): + #never observed -- leave its correction at 0 + continue + + omega_mm_j_inv = np.linalg.inv(omega_mm_j) + omega_mx_j = Omega[lm_idx, :pose_dim] #(2, 3T) + xi_m_j = xi[lm_idx] #(2,) + + delta_landmarks[j] = omega_mm_j_inv @ (xi_m_j - omega_mx_j @ delta_poses_flat) + + return delta_poses, delta_landmarks + + +def graphSLAM_run(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0), R=None, Q=None, + anchor_information=1e9, max_iterations=20, tolerance=1e-4, + verbose=False): + """ + Runs full offline GraphSLAM: init once, then repeatedly + linearize -> reduce -> solve -> apply the correction, until the + correction gets small (converged) or max_iterations is hit. + + Repeating is necessary because the motion/measurement models are + nonlinear -- one linearize/reduce/solve pass is only a Gauss-Newton + step around the CURRENT estimate, not an exact answer. Each pass + re-linearizes around the updated mu_poses/mu_landmarks from the + previous pass. + + Parameters + ---------- + odometry_log, landmark_log : see graphSLAM_init / graphSLAM_linearize + x0 : initial pose, (x, y, theta) + R, Q : motion / measurement noise covariances + anchor_information : information pinning x_0 in place + max_iterations : stop after this many passes even if not converged + tolerance : stop early once the largest correction (pose or + landmark, mixing meters and radians) drops + below this + verbose : print the largest correction each iteration + + Returns + ------- + mu_poses : (T, 3) final pose estimates + mu_landmarks : (M, 2) final landmark estimates + """ + mu_poses, mu_landmarks = graphSLAM_init(odometry_log, landmark_log, x0=x0) + num_timesteps = len(mu_poses) + num_landmarks = len(mu_landmarks) + + for iteration in range(max_iterations): + Omega, xi = graphSLAM_linearize( + mu_poses, mu_landmarks, odometry_log, landmark_log, + R=R, Q=Q, anchor_information=anchor_information, + ) + Omega_reduced, xi_reduced = graphSLAM_reduce(Omega, xi, num_timesteps, num_landmarks) + delta_poses, delta_landmarks = graphSLAM_solve( + Omega, xi, Omega_reduced, xi_reduced, num_timesteps, num_landmarks, + ) + + mu_poses = mu_poses + delta_poses + mu_poses[:, 2] = normalize_angle(mu_poses[:, 2]) + #NaN (never-observed) landmarks get a 0 delta, so NaN + 0 = NaN -- + #they stay unestimated rather than silently becoming (0, 0) + mu_landmarks = mu_landmarks + delta_landmarks + + max_delta = max(np.max(np.abs(delta_poses)), np.nanmax(np.abs(delta_landmarks))) + if verbose: + print(f"iteration {iteration}: max correction = {max_delta}") + + if max_delta < tolerance: + break + + return mu_poses, mu_landmarks + +#diagnostics below --------------------------------------------------------------- +def graphSLAM_accuracy(mu_poses, dead_reckoned_poses, truth_log): + """ + Compares GraphSLAM's optimized trajectory against dead reckoning alone, + both measured against ground truth, to answer "did the optimization + actually help, and by how much". + + truth_log entries are (x, y, theta_degrees) -- theta comes from + get_compass_reading(), which is DEGREES, while mu_poses/ + dead_reckoned_poses are in RADIANS -- so headings are converted before + comparing. + + Parameters + ---------- + mu_poses : (T, 3) ndarray, graphSLAM_run's final poses + dead_reckoned_poses : (T, 3) ndarray, graphSLAM_init's raw poses + (the "before" baseline, no landmark correction) + truth_log : list of (x, y, theta_degrees), same length as + mu_poses + + Returns + ------- + dict with mean/rmse/max position error (meters) and mean heading error + (degrees) for both mu_poses and dead_reckoned_poses, plus + improvement_pct: how much smaller graphSLAM's mean position error is + than dead reckoning's, as a percentage (100% = perfect, i.e. zero + error; 0% = no better than dead reckoning; negative = worse). + """ + truth = np.array(truth_log, dtype=np.float64) + truth_theta_rad = normalize_angle(np.radians(truth[:, 2])) + + def position_error(poses): + return np.hypot(poses[:, 0] - truth[:, 0], poses[:, 1] - truth[:, 1]) + + def heading_error_deg(poses): + diff = normalize_angle(poses[:, 2] - truth_theta_rad) + return np.degrees(np.abs(diff)) + + slam_pos_err = position_error(mu_poses) + dr_pos_err = position_error(dead_reckoned_poses) + + slam_mean = slam_pos_err.mean() + dr_mean = dr_pos_err.mean() + #100% = graphSLAM eliminated all position error relative to dead + #reckoning's error; 0% = no improvement; negative = graphSLAM made it worse + improvement_pct = 100.0 * (1.0 - slam_mean / dr_mean) if dr_mean > 0 else 0.0 + + return { + "slam_mean_position_error_m": slam_mean, + "slam_rmse_position_error_m": math.sqrt(np.mean(slam_pos_err ** 2)), + "slam_max_position_error_m": slam_pos_err.max(), + "slam_mean_heading_error_deg": heading_error_deg(mu_poses).mean(), + "dead_reckoning_mean_position_error_m": dr_mean, + "dead_reckoning_max_position_error_m": dr_pos_err.max(), + "improvement_over_dead_reckoning_pct": improvement_pct, + } + + +def print_graphSLAM_accuracy(mu_poses, dead_reckoned_poses, truth_log): + """Prints graphSLAM_accuracy's report in a readable form.""" + report = graphSLAM_accuracy(mu_poses, dead_reckoned_poses, truth_log) + print("GraphSLAM accuracy vs. ground truth") + print(f" dead reckoning mean position error: {report['dead_reckoning_mean_position_error_m']:.3f} m " + f"(max {report['dead_reckoning_max_position_error_m']:.3f} m)") + print(f" graphSLAM mean position error: {report['slam_mean_position_error_m']:.3f} m " + f"(rmse {report['slam_rmse_position_error_m']:.3f} m, max {report['slam_max_position_error_m']:.3f} m)") + print(f" graphSLAM mean heading error: {report['slam_mean_heading_error_deg']:.2f} deg") + print(f" improvement over dead reckoning: {report['improvement_over_dead_reckoning_pct']:.1f}%") + return report + + +def print_landmark_triangulation_check(mu_landmarks, true_landmark_positions, label=None): + """ + Compares landmark position estimates against the known true landmark + positions from the environment. Works with EITHER graphSLAM_init's raw + single-sighting estimates OR graphSLAM_run's fully optimized estimates + -- pass whichever mu_landmarks you want checked, and use `label` to say + which one it is so the printed output isn't ambiguous about which + stage produced it. + + Comparing the init-only version isolates get_landmark_row's egocentric + bearing + graphSLAM_init's theta + phi world-frame triangulation from + everything else in the pipeline (linearize/reduce/solve, R/Q + weighting). If those are already far off, the bug is in the bearing + convention or correspondence, not the optimization -- tightening Q + would only make things worse by trusting bad data harder. + + Parameters + ---------- + mu_landmarks : (M, 2) ndarray, NaN row = unobserved. From EITHER + graphSLAM_init or graphSLAM_run. + true_landmark_positions : (M, 2) array-like of true (x, y), same + indexing as mu_landmarks (e.g. + [(lm.x, lm.y) for lm in robot.maze.landmarks]) + label : str, optional -- describes which mu_landmarks this is (e.g. + "graphSLAM_init (no optimization)" or "graphSLAM_run (optimized)"). + Printed as-is so the output is unambiguous about its source. + """ + true_pos = np.array(true_landmark_positions, dtype=np.float64) + header = "Landmark triangulation check" + if label: + header += f" -- {label}" + print(header) + for j in range(len(mu_landmarks)): + if np.isnan(mu_landmarks[j, 0]): + print(f" landmark {j}: never observed") + continue + err = np.hypot(mu_landmarks[j, 0] - true_pos[j, 0], mu_landmarks[j, 1] - true_pos[j, 1]) + print(f" landmark {j}: estimated ({mu_landmarks[j,0]:.2f}, {mu_landmarks[j,1]:.2f}) " + f"true ({true_pos[j,0]:.2f}, {true_pos[j,1]:.2f}) error {err:.2f} m") + + +def print_landmark_observation_spread(mu_poses, landmark_log): + """ + For each landmark, reports how many times it was observed and how + wide a range of viewing angles it was observed from (world-frame + bearing from the robot's current pose estimate to the landmark). + + A landmark seen only a few times from a narrow angular window has + poor triangulation geometry -- there's little parallax to pin down + its position confidently -- no matter how tightly Q trusts each + individual sighting. This is meant to be read alongside + print_landmark_triangulation_check: landmarks with large triangulation + error AND a narrow/sparse observation spread here are explained by + poor geometry, not a bug. + """ + n_landmarks = landmark_log[0].shape[0] + print("Landmark observation spread") + for j in range(n_landmarks): + bearings = [] + ranges = [] + for t, row in enumerate(landmark_log): + r_obs, phi_obs = row[j] + if np.isnan(r_obs): + continue + x, y, theta = mu_poses[t] + bearings.append(normalize_angle(theta + phi_obs)) + ranges.append(r_obs) + + if not bearings: + print(f" landmark {j}: never observed") + continue + + bearings = np.array(bearings) + #circular mean, so spread doesn't get corrupted by the -180/180 wrap + mean_bearing = math.atan2(np.mean(np.sin(bearings)), np.mean(np.cos(bearings))) + deviations = np.array([normalize_angle(b - mean_bearing) for b in bearings]) + spread_deg = math.degrees(deviations.max() - deviations.min()) + + print(f" landmark {j}: {len(bearings)} sightings, " + f"viewing-angle spread {spread_deg:.1f} deg, " + f"range {min(ranges):.2f}-{max(ranges):.2f} m") + + +def print_worst_pose_errors(mu_poses, truth_log, odometry_log, top_n=10): + """ + Reports the top_n timesteps with the largest position error between + mu_poses and truth_log, alongside the odometry_log entry that produced + the transition INTO that pose (odometry_log[t], connecting x_{t-1} to + x_t). Useful for checking whether the worst pose errors line up with + odometry outliers (e.g. encoder-derived velocity spikes) rather than + being spread evenly across the run -- a concentrated pattern points at + a few untrustworthy motion edges being over-trusted by R, not a + systematic bug in the optimization itself. + """ + truth = np.array(truth_log, dtype=np.float64) + pos_err = np.hypot(mu_poses[:, 0] - truth[:, 0], mu_poses[:, 1] - truth[:, 1]) + worst = np.argsort(pos_err)[::-1][:top_n] + + print(f"Worst {top_n} pose errors") + for t in worst: + v, omega, dt = odometry_log[t] + print(f" t={t}: error {pos_err[t]:.3f} m | " + f"odometry_log[t] = (v={v:.3f}, omega={omega:.3f}, dt={dt:.3f})") diff --git a/realm_tools/robot_lib/my_robot.py b/realm_tools/robot_lib/my_robot.py index 525f04e..64d5157 100644 --- a/realm_tools/robot_lib/my_robot.py +++ b/realm_tools/robot_lib/my_robot.py @@ -1,5 +1,456 @@ +from concurrent.futures import ThreadPoolExecutor from realm_tools.robot_lib.hambot import HamBot - +from realm_tools.image_lib.image_feature_lib import * +from realm_tools.robot_lib.navigation_tools import * +import operator +import numpy as np +import math class MyRobot(HamBot): - def __init__(self): + def __init__(self,action_length=0.5,enable_cnn_features=False,cnn_extractor_model=None): HamBot.__init__(self) + self.action_length = action_length + self.action_set = { + 0: [0, self.action_length], + 1: [45, self.action_length], + 2: [90, self.action_length], + 3: [135, self.action_length], + 4: [180, self.action_length], + 5: [225, self.action_length], + 6: [270, self.action_length], + 7: [315, self.action_length], + } + + # Experiment Variables + self.previous_action_index = -1 + + # Enable CNN-based feature extraction if required + self.enable_cnn_features = enable_cnn_features + self.cnn_extractor_model = cnn_extractor_model + if self.enable_cnn_features: + from realm_tools.image_lib.feature_extractor import FeatureExtractor + self.cnn_feature_extractor = FeatureExtractor(self.cnn_extractor_model) + else: + self.cnn_feature_extractor = None + + def get_robot_feature_pose(self): + while self.experiment_supervisor.step(self.timestep) != -1: + current_x, current_y, current_z = self.robot_translation_field.getSFVec3f() + break + return current_x, current_y, self.get_closest_action_heading() + + def get_robot_pov_features(self): + """ + Get combined feature vector including CNN features, multimodal features, and robot pose. + + Returns: + np.ndarray: Combined feature vector suitable for clustering. + """ + pov, landmark_mask, landmark_azimuths = self.get_pov_image() + x, y, theta = self.get_robot_pose() + + # Extract CNN features if enabled + if self.enable_cnn_features: + cnn_features = self.cnn_feature_extractor.get_cnn_features(pov) + else: + cnn_features = np.array([]) # Empty array if CNN features are not enabled + + multimodal_features = extract_combined_features(pov, landmark_mask, theta) + + return multimodal_features, cnn_features, landmark_mask, landmark_azimuths + + def capture_pov_images(self, thetas): + """ + Capture one image per heading without extracting features. + Use this for batch collection — accumulate images across many positions + then extract features in parallel in the controller. + Caller must have already teleported to the target position. + + Returns + ------- + images : list of POV images, one per heading + landmark_masks : list of (n_landmarks,) arrays — visibility per heading + landmark_azimuths : list of (n_landmarks,) arrays — egocentric bearing (rad) + to each landmark, NaN where not visible + """ + images = [] + landmark_masks = [] + landmark_azimuths = [] + for theta in thetas: + self.robot_rotation_field.setSFRotation([0, 0, 1, theta]) + pov, mask, azimuths = self.get_pov_image() + images.append(pov) + landmark_masks.append(mask) + landmark_azimuths.append(azimuths) + return images, landmark_masks, landmark_azimuths + + def get_full_robot_pov_features(self, thetas): + robot_x, robot_y, robot_theta = self.get_robot_feature_pose() + + # Phase 1 — capture all images sequentially (requires simulation steps) + images = [] + for theta in thetas: + self.robot_rotation_field.setSFRotation([0, 0, 1, theta]) + images.append(self.get_pov_image()[0]) + + # Phase 2 — extract features in parallel (pure CPU, no Webots dependency) + with ThreadPoolExecutor() as executor: + multimodal_features = list(executor.map(extract_combined_features, images)) + if self.enable_cnn_features: + cnn_features = list(executor.map( + self.cnn_feature_extractor.get_cnn_features, images)) + else: + cnn_features = None + + return np.concatenate(multimodal_features), cnn_features + + def get_pov_image(self): + while self.experiment_supervisor.step(self.timestep) != -1: + # getImage() returns raw BGRA bytes — much faster than getImageArray() + # which returns a Python list of lists that must be converted to numpy. + w = self.camera.getWidth() + h = self.camera.getHeight() + img_bgra = np.frombuffer(self.camera.getImage(), dtype=np.uint8).reshape(h, w, 4) + # Make contiguous RGB array — .copy() ensures cv2 operations downstream + # don't pay a stride penalty from the reversed-channel view. + pov = np.ascontiguousarray(img_bgra[:, :, 2::-1]) + + landmark_mask, landmark_azimuths = self.get_landmark_observations() + return pov, landmark_mask, landmark_azimuths + + def get_landmark_observations(self): + """ + Determine which landmarks (from self.maze.landmarks) are visible in the + current camera frame, and the egocentric bearing to each. + + Each landmark is identified by its unique recognitionColors. Bearings come + from CameraRecognitionObject.getPosition(), which Webots already reports in + the camera's own coordinate frame (x = forward, y = lateral) — so atan2(y, x) + is the angle relative to the robot's current heading, with no global-frame + math needed. + + Returns + ------- + landmark_mask : np.ndarray, shape (n_landmarks,) — 1 if visible, else 0 + landmark_azimuths : np.ndarray, shape (n_landmarks,) — egocentric bearing + (radians) to each landmark, NaN where not visible + """ + n_landmarks = len(self.maze.landmarks) + landmark_mask = np.zeros(n_landmarks, dtype=np.float32) + landmark_azimuths = np.full(n_landmarks, np.nan, dtype=np.float32) + color_to_id = {tuple(round(c, 2) for c in lm.color): lm.id for lm in self.maze.landmarks} + + for obj in self.camera.getRecognitionObjects(): + color = tuple(round(c, 2) for c in obj.getColors()[:3]) + landmark_id = color_to_id.get(color) + if landmark_id is None: + continue + rel_x, rel_y, rel_z = obj.getPosition() + landmark_mask[landmark_id] = 1 + landmark_azimuths[landmark_id] = math.atan2(rel_y, rel_x) + + return landmark_mask, landmark_azimuths + def get_closest_action_index(self): + return int((self.get_compass_reading() // 45)) + + def get_closest_action_heading(self): + """ + Rounds the given heading (in degrees) to the nearest increment of 45 degrees. + Handles headings in the range [0, 360] and ensures 360 is treated as 0. + + Parameters: + - heading: float or int, the heading in degrees (e.g., 314, 316, 5, 360, 355). + + Returns: + - int: The closest heading that is a multiple of 45 (0, 45, 90, 135, 180, 225, 270, 315). + """ + # Normalize heading to [0, 360) + heading = self.get_compass_reading() % 360 + + # Divide by 45, round to nearest integer, and multiply back by 45 + closest_heading = round(heading / 45) * 45 + + # Ensure 360 is returned as 0 + if closest_heading == 360: + closest_heading = 0 + + return closest_heading + + # Calculates the vector needed to move the robot to the point (x,y) + def calculate_robot_motion_vector(self, x, y): + self.sensor_calibration() + current_position = self.gps.getValues()[0:2] + return calculate_motion_vector(current_position[0], current_position[1], x, y) + + # Caps the motor velocities to ensure PID calculations do no exceed motor speeds + def velocity_saturation(self, motor_velocity): + if motor_velocity > self.max_motor_velocity: + return self.max_motor_velocity + elif motor_velocity < -1 * self.max_motor_velocity: + return -1 * self.max_motor_velocity + else: + return motor_velocity + + # Sets motor speeds using PID to turn robot to desired bearing + def rotation_PID(self, end_bearing, K_p=1): + delta = end_bearing - self.get_compass_reading() + velocity = self.velocity_saturation(K_p * abs(delta)) + if -180 <= delta <= 0 or 180 < delta <= 360: + self.left_motor.setVelocity(1 * velocity) + self.right_motor.setVelocity(-1 * velocity) + + elif 0 < delta <= 180 or -360 <= delta < -180: + self.left_motor.setVelocity(-1 * velocity) + self.right_motor.setVelocity(1 * velocity) + + # Sets motor speeds using PID to move the robot forward a desired distance in mm + def forward_motion_with_encoder_PID(self, travel_distance, starting_encoder_position, K_p=100): + delta = travel_distance - self.calculate_wheel_distance_traveled(starting_encoder_position) + velocity = self.velocity_saturation(K_p * delta) + for motor in self.all_motors: + motor.setVelocity(velocity) + + # Sets the motor speeds using PID to move the robot to the point (x,y) + def forward_motion_with_xy_PID(self, x, y, K_p=10): + current_position = self.gps.getValues()[0:2] + delta_x = x - current_position[0] + delta_y = y - current_position[1] + delta = (delta_x + delta_y) / 2 + velocity = self.velocity_saturation(K_p * delta) + for motor in self.all_motors: + motor.setVelocity(velocity) + + # Rotates the robot in place to face end_bearing and stops within margin_error (DEFAULT: +-.001) + def rotate_to(self, end_bearing, margin_error=.0001): + counter = 0 + while self.experiment_supervisor.step(self.timestep) != -1: + self.rotation_PID(end_bearing) + counter += 1 + if end_bearing - margin_error <= self.get_compass_reading() <= end_bearing + margin_error: + self.stop() + break + + # Moves the robot forward in a straight line by the amount distance (in mm) + def move_forward_with_PID(self, distance, margin_error=.01, safty=True): + forward_lidar_window = 15 + starting_encoder_position = self.get_encoder_readings() + while self.experiment_supervisor.step(self.timestep) != -1: + self.forward_motion_with_encoder_PID(distance, starting_encoder_position) + if (distance - margin_error <= + self.calculate_wheel_distance_traveled(starting_encoder_position) <= distance + margin_error): + self.stop() + break + if safty: + if (min(self.lidar.getRangeImage()[180 - forward_lidar_window:180 + forward_lidar_window]) < .4): + self.stop() + break + + # Moves the robot forward in a straight line by the amount distance (in mm) + def move_forward_no_PID(self, distance, velocity=20, margin_error=.01): + forward_lidar_window = 15 + starting_encoder_position = self.get_encoder_readings() + while self.experiment_supervisor.step(self.timestep) != -1: + self.go_forward(velocity=velocity) + if (distance - margin_error <= + self.calculate_wheel_distance_traveled(starting_encoder_position) <= distance + margin_error): + self.stop() + break + if (min(self.lidar.getRangeImage()[180 - forward_lidar_window:180 + forward_lidar_window]) < .25): + self.stop() + break + + # Moves the robot to the point (x,y) by rotating and then moving in a straight line + def move_to_xy_with_PID(self, x, y, margin_error=.1): + motion_vector = self.calculate_robot_motion_vector(x, y) + if not (motion_vector[0] - margin_error <= + self.get_compass_reading() <= + motion_vector[0] + margin_error): + self.rotate_to(motion_vector[0]) + motion_vector = self.calculate_robot_motion_vector(x, y) + self.move_forward_with_PID(motion_vector[1]) + + # Moves the robot to the point (x,y) by rotating and then moving in a straight line + def move_to_xy_no_PID(self, x, y, velocity=20, margin_error=.01): + motion_vector = self.calculate_robot_motion_vector(x, y) + print(motion_vector) + if not (motion_vector[0] - margin_error <= + self.get_compass_reading() <= + motion_vector[0] + margin_error): + self.rotate_to(motion_vector[0]) + + self.move_forward_no_PID(motion_vector[1]) + + def slow_stop(self): + while self.experiment_supervisor.step(self.timestep) != -1: + current_velocity = self.left_motor.getVelocity() + if current_velocity > .1: + for motor in self.all_motors: + motor.setVelocity(current_velocity / 4) + else: + self.stop() + break + + def rotate(self, degrees = 90, Kp=1, Ki=0, Kd=0, margin_error = 0.01): + I = 0.0 + prev_error = 0.0 + dt = 0.032 + + # degrees > 0 -> ccw + # degrees < 0 -> cw + + setpoint = (self.get_compass_reading() + degrees) % 360 + + while self.experiment_supervisor.step(self.timestep) != -1: + current_heading = self.get_compass_reading() + error = (setpoint - current_heading+180)%360 -180 # (-180,180) + P = Kp * error + + I = Ki * (I + error * dt) + + D = Kd * ((error - prev_error) / dt) + + out_signal = abs(self.sat(P + I + D)) + if -margin_error <= error <= margin_error: + self.stop() + break + elif error < 0: + self.set_right_motor_velocity(-out_signal) + self.set_left_motor_velocity(out_signal) + elif error > 0: + self.set_right_motor_velocity(out_signal) + self.set_left_motor_velocity(-out_signal) + + prev_error = error + def calculate_wheel_distance_traveled(self, starting_encoder_position): + current_encoder_readings = self.get_encoder_readings() + differences = list(map(operator.sub, current_encoder_readings, starting_encoder_position)) + average_differences = sum(differences) / len(differences) + average_distance = average_differences * self.wheel_radius + return average_distance + def move_forward(self, distance, Kp=20, margin_error=0.01): + starting_encoder_position = self.get_encoder_readings() + while self.experiment_supervisor.step(self.timestep) != -1: + error = distance - self.calculate_wheel_distance_traveled(starting_encoder_position) + if error <= margin_error: + self.stop() + break + self.go_forward(velocity=self.sat(Kp * error)) + + def perform_random_action(self, bias=True): + + available_actions = [int(i) for i in self.get_possible_actions()] + # Add motion Bias and normalize + if bias: + action_distribution = apply_softmax(add_motion_bias(available_actions, self.previous_action_index)) + else: + action_distribution = apply_softmax(available_actions) + random_action_index = np.random.choice(8, 1, p=action_distribution)[0] + + if self.check_if_action_is_possible(random_action_index): + random_action = self.action_set.get(random_action_index) + self.rotate_to(random_action[0]) + self.move_forward_with_PID(random_action[1]) + else: + random_action_index = np.argmax(action_distribution) + random_action = self.action_set.get(random_action_index) + self.rotate_to(random_action[0]) + self.move_forward_with_PID(random_action[1]) + + self.previous_action_index = random_action_index + return random_action_index + + def perform_habituation_action(self, bias=True): + self.sensor_calibration() + available_actions = [int(i) for i in self.get_possible_actions()] + # Add motion Bias and normalize + if bias: + action_distribution = apply_softmax(add_motion_bias(available_actions, self.previous_action_index)) + else: + action_distribution = apply_softmax(available_actions) + random_action_index = np.random.choice(8, 1, p=action_distribution)[0] + if self.check_if_action_is_possible(random_action_index, teleport=True): + self.perform_training_action_teleport(random_action_index) + else: + # Try the remaining actions sorted by probability + sorted_indices = np.argsort(action_distribution)[::-1] # descending order + for idx in sorted_indices: + if self.check_if_action_is_possible(idx, teleport=True): + self.perform_training_action_teleport(idx) + random_action_index = idx + break + + self.previous_action_index = random_action_index + return random_action_index + + def perform_action_with_PID(self, action_index): + action = self.action_set.get(action_index) + if self.check_if_action_is_possible(action_index=action_index): + self.rotate_to(action[0]) + self.move_forward_with_PID(action[1]) + else: + print("cant preform action") + + def perform_action_no_PID(self, action_index): + action = self.action_set.get(action_index) + if self.check_if_action_is_possible(action_index=action_index): + self.rotate_to(action[0]) + self.move_forward_no_PID(500 * action[1]) + else: + print("cant preform action") + + def perform_training_action(self, action_index): + action = self.action_set.get(action_index) + self.rotate_to(action[0]) + self.move_forward_with_PID(action[1]) + + def perform_training_action_teleport(self, action_index): + action = self.action_set.get(action_index) + curr_x, curr_y, curr_theta = self.get_robot_pose() + action_theta = math.radians(action[0]) + new_x = curr_x + self.action_length * math.cos(action_theta) + new_y = curr_y + self.action_length * math.sin(action_theta) + self.teleport_robot(x=new_x, y=new_y, theta=action_theta) + return self.get_robot_pose() + + def get_possible_actions(self): + min_action_distance = self.action_length + 0.25 + while self.experiment_supervisor.step(self.timestep) != -1: + relative_distances = RelativeDistances(lidar_range_image=self.lidar.getRangeImage()) + available_actions = [0] * 8 + bin_index = 0 + front_action_index = self.get_closest_action_index() + for bin in relative_distances.distance_bins: + action_index = (front_action_index - bin_index) % 8 + available_actions[action_index] = min(bin) > min_action_distance + bin_index += 1 + + return available_actions + + def get_possible_training_actions(self): + available_actions = self.get_possible_actions() + return [i for i in range(len(available_actions)) if available_actions[i]] + + def get_possible_training_action_mask(self): + available_actions = np.array(self.get_possible_actions()) + return np.array(np.multiply(available_actions, 1), dtype=np.float32) + + def check_if_action_is_possible(self, action_index=-1, teleport=False): + forward_lidar_window = 45 + min_action_distance = .5 + if action_index == -1: + if min(self.lidar.getRangeImage()[ + 180 - forward_lidar_window:180 + forward_lidar_window]) > min_action_distance: + return True + else: + return False + else: + action = self.action_set.get(action_index) + if teleport: + curr_x, curr_y, curr_theta = self.get_robot_pose() + self.teleport_robot(curr_x, curr_y, theta=math.radians(action[0])) + else: + self.rotate_to(action[0]) + if min(self.lidar.getRangeImage()[ + 180 - forward_lidar_window:180 + forward_lidar_window]) > min_action_distance: + return True + else: + return False diff --git a/realm_tools/robot_lib/navigation_tools.py b/realm_tools/robot_lib/navigation_tools.py new file mode 100644 index 0000000..806bdac --- /dev/null +++ b/realm_tools/robot_lib/navigation_tools.py @@ -0,0 +1,85 @@ +import math +import numpy as np +from scipy.special import softmax +class RelativeDistances: + def __init__(self, lidar_range_image, window=22): + self.window = window + self.lidar_range_image = lidar_range_image + self.bin_indices = {} + + def get_range(center): + start = (center - window) % 360 + end = (center + window) % 360 + self.bin_indices[center] = (start, end) + if start <= end: + return lidar_range_image[start:end + 1] + else: + return lidar_range_image[start:] + lidar_range_image[:end + 1] + + # Define directional bins centered at 45° increments + self.front_distances = get_range(180) + self.front_right_distances = get_range(225) + self.right_distances = get_range(270) + self.rear_right_distances = get_range(315) + self.rear_distances = get_range(0) + self.rear_left_distances = get_range(45) + self.left_distances = get_range(90) + self.front_left_distances = get_range(135) + + # Clockwise ordering from front + self.distance_bins = [ + self.front_distances, + self.front_right_distances, + self.right_distances, + self.rear_right_distances, + self.rear_distances, + self.rear_left_distances, + self.left_distances, + self.front_left_distances + ] + + def __str__(self): + label_map = { + 180: "Front", + 225: "Front Right", + 270: "Right", + 315: "Rear Right", + 0: "Rear", + 45: "Rear Left", + 90: "Left", + 135: "Front Left" + } + output = ["Distance Bin Ranges (index-based):"] + for center in [180, 225, 270, 315, 0, 45, 90, 135]: + start, end = self.bin_indices[center] + output.append(f"{label_map[center]:<13}: start = {start}, end = {end}") + return "\n".join(output) + + +# Function to calculate the angle and distance between two points (x1,y1) and (x2,y2) +def calculate_motion_vector(x1, y1, x2, y2): + theta = int(math.degrees(math.atan2((y2 - y1), (x2 - x1)))) + if theta < 0: + theta += 360 + magnitude = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + return np.array([theta, magnitude]) + +def add_motion_bias(available_actions, previous_action_index): + if previous_action_index == -1: + return available_actions + else: + action_distribution = [i for i in available_actions] + # Previous action bias + action_distribution[previous_action_index] += 3 + # Adjacent action bias + action_distribution[(previous_action_index - 1) % 8] += 2 + action_distribution[(previous_action_index + 1) % 8] += 2 + # Orthogonal action bias + action_distribution[(previous_action_index - 2) % 8] += 1 + action_distribution[(previous_action_index + 2) % 8] += 1 + # Eliminate not available actions + action_distribution = np.multiply(action_distribution, available_actions) + return action_distribution + +def apply_softmax(action_distrabution): + return softmax(action_distrabution) \ No newline at end of file diff --git a/simulation/controllers/graphSLAM/graphSLAM.py b/simulation/controllers/graphSLAM/graphSLAM.py new file mode 100644 index 0000000..6fe9a69 --- /dev/null +++ b/simulation/controllers/graphSLAM/graphSLAM.py @@ -0,0 +1,95 @@ +import os + +from realm_tools.robot_lib.graph_slam_calculations import * +os.chdir("../../..") + + +from realm_tools.robot_lib.my_robot import MyRobot +from realm_tools.robot_lib.graphSLAM_data_collection import * + +robot = MyRobot() +robot.load_environment('simulation/worlds/environments/samples/octagon.xml') +#starting position +robot.teleport_robot(0,0,0, 0) + +# initialize (t=0) +action_gen = None +action_phase = 'rotate' +cycles_completed = 0 +max_cycles = 5 + +landmark_log = [] +odometry_log = [] +truth_log = [] + +prev_encoder_readings = robot.get_encoder_readings() + +t=0 + +while robot.experiment_supervisor.step(robot.timestep) != -1: + + # measure (distance to landmarks and odometry) + ''' + finds landmarks and returns relative distance to them + using get_landmark_observations() + ''' + + #landmark collection + landmark_scan = get_landmark_row(robot) + landmark_log.append(landmark_scan) + + #odometry collection + dt = robot.timestep / 1000.0 # ms to seconds + + v, omega, prev_encoder_readings = get_odometry(robot,prev_encoder_readings, dt) + odometry_log.append((v, omega, dt)) + + #ground truth collection + x, y, _ = robot.robot_translation_field.getSFVec3f() + theta = robot.get_compass_reading() + truth_log.append((x, y, theta)) + + + # move () + # Start a new generator if none is currently running + if action_gen is None: + if action_phase == 'rotate': + action_gen = rotate_step(robot, 72) + elif action_phase == 'move': + action_gen = move_forward_step(robot, .5) + + try: + next(action_gen) + except StopIteration: + action_gen = None + if action_phase == 'rotate': + action_phase = 'move' + else: + action_phase = 'rotate' + cycles_completed += 1 + if cycles_completed >= max_cycles: + break # exit the main loop after 5 full rotate+move cycles + + print(f"Completed {cycles_completed} cycles, {len(landmark_log)} timesteps recorded") + + + +#graphSLAM calculations + +true_landmark_positions = [(lm.x, lm.y) for lm in robot.maze.landmarks] +mu_poses, mu_landmarks = graphSLAM_init(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0)) +print_landmark_triangulation_check(mu_landmarks, true_landmark_positions) + +print_landmark_observation_spread(mu_poses, landmark_log) # mu_poses from graphSLAM_init +mu_poses_final, mu_landmarks_final = graphSLAM_run(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0)) +print_landmark_triangulation_check(mu_landmarks_final, true_landmark_positions) +dead_reckoned_poses, _ = graphSLAM_init(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0)) +print_graphSLAM_accuracy(mu_poses_final, dead_reckoned_poses, truth_log) + +print_worst_pose_errors(mu_poses_final, truth_log, odometry_log, top_n=10) + +mu_poses_optimized, mu_landmarks_optimized = graphSLAM_run(odometry_log, landmark_log, x0=(0.0, 0.0, 0.0)) +print_landmark_triangulation_check(mu_landmarks_optimized, true_landmark_positions) +print_landmark_observation_spread(mu_poses_optimized, landmark_log) + +