From cc64f3609d83a3c4f87e956e7617a297b9cc45b9 Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Mon, 12 Nov 2018 00:18:27 +0100
Subject: [PATCH] circular regression using scipy minimize
---
.../catkin_ws/src/car/scripts/motion.py | 28 ++++++++++---------
src/odroid/catkin_ws/src/car/scripts/test.py | 2 +-
2 files changed, 16 insertions(+), 14 deletions(-)
diff --git a/src/odroid/catkin_ws/src/car/scripts/motion.py b/src/odroid/catkin_ws/src/car/scripts/motion.py
index 0421be4..eb9a558 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 3482abc..3bce07c 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()
--
GitLab