diff --git a/src/odroid/catkin_ws/src/car/scripts/motion.py b/src/odroid/catkin_ws/src/car/scripts/motion.py index 0421be4762e7bcb46765d2ec1f3e229401315c08..eb9a55858c8e7a547278218d6a8627d939f692fe 100644 --- a/src/odroid/catkin_ws/src/car/scripts/motion.py +++ b/src/odroid/catkin_ws/src/car/scripts/motion.py @@ -2,7 +2,7 @@ import numpy as np import odometry from scipy.spatial.distance import euclidean -from scipy.optimize import minimize, Bounds +from scipy.optimize import minimize import datetime from parameters import * @@ -21,7 +21,7 @@ def max_speed_before_slipping(turn_radius, static_friction_coef): return 1.0 -def circular_regression(coords, angles, wheelbase): +def circular_regression_brute_force(coords, angles, wheelbase): min_err = float("inf") final_angle = 0 for angle in angles: @@ -38,17 +38,19 @@ def circular_regression(coords, angles, wheelbase): final_angle = angle return final_angle - # def err(r, coords): - # e = 0 - # for c in coords: - # e += (np.sqrt((c[0] - r) ** 2 + c[1] ** 2) - abs(r)) ** 2 - # return e / len(coords) - - # min_radius = angle_to_turn_radius(np.deg2rad(-30), WHEEL_BASE) - # max_radius = angle_to_turn_radius(np.deg2rad(30), WHEEL_BASE) - # bounds = Bounds(min_radius, max_radius) - # result = minimize(err, 0, (coords), bounds=bounds, method="L-BFGS-B", options={"maxiter": 50}) - # return turn_radius_to_angle(result["x"]) + +def circular_regression(coords, angles, wheelbase): + def err(angle, coords): + if angle == 0: + return np.mean([c[0]**2 for c in coords]) + r = angle_to_turn_radius(angle, WHEEL_BASE) + return np.mean([(np.sqrt((c[0] - r) ** 2 + c[1] ** 2) - abs(r)) ** 2 for c in coords]) + + min_angle = np.deg2rad(-30) + max_angle = np.deg2rad(30) + + result = minimize(err, 0, (coords), bounds=[(min_angle, max_angle)], method="SLSQP", options={"maxiter": 50}) + return result["x"] def compute_turns(coords, wheel_base): diff --git a/src/odroid/catkin_ws/src/car/scripts/test.py b/src/odroid/catkin_ws/src/car/scripts/test.py index 3482abc0e986b7d30a9c9d42d43c4a81950f1c18..3bce07cc23c8aad5b3583e56a95c8e7d78d10e88 100644 --- a/src/odroid/catkin_ws/src/car/scripts/test.py +++ b/src/odroid/catkin_ws/src/car/scripts/test.py @@ -310,4 +310,4 @@ def fit_angle_to_radius(): if __name__ == "__main__": - odometry_calibration() + odometry_prediction()