From 3024f033479214fabeb16f08feae531df6227cc5 Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Wed, 7 Oct 2020 10:25:35 -0700
Subject: [PATCH 1/7] implemented lyapunov based controller as well

---
 PathTracking/move_to_pose/move_to_pose.py | 121 ++++++++++++++++++++--
 1 file changed, 114 insertions(+), 7 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 2ae0090dda..fccb7c78b0 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -21,8 +21,105 @@
 
 show_animation = True
 
+class PolarDynamics:
+    def f(self, x):
+        return np.zeros_like(x)
+
+    def g(self, x):
+        rho, alpha, beta = x
+        return (np.array([[-np.cos(alpha), 0],
+                          [np.sin(alpha)/rho, -1],
+                          [-np.sin(alpha)/rho, 0]])
+                if rho > 1e-6 else
+                np.array([[-1, 0],
+                          [1, -1],
+                          [-1, 0]]))
+
+class CartesianDynamics:
+    def f(self, x):
+        return np.zeros_like(x)
+
+    def g(self, state):
+        x, y, theta = state
+        return np.array([[np.cos(theta), 0],
+                         [np.sin(theta), 0],
+                         [0, 1]])
+
+
+def angdiff(thetap, theta):
+    return ((thetap - theta) + np.pi) % (2 * np.pi) - np.pi
+
+def cosdiff(thetap, theta):
+    up = np.array([np.cos(thetap), np.sin(thetap)])
+    u = np.array([np.cos(theta), np.sin(theta)])
+    return 1 - up @ up
+
+class ControllerCLF:
+    """
+    Aicardi, M., Casalino, G., Bicchi, A., & Balestrino, A. (1995). Closed loop steering of unicycle like vehicles via Lyapunov techniques. IEEE Robotics & Automation Magazine, 2(1), 27-35.
+    """
+    def __init__(self, # simulation parameters
+                 Kp = [1, 2, 1],
+                 u_dim = 2,
+                 dynamics = PolarDynamics()):
+        self.Kp = np.asarray(Kp)
+        self.u_dim = 2
+        self.dynamics = dynamics
+
+    def _clf(self, x, u):
+        return 0.5 * (self.Kp @ (x*x))
+
+    def _grad_clf(self, x, u):
+        return self.Kp * x
+
+    def _clc(self, x, u):
+        f, g = self.dynamics.f, self.dynamics.g
+        return self._grad_clf(x, u) @ (f(x) + g(x) @ u) + self._clf(x, u)
+
+    def _cost(self, x, u):
+        import cvxpy as cp # pip install cvxpy
+        return cp.sum_squares(u)
+
+    def control(self, x, t):
+        import cvxpy as cp # pip install cvxpy
+        x = np.asarray(x)
+        uvar = cp.Variable(self.u_dim)
+        uvar.value = np.zeros(self.u_dim)
+        relax = cp.Variable(1)
+        obj = cp.Minimize(self._cost(x, uvar) + 100 * relax**2)
+        constr = (self._clc(x, uvar) + relax <= 0)
+        problem = cp.Problem(obj, [constr])
+        problem.solve(solver='GUROBI')
+        if problem.status not in ["infeasible", "unbounded"]:
+            # Otherwise, problem.value is inf or -inf, respectively.
+            # print("Optimal value: %s" % problem.value)
+            pass
+        else:
+            raise ValueError(problem.status)
+        # for variable in problem.variables():
+        #     print("Variable %s: value %s" % (variable.name(), variable.value))
+        [v,w] = uvar.value
+        return v,w
+
+
+class ControllerPID:
+    def __init__(self, # simulation parameters
+                 Kp_rho = 9,
+                 Kp_alpha = 15,
+                 Kp_beta = -3):
+        self.Kp_rho = Kp_rho
+        self.Kp_alpha = Kp_alpha
+        self.Kp_beta = Kp_beta
+
+    def control(self, x, t):
+        rho, alpha, beta = x
+        v = Kp_rho * rho
+        w = Kp_alpha * alpha + Kp_beta * beta
+        return [v, w]
 
-def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
+
+def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
+                 controller=ControllerCLF()):
     """
     rho is the distance between the robot and the goal position
     alpha is the angle to the goal relative to the heading of the robot
@@ -41,6 +138,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     x_traj, y_traj = [], []
 
     rho = np.hypot(x_diff, y_diff)
+    count = 0
     while rho > 0.001:
         x_traj.append(x)
         y_traj.append(y)
@@ -52,21 +150,24 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
         # [-pi, pi] to prevent unstable behavior e.g. difference going
         # from 0 rad to 2*pi rad with slight turn
 
+        # reparameterization
         rho = np.hypot(x_diff, y_diff)
-        alpha = (np.arctan2(y_diff, x_diff)
-                 - theta + np.pi) % (2 * np.pi) - np.pi
-        beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
+        phi = np.arctan2(y_diff, x_diff)
+        alpha = angdiff(phi - theta)
+        beta = angdiff(theta_goal - phi)
 
-        v = Kp_rho * rho
-        w = Kp_alpha * alpha + Kp_beta * beta
+        # control
+        v, w = controller.control([rho, alpha, beta], t=count)
 
         if alpha > np.pi / 2 or alpha < -np.pi / 2:
             v = -v
 
+        # simulation
         theta = theta + w * dt
         x = x + v * np.cos(theta) * dt
         y = y + v * np.sin(theta) * dt
 
+        # visualization
         if show_animation:  # pragma: no cover
             plt.cla()
             plt.arrow(x_start, y_start, np.cos(theta_start),
@@ -74,6 +175,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
             plt.arrow(x_goal, y_goal, np.cos(theta_goal),
                       np.sin(theta_goal), color='g', width=0.1)
             plot_vehicle(x, y, theta, x_traj, y_traj)
+        count = count + 1
 
 
 def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
@@ -113,6 +215,10 @@ def transformation_matrix(x, y, theta):
 
 def main():
 
+    move_to_pose_configured = partial(move_to_pose,
+                                      controller=ControllerCLF(
+                                          dynamics=PolarDynamics()
+                                      ))
     for i in range(5):
         x_start = 20 * random()
         y_start = 20 * random()
@@ -124,7 +230,8 @@ def main():
               (x_start, y_start, theta_start))
         print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
               (x_goal, y_goal, theta_goal))
-        move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+        move_to_pose_configured(x_start, y_start, theta_start, x_goal, y_goal,
+                                theta_goal)
 
 
 if __name__ == '__main__':

From 56c2dc15a6930b3f9ea37de341f3cad4b6b23bdc Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Wed, 7 Oct 2020 10:26:00 -0700
Subject: [PATCH 2/7] implemented lyapunov based controller as well

---
 PathTracking/move_to_pose/move_to_pose.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index fccb7c78b0..9941dc12b3 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -12,6 +12,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 from random import random
+from functools import partial
 
 # simulation parameters
 Kp_rho = 9

From 3ab68117a199288cedbadae0a48f058a389ac792 Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Wed, 7 Oct 2020 13:56:24 -0700
Subject: [PATCH 3/7] alpha - beta should be the same is more important thhan
 beta being zero

---
 PathTracking/move_to_pose/move_to_pose.py | 122 +++++++++++++++-------
 1 file changed, 83 insertions(+), 39 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 9941dc12b3..6d6fc9bceb 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -13,6 +13,7 @@
 import numpy as np
 from random import random
 from functools import partial
+from collections import namedtuple
 
 # simulation parameters
 Kp_rho = 9
@@ -22,60 +23,114 @@
 
 show_animation = True
 
+
+PolarState = np.ndarray
+CartesianState = np.ndarray
+
+
+def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianState:
+    rho, alpha, beta = x
+    x_goal, y_goal, theta_goal = state_goal
+    phi = angdiff(theta_goal, beta)
+    x_diff = rho * np.cos(phi)
+    y_diff = rho * np.sin(phi)
+    theta = normalize_angle(alpha + phi)
+    return np.array([x_goal - x_diff,
+                     y_goal - y_diff,
+                     theta])
+
+
+def cartesian2polar(state: CartesianState, state_goal : CartesianState) -> PolarState:
+    x, y, theta = state
+    x_goal, y_goal, theta_goal = state_goal
+
+    x_diff = x_goal - x
+    y_diff = y_goal - y
+
+    # reparameterization
+    rho = np.hypot(x_diff, y_diff)
+    phi = np.arctan2(y_diff, x_diff)
+    alpha = angdiff(phi , theta)
+    beta = angdiff(theta_goal , phi)
+    return np.array((rho, alpha, beta))
+
+
+
 class PolarDynamics:
-    def f(self, x):
+    def f(self, x : PolarState):
         return np.zeros_like(x)
 
-    def g(self, x):
+    def g(self, x : PolarState):
         rho, alpha, beta = x
         return (np.array([[-np.cos(alpha), 0],
                           [np.sin(alpha)/rho, -1],
                           [-np.sin(alpha)/rho, 0]])
-                if rho > 1e-6 else
+                if (rho > 1e-2) else
                 np.array([[-1, 0],
                           [1, -1],
                           [-1, 0]]))
 
 class CartesianDynamics:
-    def f(self, x):
+    def f(self, x : CartesianState):
         return np.zeros_like(x)
 
-    def g(self, state):
+    def g(self, state: CartesianState):
         x, y, theta = state
         return np.array([[np.cos(theta), 0],
                          [np.sin(theta), 0],
                          [0, 1]])
 
 
+def normalize_angle(theta):
+    # Restrict alpha and beta (angle differences) to the range
+    # [-pi, pi] to prevent unstable behavior e.g. difference going
+    # from 0 rad to 2*pi rad with slight turn
+    return (theta + np.pi) % (2 * np.pi) - np.pi
+
 def angdiff(thetap, theta):
-    return ((thetap - theta) + np.pi) % (2 * np.pi) - np.pi
+    return normalize_angle(thetap - theta)
+
+
+def cosdist(thetap, theta):
+    return 1 - np.cos(thetap - theta)
 
-def cosdiff(thetap, theta):
-    up = np.array([np.cos(thetap), np.sin(thetap)])
-    u = np.array([np.cos(theta), np.sin(theta)])
-    return 1 - up @ up
 
 class ControllerCLF:
     """
     Aicardi, M., Casalino, G., Bicchi, A., & Balestrino, A. (1995). Closed loop steering of unicycle like vehicles via Lyapunov techniques. IEEE Robotics & Automation Magazine, 2(1), 27-35.
     """
     def __init__(self, # simulation parameters
-                 Kp = [1, 2, 1],
+                 Kp = np.array([9, 15, 5])/10.,
                  u_dim = 2,
                  dynamics = PolarDynamics()):
         self.Kp = np.asarray(Kp)
         self.u_dim = 2
         self.dynamics = dynamics
 
-    def _clf(self, x, u):
-        return 0.5 * (self.Kp @ (x*x))
+    def _clf_terms(self, x):
+        rho, alpha, beta = x
+        return np.array((0.5 * self.Kp[0] * rho ** 2,
+                         self.Kp[1] * cosdist(alpha, 0),
+                         self.Kp[2] * cosdist(alpha, beta)
+        ))
 
-    def _grad_clf(self, x, u):
-        return self.Kp * x
+    def _clf(self, x):
+        return self._clf_terms(x).sum()
+
+    def _grad_clf(self, x):
+        rho, alpha, beta = x
+        return np.array((self.Kp[0] * rho ,
+                         self.Kp[1] * np.sin(alpha) + self.Kp[2] * np.sin(alpha - beta) ,
+                         - self.Kp[2] * np.sin(alpha - beta)))
 
     def _clc(self, x, u):
         f, g = self.dynamics.f, self.dynamics.g
-        return self._grad_clf(x, u) @ (f(x) + g(x) @ u) + self._clf(x, u)
+        gclf = self._grad_clf(x)
+        print("x :", x)
+        print("clf terms :", self._clf_terms(x))
+        print("grad_x clf:", gclf)
+        print("grad_u clf:", gclf @ g(x))
+        return gclf @ (f(x) + g(x) @ u) + 10*self._clf(x)
 
     def _cost(self, x, u):
         import cvxpy as cp # pip install cvxpy
@@ -87,9 +142,9 @@ def control(self, x, t):
         uvar = cp.Variable(self.u_dim)
         uvar.value = np.zeros(self.u_dim)
         relax = cp.Variable(1)
-        obj = cp.Minimize(self._cost(x, uvar) + 100 * relax**2)
-        constr = (self._clc(x, uvar) + relax <= 0)
-        problem = cp.Problem(obj, [constr])
+        obj = cp.Minimize(self._cost(x, uvar) + 10*self._clc(x, uvar))
+        #constr = (self._clc(x, uvar) + relax <= 0)
+        problem = cp.Problem(obj)#, [constr])
         problem.solve(solver='GUROBI')
         if problem.status not in ["infeasible", "unbounded"]:
             # Otherwise, problem.value is inf or -inf, respectively.
@@ -99,8 +154,7 @@ def control(self, x, t):
             raise ValueError(problem.status)
         # for variable in problem.variables():
         #     print("Variable %s: value %s" % (variable.name(), variable.value))
-        [v,w] = uvar.value
-        return v,w
+        return uvar.value
 
 
 class ControllerPID:
@@ -116,6 +170,8 @@ def control(self, x, t):
         rho, alpha, beta = x
         v = Kp_rho * rho
         w = Kp_alpha * alpha + Kp_beta * beta
+        if alpha > np.pi / 2 or alpha < -np.pi / 2:
+            v = -v
         return [v, w]
 
 
@@ -144,24 +200,11 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
         x_traj.append(x)
         y_traj.append(y)
 
-        x_diff = x_goal - x
-        y_diff = y_goal - y
-
-        # Restrict alpha and beta (angle differences) to the range
-        # [-pi, pi] to prevent unstable behavior e.g. difference going
-        # from 0 rad to 2*pi rad with slight turn
-
-        # reparameterization
-        rho = np.hypot(x_diff, y_diff)
-        phi = np.arctan2(y_diff, x_diff)
-        alpha = angdiff(phi - theta)
-        beta = angdiff(theta_goal - phi)
-
+        state = cartesian2polar(np.array([x, y, theta]),
+                                np.array([x_goal, y_goal, theta_goal]))
+        rho, alpha, beta = state
         # control
-        v, w = controller.control([rho, alpha, beta], t=count)
-
-        if alpha > np.pi / 2 or alpha < -np.pi / 2:
-            v = -v
+        v, w = controller.control(state, t=count)
 
         # simulation
         theta = theta + w * dt
@@ -215,11 +258,12 @@ def transformation_matrix(x, y, theta):
 
 
 def main():
-
     move_to_pose_configured = partial(move_to_pose,
                                       controller=ControllerCLF(
                                           dynamics=PolarDynamics()
                                       ))
+    # move_to_pose_configured = partial(move_to_pose,
+    #                                   controller=ControllerPID())
     for i in range(5):
         x_start = 20 * random()
         y_start = 20 * random()

From 67ac9f1cc9ac7d8972bedc95625988a630fbab4a Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Thu, 8 Oct 2020 13:00:38 -0700
Subject: [PATCH 4/7] gradients are all right

---
 PathTracking/move_to_pose/move_to_pose.py | 246 +++++++++++++++++-----
 1 file changed, 189 insertions(+), 57 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 6d6fc9bceb..69df7f9854 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -14,21 +14,17 @@
 from random import random
 from functools import partial
 from collections import namedtuple
+import sys
 
 # simulation parameters
-Kp_rho = 9
-Kp_alpha = 15
-Kp_beta = -3
-dt = 0.01
 
-show_animation = True
-
-
-PolarState = np.ndarray
-CartesianState = np.ndarray
+PolarState = namedtuple('PolarState', 'rho alpha beta')
+CartesianState = namedtuple('CartesianState', 'x y theta')
 
 
 def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianState:
+    """
+    """
     rho, alpha, beta = x
     x_goal, y_goal, theta_goal = state_goal
     phi = angdiff(theta_goal, beta)
@@ -65,7 +61,7 @@ def g(self, x : PolarState):
         return (np.array([[-np.cos(alpha), 0],
                           [np.sin(alpha)/rho, -1],
                           [-np.sin(alpha)/rho, 0]])
-                if (rho > 1e-2) else
+                if (rho > 1e-6) else
                 np.array([[-1, 0],
                           [1, -1],
                           [-1, 0]]))
@@ -95,54 +91,161 @@ def cosdist(thetap, theta):
     return 1 - np.cos(thetap - theta)
 
 
+class CLFPolar:
+    def __init__(self,
+                 Kp = np.array([9, 15, 5, 5])/10.):
+        self.Kp = np.asarray(Kp)
+
+    def clf_terms(self, x, x_goal):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        return self._clf_terms(rho, alpha,beta)
+
+    def _clf_terms(self, rho, alpha, beta):
+        return np.array((0.5 * self.Kp[0] * rho ** 2,
+                         self.Kp[1] * (1-np.cos(alpha)),
+                         self.Kp[2] * (1-np.cos(beta)),
+                         self.Kp[3] * (1-np.cos(alpha - beta))
+        ))
+
+    def grad_clf(self, x, x_goal):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        return self._grad_clf(rho, alpha, beta)
+
+    def _grad_clf(self, rho, alpha, beta):
+        """
+        >>> self = CLFPolar()
+        >>> x0 = np.random.rand(3)
+        >>> ajac = self._grad_clf(*x0)
+        >>> njac = numerical_jac(lambda x: self._clf_terms(*x).sum(), x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        """
+        return np.array((self.Kp[0] * rho ,
+                         self.Kp[1] * np.sin(alpha)
+                         +  self.Kp[3] * np.sin(alpha - beta) ,
+                         self.Kp[2] * np.sin(beta)
+                         - self.Kp[3] * np.sin(alpha - beta)))
+
+    def isconverged(self, x, x_goal):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        return rho < 1e-3
+
+
+def numerical_jac(func, x0, eps):
+    """
+    >>> def func(x): return np.array([np.cos(x[0]), np.sin(x[1])])
+    >>> def jacfunc(x): return np.array([[-np.sin(x[0]), 0], [0, np.cos(x[1])]])
+    >>> x0 = np.random.rand(2)
+    >>> njac = numerical_jac(func, x0, 1e-6)
+    >>> ajac = jacfunc(x0)
+    >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+    """
+    f0 = func(x0)
+    m = 1 if np.isscalar(f0) else f0.shape[-1]
+    jac = np.empty((m, x0.shape[-1]))
+    Dx = eps * np.eye(x0.shape[-1])
+    XpDx = x0 + Dx
+    for c in range(x0.shape[-1]):
+        jac[:, c:c+1] = (func(XpDx[c, :]).reshape(-1, 1) - f0.reshape(-1, 1)) / eps
+
+    return jac
+
+
+class CLFCartesian:
+    def __init__(self,
+                 Kp = np.array([9, 15, 5])/10.):
+        self.Kp = np.asarray(Kp)
+
+    def clf_terms(self, state, state_goal):
+        rho, alpha, beta = cartesian2polar(state, state_goal)
+        x,y, theta = state
+        x_goal, y_goal, theta_goal = state_goal
+        return np.array((0.5 * self.Kp[0] * rho ** 2,
+                         self.Kp[1] * cosdist(alpha, 0),
+                         self.Kp[2] * cosdist(theta_goal, theta)
+        ))
+
+    def _grad_clf_terms(self, state, state_goal):
+        """
+        >>> self = CLFCartesian()
+        >>> x0 = np.random.rand(3)
+        >>> x0_goal = np.random.rand(3)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 0]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[0], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 1]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[1], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 2]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[2], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        """
+        x_diff, y_diff, theta_diff = state_goal - state
+        rho, alpha, beta = cartesian2polar(state, state_goal)
+        return np.array([[- self.Kp[0] * x_diff,
+                          self.Kp[1] * np.sin(alpha) * y_diff / (rho**2),
+                          0],
+                         [- self.Kp[0] * y_diff,
+                          - self.Kp[1] * np.sin(alpha) * x_diff / (rho**2),
+                         0],
+                         [0,
+                          -self.Kp[1] * np.sin(alpha),
+                          - self.Kp[2] * np.sin(theta_diff)]
+                         ])
+    def grad_clf(self, state, state_goal):
+        """
+        >>> self = CLFCartesian()
+        >>> x0 = np.random.rand(3)
+        >>> x0_goal = np.random.rand(3)
+        >>> ajac = self.grad_clf(x0, x0_goal)
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal).sum(), x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        """
+        return self._grad_clf_terms(state, state_goal).sum(axis=-1)
+
+    def isconverged(self, x, x_goal):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        return rho < 1e-3
+
+
 class ControllerCLF:
     """
     Aicardi, M., Casalino, G., Bicchi, A., & Balestrino, A. (1995). Closed loop steering of unicycle like vehicles via Lyapunov techniques. IEEE Robotics & Automation Magazine, 2(1), 27-35.
     """
     def __init__(self, # simulation parameters
-                 Kp = np.array([9, 15, 5])/10.,
                  u_dim = 2,
-                 dynamics = PolarDynamics()):
-        self.Kp = np.asarray(Kp)
+                 dynamics = PolarDynamics(),
+                 clf = CLFPolar()):
         self.u_dim = 2
         self.dynamics = dynamics
+        self.clf = clf
 
-    def _clf_terms(self, x):
-        rho, alpha, beta = x
-        return np.array((0.5 * self.Kp[0] * rho ** 2,
-                         self.Kp[1] * cosdist(alpha, 0),
-                         self.Kp[2] * cosdist(alpha, beta)
-        ))
-
-    def _clf(self, x):
-        return self._clf_terms(x).sum()
+    def _clf(self, x, x_goal):
+        return self.clf.clf_terms(x, x_goal).sum()
 
-    def _grad_clf(self, x):
-        rho, alpha, beta = x
-        return np.array((self.Kp[0] * rho ,
-                         self.Kp[1] * np.sin(alpha) + self.Kp[2] * np.sin(alpha - beta) ,
-                         - self.Kp[2] * np.sin(alpha - beta)))
+    def _grad_clf(self, x, x_goal):
+        return self.clf.grad_clf(x, x_goal)
 
-    def _clc(self, x, u):
+    def _clc(self, x, x_goal, u):
         f, g = self.dynamics.f, self.dynamics.g
-        gclf = self._grad_clf(x)
+        gclf = self._grad_clf(x, x_goal)
         print("x :", x)
-        print("clf terms :", self._clf_terms(x))
+        print("clf terms :", self.clf.clf_terms(x, x_goal))
         print("grad_x clf:", gclf)
+        print("g(x): ", g(x))
         print("grad_u clf:", gclf @ g(x))
-        return gclf @ (f(x) + g(x) @ u) + 10*self._clf(x)
+        return gclf @ (f(x) + g(x) @ u) + 10*self._clf(x, x_goal)
 
     def _cost(self, x, u):
         import cvxpy as cp # pip install cvxpy
         return cp.sum_squares(u)
 
-    def control(self, x, t):
+    def control(self, x, x_goal, t):
         import cvxpy as cp # pip install cvxpy
         x = np.asarray(x)
         uvar = cp.Variable(self.u_dim)
         uvar.value = np.zeros(self.u_dim)
         relax = cp.Variable(1)
-        obj = cp.Minimize(self._cost(x, uvar) + 10*self._clc(x, uvar))
+        obj = cp.Minimize(self._cost(x, uvar) + 10*self._clc(x, x_goal, uvar))
         #constr = (self._clc(x, uvar) + relax <= 0)
         problem = cp.Problem(obj)#, [constr])
         problem.solve(solver='GUROBI')
@@ -157,6 +260,10 @@ def control(self, x, t):
         return uvar.value
 
 
+    def isconverged(self, state, state_goal):
+        return self.clf.isconverged(state, state_goal)
+
+
 class ControllerPID:
     def __init__(self, # simulation parameters
                  Kp_rho = 9,
@@ -166,16 +273,25 @@ def __init__(self, # simulation parameters
         self.Kp_alpha = Kp_alpha
         self.Kp_beta = Kp_beta
 
-    def control(self, x, t):
-        rho, alpha, beta = x
+    def control(self, x, x_goal, t):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        Kp_rho   = self.Kp_rho
+        Kp_alpha = self.Kp_alpha
+        Kp_beta  = self.Kp_beta
         v = Kp_rho * rho
         w = Kp_alpha * alpha + Kp_beta * beta
         if alpha > np.pi / 2 or alpha < -np.pi / 2:
             v = -v
         return [v, w]
 
+    def isconverged(self, x, x_goal):
+        rho, alpha, beta = cartesian2polar(x, x_goal)
+        return rho < 1e-3
+
 
 def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
+                 dt = 0.01,
+                 show_animation = True,
                  controller=ControllerCLF()):
     """
     rho is the distance between the robot and the goal position
@@ -189,28 +305,25 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
     y = y_start
     theta = theta_start
 
-    x_diff = x_goal - x
-    y_diff = y_goal - y
-
     x_traj, y_traj = [], []
 
-    rho = np.hypot(x_diff, y_diff)
+    state = np.array([x, y, theta])
+    state_goal = np.array([x_goal, y_goal, theta_goal])
     count = 0
-    while rho > 0.001:
+    while not controller.isconverged(state, state_goal):
         x_traj.append(x)
         y_traj.append(y)
 
-        state = cartesian2polar(np.array([x, y, theta]),
-                                np.array([x_goal, y_goal, theta_goal]))
-        rho, alpha, beta = state
         # control
-        v, w = controller.control(state, t=count)
+        v, w = controller.control(state, state_goal, t=count)
 
         # simulation
         theta = theta + w * dt
         x = x + v * np.cos(theta) * dt
         y = y + v * np.sin(theta) * dt
 
+        state = np.array([x, y, theta])
+
         # visualization
         if show_animation:  # pragma: no cover
             plt.cla()
@@ -218,11 +331,11 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
                       np.sin(theta_start), color='r', width=0.1)
             plt.arrow(x_goal, y_goal, np.cos(theta_goal),
                       np.sin(theta_goal), color='g', width=0.1)
-            plot_vehicle(x, y, theta, x_traj, y_traj)
+            plot_vehicle(x, y, theta, x_traj, y_traj, dt)
         count = count + 1
 
 
-def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
+def plot_vehicle(x, y, theta, x_traj, y_traj, dt):  # pragma: no cover
     # Corners of triangular vehicle when pointing to the right (0 radians)
     p1_i = np.array([0.5, 0, 1]).T
     p2_i = np.array([-0.5, 0.25, 1]).T
@@ -241,10 +354,10 @@ def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
 
     # for stopping simulation with the esc key.
     plt.gcf().canvas.mpl_connect('key_release_event',
-            lambda event: [exit(0) if event.key == 'escape' else None])
+            lambda event: [sys.exit(0) if event.key == 'escape' else None])
 
-    plt.xlim(0, 20)
-    plt.ylim(0, 20)
+    plt.xlim(-2, 22)
+    plt.ylim(-2, 22)
 
     plt.pause(dt)
 
@@ -257,13 +370,30 @@ def transformation_matrix(x, y, theta):
     ])
 
 
-def main():
-    move_to_pose_configured = partial(move_to_pose,
+class Configs:
+    @property
+    def clf_polar(self):
+        return dict(simulator=partial(move_to_pose,
+                                      controller=ControllerCLF(
+                                          dynamics=PolarDynamics(),
+                                          clf = CLFPolar()
+                                      )))
+
+    @property
+    def clf_cartesian(self):
+        return dict(simulator=partial(move_to_pose,
                                       controller=ControllerCLF(
-                                          dynamics=PolarDynamics()
-                                      ))
-    # move_to_pose_configured = partial(move_to_pose,
-    #                                   controller=ControllerPID())
+                                          dynamics=CartesianDynamics(),
+                                          clf = CLFCartesian()
+                                      )))
+
+    @property
+    def pid(self):
+        return dict(simulator=partial(move_to_pose,
+                                      controller=ControllerPID()))
+
+
+def main(simulator = move_to_pose):
     for i in range(5):
         x_start = 20 * random()
         y_start = 20 * random()
@@ -275,9 +405,11 @@ def main():
               (x_start, y_start, theta_start))
         print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
               (x_goal, y_goal, theta_goal))
-        move_to_pose_configured(x_start, y_start, theta_start, x_goal, y_goal,
+        simulator(x_start, y_start, theta_start, x_goal, y_goal,
                                 theta_goal)
 
 
 if __name__ == '__main__':
-    main()
+    import doctest
+    doctest.testmod()
+    #main(**getattr(Configs(), 'clf_polar')) # 'pid', 'clf_polar' or 'clf_cartesian'

From 3a0ec0dae522ea80df846e3993ef0a751ae9918e Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Thu, 8 Oct 2020 15:06:18 -0700
Subject: [PATCH 5/7] conversion is correct

---
 PathTracking/move_to_pose/move_to_pose.py | 51 ++++++++++++++++-------
 1 file changed, 37 insertions(+), 14 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 69df7f9854..bf3ba8d92a 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -8,6 +8,7 @@
 P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
 
 """
+from datetime import datetime
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -15,28 +16,51 @@
 from functools import partial
 from collections import namedtuple
 import sys
+from torch.utils.tensorboard import SummaryWriter
 
 # simulation parameters
 
 PolarState = namedtuple('PolarState', 'rho alpha beta')
 CartesianState = namedtuple('CartesianState', 'x y theta')
 
+LOG = SummaryWriter('data/runs/' + datetime.now().strftime("%m%d-%H%M"))
+
 
 def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianState:
     """
+    rho is the distance between the robot and the goal position
+    alpha is the angle to the goal relative to the heading of the robot
+    beta is the angle between the robot's position and the goal position plus the goal angle
+
+    >>> polar = np.random.rand(3) * np.array([1, 2*np.pi, 2*np.pi]) - np.array([0, np.pi, np.pi])
+    >>> state_goal = np.random.rand(3) * np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
+    >>> state = polar2cartesian(polar, state_goal)
+    >>> polarp = cartesian2polar(state, state_goal)
+    >>> np.testing.assert_allclose(polar, polarp)
     """
     rho, alpha, beta = x
     x_goal, y_goal, theta_goal = state_goal
     phi = angdiff(theta_goal, beta)
     x_diff = rho * np.cos(phi)
     y_diff = rho * np.sin(phi)
-    theta = normalize_angle(alpha + phi)
+    theta = angdiff(phi, alpha)
     return np.array([x_goal - x_diff,
                      y_goal - y_diff,
                      theta])
 
 
 def cartesian2polar(state: CartesianState, state_goal : CartesianState) -> PolarState:
+    """
+    rho is the distance between the robot and the goal position
+    alpha is the angle to the goal relative to the heading of the robot
+    beta is the angle between the robot's position and the goal position plus the goal angle
+
+    >>> state = np.random.rand(3)* np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
+    >>> state_goal = np.random.rand(3)* np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
+    >>> polar = cartesian2polar(state, state_goal)
+    >>> statep = polar2cartesian(polar, state_goal)
+    >>> np.testing.assert_allclose(state, statep)
+    """
     x, y, theta = state
     x_goal, y_goal, theta_goal = state_goal
 
@@ -93,7 +117,7 @@ def cosdist(thetap, theta):
 
 class CLFPolar:
     def __init__(self,
-                 Kp = np.array([9, 15, 5, 5])/10.):
+                 Kp = np.array([9, 15, 5])/10.):
         self.Kp = np.asarray(Kp)
 
     def clf_terms(self, x, x_goal):
@@ -103,27 +127,25 @@ def clf_terms(self, x, x_goal):
     def _clf_terms(self, rho, alpha, beta):
         return np.array((0.5 * self.Kp[0] * rho ** 2,
                          self.Kp[1] * (1-np.cos(alpha)),
-                         self.Kp[2] * (1-np.cos(beta)),
-                         self.Kp[3] * (1-np.cos(alpha - beta))
+                         self.Kp[2] * (1-np.cos(alpha - beta))
         ))
 
     def grad_clf(self, x, x_goal):
         rho, alpha, beta = cartesian2polar(x, x_goal)
-        return self._grad_clf(rho, alpha, beta)
+        return self._grad_clf_terms(rho, alpha, beta).sum(axis=-1)
 
-    def _grad_clf(self, rho, alpha, beta):
+    def _grad_clf_terms(self, rho, alpha, beta):
         """
         >>> self = CLFPolar()
         >>> x0 = np.random.rand(3)
-        >>> ajac = self._grad_clf(*x0)
+        >>> ajac = self._grad_clf_terms(*x0).sum(axis=-1)
         >>> njac = numerical_jac(lambda x: self._clf_terms(*x).sum(), x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         """
-        return np.array((self.Kp[0] * rho ,
-                         self.Kp[1] * np.sin(alpha)
-                         +  self.Kp[3] * np.sin(alpha - beta) ,
-                         self.Kp[2] * np.sin(beta)
-                         - self.Kp[3] * np.sin(alpha - beta)))
+        return np.array([[self.Kp[0] * rho,  0, 0],
+                         [0, self.Kp[1] * np.sin(alpha),
+                          self.Kp[2] * np.sin(alpha - beta)] ,
+                         [0,  0, - self.Kp[2] * np.sin(alpha - beta)]])
 
     def isconverged(self, x, x_goal):
         rho, alpha, beta = cartesian2polar(x, x_goal)
@@ -225,9 +247,10 @@ def _clf(self, x, x_goal):
     def _grad_clf(self, x, x_goal):
         return self.clf.grad_clf(x, x_goal)
 
-    def _clc(self, x, x_goal, u):
+    def _clc(self, x, x_goal, u, t):
         f, g = self.dynamics.f, self.dynamics.g
         gclf = self._grad_clf(x, x_goal)
+        LOG.add_scalar("x_0", x[0], t)
         print("x :", x)
         print("clf terms :", self.clf.clf_terms(x, x_goal))
         print("grad_x clf:", gclf)
@@ -245,7 +268,7 @@ def control(self, x, x_goal, t):
         uvar = cp.Variable(self.u_dim)
         uvar.value = np.zeros(self.u_dim)
         relax = cp.Variable(1)
-        obj = cp.Minimize(self._cost(x, uvar) + 10*self._clc(x, x_goal, uvar))
+        obj = cp.Minimize(self._cost(x, uvar) + 10*self._clc(x, x_goal, uvar, t))
         #constr = (self._clc(x, uvar) + relax <= 0)
         problem = cp.Problem(obj)#, [constr])
         problem.solve(solver='GUROBI')

From 852a1012a08ee6b04da02c0f4b811475825a2416 Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Fri, 9 Oct 2020 09:44:05 -0700
Subject: [PATCH 6/7] polar coordinate controller works.

---
 PathTracking/move_to_pose/move_to_pose.py | 99 ++++++++++++++---------
 1 file changed, 59 insertions(+), 40 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index bf3ba8d92a..1c49e13322 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -90,6 +90,18 @@ def g(self, x : PolarState):
                           [1, -1],
                           [-1, 0]]))
 
+class CartesianDynamicsWrapper:
+    def __init__(self, x_goal, polar_dynamics = PolarDynamics()):
+        self.x_goal = x_goal
+        self.polar_dynamics = polar_dynamics
+
+    def f(self, x):
+        return polar2cartesian(self.polar_dynamics.f(cartesian2polar(x, self.x_goal)))
+
+    def g(self, x):
+        return polar2cartesian(self.polar_dynamics.g(cartesian2polar(x, self.x_goal)))
+
+
 class CartesianDynamics:
     def f(self, x : CartesianState):
         return np.zeros_like(x)
@@ -120,28 +132,28 @@ def __init__(self,
                  Kp = np.array([9, 15, 5])/10.):
         self.Kp = np.asarray(Kp)
 
-    def clf_terms(self, x, x_goal):
-        rho, alpha, beta = cartesian2polar(x, x_goal)
-        return self._clf_terms(rho, alpha,beta)
+    def clf_terms(self, polar):
+        return self._clf_terms(polar)
 
-    def _clf_terms(self, rho, alpha, beta):
+    def _clf_terms(self, polar):
+        rho, alpha, beta = polar
         return np.array((0.5 * self.Kp[0] * rho ** 2,
                          self.Kp[1] * (1-np.cos(alpha)),
                          self.Kp[2] * (1-np.cos(alpha - beta))
         ))
 
-    def grad_clf(self, x, x_goal):
-        rho, alpha, beta = cartesian2polar(x, x_goal)
-        return self._grad_clf_terms(rho, alpha, beta).sum(axis=-1)
+    def grad_clf(self, polar):
+        return self._grad_clf_terms(polar).sum(axis=-1)
 
-    def _grad_clf_terms(self, rho, alpha, beta):
+    def _grad_clf_terms(self, polar):
         """
         >>> self = CLFPolar()
         >>> x0 = np.random.rand(3)
-        >>> ajac = self._grad_clf_terms(*x0).sum(axis=-1)
-        >>> njac = numerical_jac(lambda x: self._clf_terms(*x).sum(), x0, 1e-6)[0]
+        >>> ajac = self._grad_clf_terms(x0).sum(axis=-1)
+        >>> njac = numerical_jac(lambda x: self._clf_terms(x).sum(), x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         """
+        rho, alpha, beta = polar
         return np.array([[self.Kp[0] * rho,  0, 0],
                          [0, self.Kp[1] * np.sin(alpha),
                           self.Kp[2] * np.sin(alpha - beta)] ,
@@ -235,28 +247,32 @@ class ControllerCLF:
     """
     def __init__(self, # simulation parameters
                  u_dim = 2,
+                 coordinate_converter = cartesian2polar,
                  dynamics = PolarDynamics(),
                  clf = CLFPolar()):
         self.u_dim = 2
+        self.coordinate_converter = coordinate_converter
         self.dynamics = dynamics
         self.clf = clf
 
-    def _clf(self, x, x_goal):
-        return self.clf.clf_terms(x, x_goal).sum()
+    def _clf(self, polar):
+        return self.clf.clf_terms(polar).sum()
 
-    def _grad_clf(self, x, x_goal):
-        return self.clf.grad_clf(x, x_goal)
+    def _grad_clf(self, polar):
+        return self.clf.grad_clf(polar)
 
     def _clc(self, x, x_goal, u, t):
+        polar = self.coordinate_converter(x, x_goal)
         f, g = self.dynamics.f, self.dynamics.g
-        gclf = self._grad_clf(x, x_goal)
+        gclf = self._grad_clf(polar)
         LOG.add_scalar("x_0", x[0], t)
         print("x :", x)
-        print("clf terms :", self.clf.clf_terms(x, x_goal))
+        print("clf terms :", self.clf.clf_terms(polar))
+        print("clf:", self.clf.clf_terms(polar).sum())
         print("grad_x clf:", gclf)
-        print("g(x): ", g(x))
-        print("grad_u clf:", gclf @ g(x))
-        return gclf @ (f(x) + g(x) @ u) + 10*self._clf(x, x_goal)
+        print("g(x): ", g(polar))
+        print("grad_u clf:", gclf @ g(polar))
+        return gclf @ (f(polar) + g(polar) @ u) + 10 * self._clf(polar)
 
     def _cost(self, x, u):
         import cvxpy as cp # pip install cvxpy
@@ -312,10 +328,11 @@ def isconverged(self, x, x_goal):
         return rho < 1e-3
 
 
-def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
+def move_to_pose(state_start, state_goal,
                  dt = 0.01,
                  show_animation = True,
-                 controller=ControllerCLF()):
+                 controller=ControllerCLF(),
+                 dynamics=CartesianDynamics()):
     """
     rho is the distance between the robot and the goal position
     alpha is the angle to the goal relative to the heading of the robot
@@ -324,34 +341,29 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal,
     Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal
     Kp_beta*beta rotates the line so that it is parallel to the goal angle
     """
-    x = x_start
-    y = y_start
-    theta = theta_start
 
     x_traj, y_traj = [], []
 
-    state = np.array([x, y, theta])
-    state_goal = np.array([x_goal, y_goal, theta_goal])
+    state = state_start.copy()
     count = 0
     while not controller.isconverged(state, state_goal):
+        x, y, theta = state
         x_traj.append(x)
         y_traj.append(y)
 
         # control
-        v, w = controller.control(state, state_goal, t=count)
+        ctrl = controller.control(state, state_goal, t=count)
 
         # simulation
-        theta = theta + w * dt
-        x = x + v * np.cos(theta) * dt
-        y = y + v * np.sin(theta) * dt
-
-        state = np.array([x, y, theta])
+        state = state + (dynamics.f(state) + dynamics.g(state) @ ctrl) * dt
 
         # visualization
         if show_animation:  # pragma: no cover
             plt.cla()
+            x_start, y_start, theta_start = state_start
             plt.arrow(x_start, y_start, np.cos(theta_start),
                       np.sin(theta_start), color='r', width=0.1)
+            x_goal, y_goal, theta_goal = state_goal
             plt.arrow(x_goal, y_goal, np.cos(theta_goal),
                       np.sin(theta_goal), color='g', width=0.1)
             plot_vehicle(x, y, theta, x_traj, y_traj, dt)
@@ -396,15 +408,21 @@ def transformation_matrix(x, y, theta):
 class Configs:
     @property
     def clf_polar(self):
-        return dict(simulator=partial(move_to_pose,
-                                      controller=ControllerCLF(
-                                          dynamics=PolarDynamics(),
-                                          clf = CLFPolar()
-                                      )))
+        return dict(simulator=partial(
+            lambda x, x_g, **kw: move_to_pose(
+                x , x_g,
+                dynamics=CartesianDynamics(),
+                **kw),
+            controller=ControllerCLF(
+                coordinate_converter = cartesian2polar,
+                dynamics=PolarDynamics(),
+                clf = CLFPolar()
+            )))
 
     @property
     def clf_cartesian(self):
         return dict(simulator=partial(move_to_pose,
+                                      dynamics=CartesianDynamics(),
                                       controller=ControllerCLF(
                                           dynamics=CartesianDynamics(),
                                           clf = CLFCartesian()
@@ -413,6 +431,7 @@ def clf_cartesian(self):
     @property
     def pid(self):
         return dict(simulator=partial(move_to_pose,
+                                      dynamics=CartesianDynamics(),
                                       controller=ControllerPID()))
 
 
@@ -428,11 +447,11 @@ def main(simulator = move_to_pose):
               (x_start, y_start, theta_start))
         print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
               (x_goal, y_goal, theta_goal))
-        simulator(x_start, y_start, theta_start, x_goal, y_goal,
-                                theta_goal)
+        simulator(np.array([x_start, y_start, theta_start]),
+                  np.array([x_goal, y_goal, theta_goal]))
 
 
 if __name__ == '__main__':
     import doctest
     doctest.testmod()
-    #main(**getattr(Configs(), 'clf_polar')) # 'pid', 'clf_polar' or 'clf_cartesian'
+    main(**getattr(Configs(), 'clf_polar')) # 'pid', 'clf_polar' or 'clf_cartesian'

From 9e1b70687d2ec2a4b2013a0b9851223525163ef1 Mon Sep 17 00:00:00 2001
From: Vikas Dhiman <wecacuee@users.noreply.github.com>
Date: Fri, 9 Oct 2020 11:51:24 -0700
Subject: [PATCH 7/7] change the definition of alpha

---
 PathTracking/move_to_pose/move_to_pose.py | 135 +++++++++++++---------
 1 file changed, 83 insertions(+), 52 deletions(-)

diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py
index 1c49e13322..779f3c2623 100644
--- a/PathTracking/move_to_pose/move_to_pose.py
+++ b/PathTracking/move_to_pose/move_to_pose.py
@@ -20,8 +20,9 @@
 
 # simulation parameters
 
-PolarState = namedtuple('PolarState', 'rho alpha beta')
-CartesianState = namedtuple('CartesianState', 'x y theta')
+PolarState = namedtuple('PolarState', 'rho alpha beta'.split())
+CartesianState = namedtuple('CartesianState', 'x y theta'.split())
+CartesianStateWithGoal = namedtuple('CartesianStateWithGoal', 'state state_goal'.split())
 
 LOG = SummaryWriter('data/runs/' + datetime.now().strftime("%m%d-%H%M"))
 
@@ -29,8 +30,13 @@
 def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianState:
     """
     rho is the distance between the robot and the goal position
-    alpha is the angle to the goal relative to the heading of the robot
-    beta is the angle between the robot's position and the goal position plus the goal angle
+    : \sqrt((x*-x)^2 + (y*-y)^2)
+
+    alpha is the heading of the robot relative the angle to the goal
+    : theta - atan2((y*-y),(x*-x))
+
+    beta is the goal position relative to the angle to the goal
+    : theta* - atan2((y*-y),(x*-x))
 
     >>> polar = np.random.rand(3) * np.array([1, 2*np.pi, 2*np.pi]) - np.array([0, np.pi, np.pi])
     >>> state_goal = np.random.rand(3) * np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
@@ -41,9 +47,9 @@ def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianStat
     rho, alpha, beta = x
     x_goal, y_goal, theta_goal = state_goal
     phi = angdiff(theta_goal, beta)
+    theta = normalize_angle(phi + alpha)
     x_diff = rho * np.cos(phi)
     y_diff = rho * np.sin(phi)
-    theta = angdiff(phi, alpha)
     return np.array([x_goal - x_diff,
                      y_goal - y_diff,
                      theta])
@@ -52,8 +58,11 @@ def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianStat
 def cartesian2polar(state: CartesianState, state_goal : CartesianState) -> PolarState:
     """
     rho is the distance between the robot and the goal position
-    alpha is the angle to the goal relative to the heading of the robot
-    beta is the angle between the robot's position and the goal position plus the goal angle
+    : \sqrt((x*-x)^2 + (y*-y)^2)
+    alpha is the heading of the robot relative the angle to the goal
+    : theta - atan2((y*-y),(x*-x))
+    beta is the goal position relative to the angle to the goal
+    : theta* - atan2((y*-y),(x*-x))
 
     >>> state = np.random.rand(3)* np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
     >>> state_goal = np.random.rand(3)* np.array([2, 2, 2*np.pi]) - np.array([1, 1, np.pi])
@@ -70,7 +79,7 @@ def cartesian2polar(state: CartesianState, state_goal : CartesianState) -> Polar
     # reparameterization
     rho = np.hypot(x_diff, y_diff)
     phi = np.arctan2(y_diff, x_diff)
-    alpha = angdiff(phi , theta)
+    alpha = angdiff(theta, phi)
     beta = angdiff(theta_goal , phi)
     return np.array((rho, alpha, beta))
 
@@ -83,11 +92,11 @@ def f(self, x : PolarState):
     def g(self, x : PolarState):
         rho, alpha, beta = x
         return (np.array([[-np.cos(alpha), 0],
-                          [np.sin(alpha)/rho, -1],
+                          [-np.sin(alpha)/rho, 1],
                           [-np.sin(alpha)/rho, 0]])
                 if (rho > 1e-6) else
                 np.array([[-1, 0],
-                          [1, -1],
+                          [-1, 1],
                           [-1, 0]]))
 
 class CartesianDynamicsWrapper:
@@ -119,6 +128,7 @@ def normalize_angle(theta):
     # from 0 rad to 2*pi rad with slight turn
     return (theta + np.pi) % (2 * np.pi) - np.pi
 
+
 def angdiff(thetap, theta):
     return normalize_angle(thetap - theta)
 
@@ -127,37 +137,58 @@ def cosdist(thetap, theta):
     return 1 - np.cos(thetap - theta)
 
 
+def angdist(thetap, theta):
+    return angdiff(thetap, theta)**2
+
 class CLFPolar:
     def __init__(self,
-                 Kp = np.array([9, 15, 5])/10.):
+                 Kp = np.array([5, 15, 40, 0])/10.):
         self.Kp = np.asarray(Kp)
 
-    def clf_terms(self, polar):
-        return self._clf_terms(polar)
+    def clf_terms(self, polar, x_goal):
+        return self._clf_terms(polar, x_goal)
 
-    def _clf_terms(self, polar):
+    def _clf_terms(self, polar, x_goal):
         rho, alpha, beta = polar
         return np.array((0.5 * self.Kp[0] * rho ** 2,
-                         self.Kp[1] * (1-np.cos(alpha)),
-                         self.Kp[2] * (1-np.cos(alpha - beta))
+                         self.Kp[1] * angdist(alpha, 0),
+                         self.Kp[2] * angdist(beta, 0),
+                         self.Kp[3] * (1-np.cos(beta - alpha))
         ))
 
-    def grad_clf(self, polar):
-        return self._grad_clf_terms(polar).sum(axis=-1)
+    def grad_clf(self, polar, x_goal):
+        """
+        >>> self = CLFPolar()
+        >>> x0 = np.random.rand(3)
+        >>> x_goal = np.random.rand(3)
+        >>> ajac = self.grad_clf(x0, x_goal)
+        >>> njac = numerical_jac(lambda x: self._clf_terms(x, x_goal).sum(), x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        """
+        return self._grad_clf_terms(polar, x_goal).sum(axis=-1)
 
-    def _grad_clf_terms(self, polar):
+    def _grad_clf_terms(self, polar, x_goal):
         """
         >>> self = CLFPolar()
         >>> x0 = np.random.rand(3)
-        >>> ajac = self._grad_clf_terms(x0).sum(axis=-1)
-        >>> njac = numerical_jac(lambda x: self._clf_terms(x).sum(), x0, 1e-6)[0]
+        >>> x0_goal = np.random.rand(3)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 0]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[0], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 1]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[1], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 2]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[2], x0, 1e-6)[0]
+        >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
+        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 3]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[3], x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         """
         rho, alpha, beta = polar
-        return np.array([[self.Kp[0] * rho,  0, 0],
-                         [0, self.Kp[1] * np.sin(alpha),
-                          self.Kp[2] * np.sin(alpha - beta)] ,
-                         [0,  0, - self.Kp[2] * np.sin(alpha - beta)]])
+        return np.array([[self.Kp[0] * rho,  0, 0, 0],
+                         [0, self.Kp[1] * np.sin(alpha), 0, - self.Kp[3] * np.sin(beta - alpha)],
+                         [0,  0, self.Kp[2] * np.sin(beta), self.Kp[3] * np.sin(beta - alpha)]])
 
     def isconverged(self, x, x_goal):
         rho, alpha, beta = cartesian2polar(x, x_goal)
@@ -186,7 +217,7 @@ def numerical_jac(func, x0, eps):
 
 class CLFCartesian:
     def __init__(self,
-                 Kp = np.array([9, 15, 5])/10.):
+                 Kp = np.array([9, 15, 40])/10.):
         self.Kp = np.asarray(Kp)
 
     def clf_terms(self, state, state_goal):
@@ -194,8 +225,8 @@ def clf_terms(self, state, state_goal):
         x,y, theta = state
         x_goal, y_goal, theta_goal = state_goal
         return np.array((0.5 * self.Kp[0] * rho ** 2,
-                         self.Kp[1] * cosdist(alpha, 0),
-                         self.Kp[2] * cosdist(theta_goal, theta)
+                         self.Kp[1] * (1-np.cos(alpha)),
+                         self.Kp[2] * (1-np.cos(beta))
         ))
 
     def _grad_clf_terms(self, state, state_goal):
@@ -204,26 +235,25 @@ def _grad_clf_terms(self, state, state_goal):
         >>> x0 = np.random.rand(3)
         >>> x0_goal = np.random.rand(3)
         >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 0]
-        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[0], x0, 1e-6)[0]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[0], x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 1]
-        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[1], x0, 1e-6)[0]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[1], x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 2]
-        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal)[2], x0, 1e-6)[0]
+        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[2], x0, 1e-6)[0]
         >>> np.testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
         """
         x_diff, y_diff, theta_diff = state_goal - state
         rho, alpha, beta = cartesian2polar(state, state_goal)
         return np.array([[- self.Kp[0] * x_diff,
-                          self.Kp[1] * np.sin(alpha) * y_diff / (rho**2),
-                          0],
+                          - self.Kp[1] * np.sin(alpha) * y_diff / (rho**2),
+                          - self.Kp[2] * np.sin(beta) * y_diff / (rho**2)
+                          ],
                          [- self.Kp[0] * y_diff,
-                          - self.Kp[1] * np.sin(alpha) * x_diff / (rho**2),
-                         0],
-                         [0,
-                          -self.Kp[1] * np.sin(alpha),
-                          - self.Kp[2] * np.sin(theta_diff)]
+                          self.Kp[1] * np.sin(alpha) * x_diff / (rho**2),
+                          self.Kp[2] * np.sin(beta) * x_diff / (rho**2)],
+                         [0, self.Kp[1] * np.sin(alpha), 0]
                          ])
     def grad_clf(self, state, state_goal):
         """
@@ -255,24 +285,24 @@ def __init__(self, # simulation parameters
         self.dynamics = dynamics
         self.clf = clf
 
-    def _clf(self, polar):
-        return self.clf.clf_terms(polar).sum()
+    def _clf(self, polar, x_goal):
+        return self.clf.clf_terms(polar, x_goal).sum()
 
-    def _grad_clf(self, polar):
-        return self.clf.grad_clf(polar)
+    def _grad_clf(self, polar, x_goal):
+        return self.clf.grad_clf(polar, x_goal)
 
     def _clc(self, x, x_goal, u, t):
         polar = self.coordinate_converter(x, x_goal)
         f, g = self.dynamics.f, self.dynamics.g
-        gclf = self._grad_clf(polar)
+        gclf = self._grad_clf(polar, x_goal)
         LOG.add_scalar("x_0", x[0], t)
-        print("x :", x)
-        print("clf terms :", self.clf.clf_terms(polar))
-        print("clf:", self.clf.clf_terms(polar).sum())
-        print("grad_x clf:", gclf)
-        print("g(x): ", g(polar))
-        print("grad_u clf:", gclf @ g(polar))
-        return gclf @ (f(polar) + g(polar) @ u) + 10 * self._clf(polar)
+        # print("x :", x)
+        # print("clf terms :", self.clf.clf_terms(polar, x_goal))
+        # print("clf:", self.clf.clf_terms(polar, x_goal).sum())
+        # print("grad_x clf:", gclf)
+        # print("g(x): ", g(polar))
+        # print("grad_u clf:", gclf @ g(polar))
+        return gclf @ (f(polar) + g(polar) @ u) + 10 * self._clf(polar, x_goal)
 
     def _cost(self, x, u):
         import cvxpy as cp # pip install cvxpy
@@ -306,7 +336,7 @@ def isconverged(self, state, state_goal):
 class ControllerPID:
     def __init__(self, # simulation parameters
                  Kp_rho = 9,
-                 Kp_alpha = 15,
+                 Kp_alpha = -15,
                  Kp_beta = -3):
         self.Kp_rho = Kp_rho
         self.Kp_alpha = Kp_alpha
@@ -424,6 +454,7 @@ def clf_cartesian(self):
         return dict(simulator=partial(move_to_pose,
                                       dynamics=CartesianDynamics(),
                                       controller=ControllerCLF(
+                                          coordinate_converter = lambda x, x_g: (x),
                                           dynamics=CartesianDynamics(),
                                           clf = CLFCartesian()
                                       )))
@@ -453,5 +484,5 @@ def main(simulator = move_to_pose):
 
 if __name__ == '__main__':
     import doctest
-    doctest.testmod()
+    doctest.testmod() # always run unittests first
     main(**getattr(Configs(), 'clf_polar')) # 'pid', 'clf_polar' or 'clf_cartesian'