# Simple Car


One of the easiest examples to understand is the simple car, which is shown in Figure 13.1. We all know that a car cannot drive sideways because the back wheels would have to slide instead of roll. This is why parallel parking is challenging. If all four wheels could be turned simultaneously toward the curb, it would be trivial to park a car. The complicated maneuvers for parking a simple car arise because of rolling constraints. 




 The car can be imagined as a rigid body that moves in the plane. Therefore, its C-space is $ {\cal C}= {\mathbb{R}}^2 \times {\mathbb{S}}^1$. Figure 13.1 indicates several parameters associated with the car. A configuration is denoted by $ q = (x,y,\theta)$. The body frame of the car places the origin at the center of rear axle, and the $ x$-axis points along the main axis of the car. Let $ s$ denote the (signed) speed13.2 of the car. Let $ \phi $ denote the steering angle (it is negative for the wheel orientations shown in Figure 13.1). The distance between the front and rear axles is represented as $ L$. If the steering angle is fixed at $ \phi $, the car travels in a circular motion, in which the radius of the circle is $ \rho$. Note that $ \rho$ can be determined from the intersection of the two axes shown in Figure 13.1 (the angle between these axes is $ \vert\phi\vert$).

Using the current notation, the task is to represent the motion of the car as a set of equations of the form (13.11)

\begin{equation}\dot x = f_1(x,y,\theta,s,\phi)\end{equation} 

\begin{equation}\dot y = f_2(x,y,\theta,s,\phi)\end{equation} 

\begin{equation}{\dot \theta} = f_3(x,y,\theta,s,\phi)\end{equation}


# Dubins Curve



 It was shown in Dubin's paper "On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents" that between any two configurations, the shortest path for the Dubins car can always be expressed as a combination of no more than three motion primitives. Each motion primitive applies a constant action over an interval of time. Furthermore, the only actions that are needed to traverse the shortest paths are $ u \in \{-1,0,1\}$. The primitives and their associated symbols are shown in Figure 15.3. The $ S$ primitive drives the car straight ahead. The $ L$ and $ R$ primitives turn as sharply as possible to the left and right, respectively. Using these symbols, each possible kind of shortest path can be designated as a sequence of three symbols that corresponds to the order in which the primitives are applied. Let such a sequence be called a word . There is no need to have two consecutive primitives of the same kind because they can be merged into one. Under this observation, ten possible words of length three are possible. Dubins showed that only these six words are possibly optimal:

$\displaystyle \{ LRL,\; RLR, \; LSL,\; LSR,\; RSL,\; RSR \} .$ 	(15.44)

The shortest path between any two configurations can always be characterized by one of these words. These are called the Dubins curves. 



 Figure 15.4: The trajectories for two words are shown in $ {\cal W}= {\mathbb{R}}^2$. 

To be more precise, the duration of each primitive should also be specified. For $ L$ or $ R$, let a subscript denote the total amount of rotation that accumulates during the application of the primitive. For $ S$, let a subscript denote the total distance traveled. Using such subscripts, the Dubins curves can be more precisely characterized as

$$\displaystyle \{ L_\alpha R_\beta L_\gamma, \; R_\alpha L_\beta R_\gamma, \; L_\alpha S_d L_\gamma, \; L_\alpha S_d R_\gamma, \; R_\alpha S_d L_\gamma, \; R_\alpha S_d R_\gamma \} ,$$ 	(15.45)

in which $ \alpha, \gamma \in [0,2 \pi)$, $ \beta \in (\pi,2 \pi)$, and $ d \geq 0$. Figure 15.4 illustrates two cases. Note that $ \beta$ must be greater than $ \pi $ (if it is less, then some other word becomes optimal).

It will be convenient to invent a compressed form of the words to group together paths that are qualitatively similar. This will be particularly valuable when Reeds-Shepp curves are introduced in Section 15.3.2 because there are 46 of them, as opposed to $ 6$ Dubins curves. Let $ C$ denote a symbol that means ``curve,'' and represents either $ R$ or $ L$. Using $ C$, the six words in (15.44) can be compressed to only two base words:

$$\displaystyle \{ CCC, \; CSC \} .$$ 	(15.46)

In this compressed form, remember that two consecutive $ C$s must be filled in by distinct turns ($ RR$ and $ LL$ are not allowed as subsequences). In compressed form, the base words can be specified more precisely as

$$\displaystyle \{ C_\alpha C_\beta C_\gamma, \; C_\alpha S_d C_\gamma \} ,$$ 	(15.47)

in which $ \alpha, \gamma \in [0,2 \pi)$, $ \beta \in (\pi,2 \pi)$, and $ d \geq 0$. 

## Diagram for all six types of dubins curves are given below :-







From the above figure, we can see that $$ 90 + \phi  + \theta + 90 + \theta_i = 360 $$

$$ \therefore \theta_i = 180 - (\theta + \phi) $$

Our robot car is moving along a left arc from source position at point $ t_1(x_s,y_s) $ to goal position value at point $ t_2(x_g,y_g) $ 

$$ x_g = x_s - \rho cos(90-\theta) + \rho cos(90-\theta_i) $$

$$ x_g = x_s - \rho (cos90cos\theta + sin90sin\theta) + \rho (cos90cos\theta_i + sin90sin\theta_i) $$

we know that cos90 = 0, sin90 = 1 $$ x_g = x_s - \rho sin\theta + \rho sin\theta_i $$ 

$$ x_g = x_s - \rho sin\theta + \rho sin[180 - (\theta + \phi)] $$ 

$$ x_g = x_s - \rho sin\theta + \rho sin90cos[90-(\theta + \phi)]+cos90sin[90-(\theta + \phi)] $$ 

$$ x_g = x_s - \rho sin\theta + \rho cos[90-(\theta + \phi)] $$

$$ x_g = x_s - \rho sin\theta + \rho cos90cos(\theta + \phi)+ sin90sin(\theta + \phi) $$

$$ \therefore x_g = x_s - \rho sin\theta + \rho sin(\theta + \phi) $$

$$ y_g = y_s + \rho cos\theta + \rho cos\theta_i $$

$$ y_g = y_s + \rho cos\theta + \rho cos[180 - (\theta + \phi)] $$ 

$$ y_g = y_s + \rho cos\theta + \rho [cos90cos(90-(\theta + \phi)) - sin90sin(90-(\theta + \phi))] $$ 

$$ y_g = y_s + \rho cos\theta - \rho sin[90-(\theta + \phi)] $$ 

$$ y_g = y_s + \rho cos\theta - \rho [sin90cos(\theta + \phi) - cos90sin(\theta + \phi)] $$

$$ \therefore y_g = y_s + \rho cos\theta - \rho cos(\theta + \phi) $$

Let $(x_0,y_0;\alpha)$ be the initial configuration and $(x_1,y_1;\beta)$ e the terminal configuration.
Three corresponding operators, $L_v$(for left turn), $R_v$(for right turn) and $S_v$(for straight), are needed which transform an arbitrary configuration $(x,y,\theta)$ into its corresponding image configuration,

$$L_v(x,y;\theta) = (x_s - \rho sin\theta + \rho sin(\theta + \phi),(y_s + \rho cos\theta - \rho cos(\theta + \phi);(\theta + \phi))$$

Now imagine a right turn from $t_2$ to $t_1$ where, assume that the intial yaw angle is $\theta_h = \theta + \phi$ and final yaw angle is $(\theta_h - \phi)$ with respect to x axis

$$R_v(x,y;\theta_h) = (x_s - \rho sin(\theta_h - \phi) + \rho sin\theta_h),(y_s + \rho cos(\theta_h - \phi) - \rho cos\theta_h);(\theta - \phi))$$

$$S_v(x,y;\theta) = (x_s + vsin\theta, y_s + vcos\theta; \theta)$$

where, v is the length along the line segment

From above figure, $$\angle t_2Ct_1 = (90 - \theta_i) + (90 - \theta) = 180 - \theta_i - \theta = \theta + \phi - \theta = \phi$$

For left or right arc, the length of the line segment is denoted by $$v = r\phi$$ 
Euclidean distance represented as $$ d = \sqrt {(x_1 - x_0)^2 + (y_1 - y_0)^2}$$

Dubin path lengths of three segments of a whole path are t, p, q respectively where, l = t + p + q


```python
"""
Dubins Path
"""

import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import draw
import random
%matplotlib inline
```


```python
# class for PATH element
class PATH:
    def __init__(self, L, mode, x, y, yaw):
        self.L = L  # total path length [float]
        self.mode = mode  # type of each part of the path [string]
        self.x = x  # final x positions [m]
        self.y = y  # final y positions [m]
        self.yaw = yaw  # final yaw angles [rad]
```


```python
# utils
def pi_2_pi(theta):
    while theta > math.pi:
        theta -= 2.0 * math.pi

    while theta < -math.pi:
        theta += 2.0 * math.pi

    return theta
```


```python
def mod2pi(theta):
    return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
```

Now apply all three operators upon source waypoint to get the goal position values for any LSL dubin path

$$L_q(S_p(L_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

By applying the corresponding operators, we will get the following equation for $x_1$

$$x_0 - \rho sin\alpha + \rho sin(\alpha+\phi_1)+ pcos(\alpha+\phi_1)-\rho sin(\alpha+\phi_1)+\rho sin(\alpha+\phi_1+\phi_2)=x_1$$

$$x_0 - \rho sin\alpha + \rho sin(\alpha+\frac{t}{\rho}) + pcos(\alpha+\frac{t}{\rho})-\rho sin(\alpha+\frac{t}{\rho})+\rho sin(\alpha+\frac{t}{\rho}+\frac{q}{\rho})=x_1$$

$$\frac{x_0}{\rho}-sin\alpha + sin(\alpha+\frac{t}{\rho})+ \frac{p}{\rho}cos(\alpha+\frac{t}{\rho})-sin(\alpha+\frac{t}{\rho})+sin(\alpha+\frac{t}{\rho}+\frac{q}{\rho})=\frac{x_1}{\rho}$$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

Now above equation can be overwritten as equation(1) $$\hat{x_0}-sin\alpha + sin(\alpha+\hat{t})+ \hat{p}cos(\alpha+\hat{t})-sin(\alpha+\hat{t})+sin(\alpha+\hat{t}+\hat{q})=\hat{x_1}$$ 

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos\alpha-cos(\alpha+\hat{t}) + \hat{p}sin(\alpha+\hat{t})+cos(\alpha+\hat{t})-cos(\alpha+\hat{t}+\hat{q})=\hat{y_1}$$

We will get another equation $$\alpha+\hat{t}+\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha+\hat{t})=\hat{x_1}-\hat{x_0}+sin\alpha-sin(\alpha+\hat{t}+\hat{q})$$ 

$$\hat{p}cos(\alpha+\hat{t})=(\hat{x_1}-\hat{x_0})+(sin\alpha-sin\beta)$$ 

$$\hat{p}sin(\alpha+\hat{t})=(\hat{y_1}-\hat{y_0})-(cos\alpha-cos\beta)$$ 

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 - 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$ 

After dividing equation(2) by (1), we get

$$ tan(\alpha+\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})-(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})+(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha+\hat{t})= \arctan{k} = rec $$ 

$$\hat{t} = (-\alpha + rec)mod2\pi $$

$$\hat{q} = (\beta - rec)mod2\pi $$


```python
def LSL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)
    
    p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dx * (sin_a - sin_b) + 2 * dy * (cos_a + cos_b)

    if p_lsl < 0:
        return None, None, None, ["L", "S", "L"]
    else:
        p_lsl = math.sqrt(p_lsl)

    k = (dy - cos_a + cos_b)/(dx + sin_a - sin_b)
    t_lsl = mod2pi(-alpha + math.atan(k))
    q_lsl = mod2pi(beta - math.atan(k))

    return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
```

Now apply all three operators upon source waypoint to get the goal position values for any RSR dubin path

$$R_q(S_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation (2) for $x_1$ 

$$\hat{x_0}-sin(\alpha-\hat{t}) + sin\alpha + \hat{p}cos(\alpha-\hat{t})-sin(\alpha-\hat{t}-\hat{q})+sin(\alpha-\hat{t}) =\hat{x_1}$$ 

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + \hat{p}sin(\alpha-\hat{t})+cos(\alpha-\hat{t}-\hat{q})-cos(\alpha-\hat{t})=\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha-\hat{t})=(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)$$ 

$$\hat{p}sin(\alpha-\hat{t})=(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)$$ 

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$


Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$ 

After dividing equation(2) by (1), we get

$$ tan(\alpha-\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha-\hat{t})= \arctan{k} = rec $$ 

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$


```python
def RSR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_rsr = 2 + dist ** 2 - 2 * cos_a_b - 2 * dx * (sin_a - sin_b) + 2 * dy * (cos_a - cos_b)

    if p_rsr < 0:
        return None, None, None, ["R", "S", "R"]
    else:
        p_rsr = math.sqrt(p_rsr)

    k = (dy + cos_a - cos_b)/(dx - sin_a + sin_b)
    t_rsr = mod2pi(alpha - math.atan(k))
    q_rsr = mod2pi(-beta + math.atan(k))

    return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]


```

Now apply all three operators upon source waypoint to get the goal position values for any LSR dubin path

$$L_q(S_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

By applying the corresponding operators, we will get the following equation for $x_1$

$$x_0 - \rho sin(\alpha-\phi_1)+\rho sin\alpha + pcos(\alpha-\phi_1)-\rho sin(\alpha-\phi_1)+\rho sin(\alpha-\phi_1+\phi_2)=x_1$$

$$x_0 - \rho sin(\alpha-\frac{t}{\rho})+\rho sin\alpha + pcos(\alpha-\frac{t}{\rho})-\rho sin(\alpha-\frac{t}{\rho})+\rho sin(\alpha-\frac{t}{\rho}+\frac{q}{\rho})=x_1$$

$$\frac{x_0}{\rho}-sin(\alpha-\frac{t}{\rho})+sin\alpha + \frac{p}{\rho}cos(\alpha-\frac{t}{\rho})-sin(\alpha-\frac{t}{\rho})+sin(\alpha-\frac{t}{\rho}+\frac{q}{\rho})=\frac{x_1}{\rho}$$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

Now above equation can be overwritten as equation(1) $$\hat{x_0}-sin(\alpha-\hat{t})+sin\alpha + \hat{p}cos(\alpha-\hat{t})-sin(\alpha-\hat{t})+sin(\alpha-\hat{t}+\hat{q})=\hat{x_1}$$ 

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + \hat{p}sin(\alpha-\hat{t})+cos(\alpha-\hat{t})-cos(\alpha-\hat{t}+\hat{q})=\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}+\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})=\hat{x_1}-\hat{x_0}-sin\alpha-sin(\alpha-\hat{t}+\hat{q})$$ 

$$\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})=(\hat{x_1}-\hat{x_0})-(sin\alpha+sin\beta)$$ 

$$\hat{p}sin(\alpha-\hat{t})+2sin(\alpha-\hat{t})=(\hat{y_1}-\hat{y_0})+(cos\alpha+cos\beta)$$ 

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha+sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha+cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha+sin\beta)^2 + (cos\alpha+cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2+2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2+2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]+2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$ 

After dividing equation(1) by (2), we get

$$\frac{\hat{p}cos(\alpha-\hat{t})-2sin(\alpha-\hat{t})}{\hat{p}sin(\alpha-\hat{t})+2cos(\alpha-\hat{t})} = \frac{(\hat{x_1}-\hat{x_0})-(sin\alpha+sin\beta)}{(\hat{y_1}-\hat{y_0})+(cos\alpha+cos\beta)} = k$$
where, k is a constant

$$\frac{\hat{p}-2tan(\alpha-\hat{t})}{\hat{p}tan(\alpha-\hat{t})+ 2} = k $$

$$\hat{p}-2tan(\alpha-\hat{t}) = k\hat{p}tan(\alpha-\hat{t})+ 2k $$
 
$$tan(\alpha-\hat{t})= \frac{\hat{p}-2k}{k\hat{p}+2} $$ 

$$(\alpha-\hat{t})= \arctan{\frac{\hat{p}-2k}{k\hat{p}+2}} = rec $$ 

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (\beta - rec)mod2\pi $$


```python
def LSR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_lsr = -2 + dist ** 2 + 2 * cos_a_b - 2 * dx * (sin_a + sin_b) + 2 * dy * (cos_a + cos_b)

    if p_lsr < 0:
        return None, None, None, ["L", "S", "R"]
    else:
        p_lsr = math.sqrt(p_lsr)

    k = (dx - sin_a - sin_b)/(dy + cos_a + cos_b)    
    rec = math.atan2(p_lsr - 2*k , k*p_lsr + 2) 
    t_lsr = mod2pi(alpha - rec)
    q_lsr = mod2pi(beta - rec)

    return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]


```

Now apply all three operators upon source waypoint to get the goal position values for any RSL dubin path

$$R_q(S_p(L_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ and $q=\rho\phi_2$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation for $x_1$

$$\hat{x_0}-sin\alpha+sin(\alpha+\hat{t}) + \hat{p}cos(\alpha+\hat{t})-sin(\alpha+\hat{t}-\hat{q})+sin(\alpha+\hat{t})=\hat{x_1}$$ 

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos\alpha-cos(\alpha+\hat{t}) + \hat{p}sin(\alpha+\hat{t})+cos(\alpha+\hat{t}-\hat{q})-cos(\alpha+\hat{t})=\hat{y_1}$$

We will get another equation $$\alpha+\hat{t}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$\hat{p}cos(\alpha+\hat{t})+2sin(\alpha+\hat{t})=(\hat{x_1}-\hat{x_0})+(sin\alpha+sin\beta)$$ 

$$\hat{p}sin(\alpha+\hat{t})-2cos(\alpha+\hat{t})=(\hat{y_1}-\hat{y_0})-(cos\alpha+cos\beta)$$ 

After $(1)^2 + (2)^2$, we get

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha+sin\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha+cos\beta)^2 - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha+sin\beta)^2 + (cos\alpha+cos\beta)^2 + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2+2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2+2cos\alpha cos\beta + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]+2(sin\alpha sin\beta+cos\alpha cos\beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 + 4 = (\hat{d})^2 + 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

Let us assume $$d_n = (\hat{d})^2 - 2 + 2cos(\alpha - \beta) + 2(\hat{x_1}-\hat{x_0})(sin\alpha+sin\beta) - 2(\hat{y_1}-\hat{y_0})(cos\alpha+cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$ 

After dividing equation(1) by (2), we get

$$\frac{\hat{p}cos(\alpha+\hat{t})+2sin(\alpha+\hat{t})}{\hat{p}sin(\alpha+\hat{t})-2cos(\alpha+\hat{t})} = \frac{(\hat{x_1}-\hat{x_0})+(sin\alpha+sin\beta)}{(\hat{y_1}-\hat{y_0})-(cos\alpha+cos\beta)} = k$$
where, k is a constant

$$\frac{\hat{p}+2tan(\alpha+\hat{t})}{\hat{p}tan(\alpha+\hat{t})- 2} = k $$

$$\hat{p}+2tan(\alpha+\hat{t}) = k\hat{p}tan(\alpha+\hat{t})- 2k $$
 
$$tan(\alpha+\hat{t})= \frac{\hat{p}+2k}{k\hat{p}-2} $$ 

$$(\alpha+\hat{t})= \arctan{\frac{\hat{p}+2k}{k\hat{p}-2}} = rec $$ 

$$\hat{t} = (-\alpha + rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$


```python
def RSL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    p_rsl = -2 + dist ** 2 + 2 * cos_a_b + 2 * dx * (sin_a + sin_b) - 2* dy * (cos_a + cos_b)

    if p_rsl < 0:
        return None, None, None, ["R", "S", "L"]
    else:
        p_rsl = math.sqrt(p_rsl)

    k = (dx + sin_a + sin_b)/(dy - cos_a - cos_b)    
    rec = math.atan2(p_rsl + 2 * k , k * p_rsl - 2)
    t_rsl = mod2pi(-alpha + rec)
    q_rsl = mod2pi(-beta + rec)

    return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]


```

Now apply all three operators upon source waypoint to get the goal position values for any RLR dubin path

$$R_q(L_p(R_t(x_0,y_0;\alpha)))=(x_1,y_1;\beta)$$

We know that $t=\rho\phi_1$ , $p=\rho\phi_2$ and $q=\rho\phi_3$

Let us assume that $$\hat{x_0}=\frac{x_0}{\rho},\hat{x_1}=\frac{x_1}{\rho},\hat{y_0}=\frac{y_0}{\rho},\hat{y_1}=\frac{y_1}{\rho},\hat{t}=\frac{t}{\rho},\hat{p}=\frac{p}{\rho},\hat{q}=\frac{q}{\rho},\hat{d}=\frac{d}{\rho}$$

By applying the corresponding operators, we will get the following equation (1) for $x_1$ 

$$\hat{x_0}-sin(\alpha-\hat{t}) + sin\alpha - sin(\alpha-\hat{t})+ sin(\alpha-\hat{t}+\hat{p}) - sin(\alpha-\hat{t}+\hat{p}-\hat{q}) + sin(\alpha-\hat{t}+\hat{p}) =\hat{x_1}$$ 

By applying the corresponding operators, we will get the following equation (2) for $y_1$

$$\hat{y_0}+cos(\alpha-\hat{t})-cos\alpha + cos(\alpha-\hat{t}) - cos(\alpha-\hat{t}+\hat{p})+cos(\alpha-\hat{t}+\hat{p}-\hat{q}) - cos(\alpha-\hat{t}+\hat{p}) =\hat{y_1}$$

We will get another equation $$\alpha-\hat{t}+\hat{p}-\hat{q}=\beta(mod2\pi) $$

Now equation (1) and (2) can be rewritten as follows

$$ 2sin(\alpha-\hat{t}+\hat{p}) - 2sin(\alpha-\hat{t}) = (\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)$$ 

$$ - 2cos(\alpha-\hat{t}+\hat{p}) + 2cos(\alpha-\hat{t}) = (\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)$$ 

After $(1)^2 + (2)^2$, we get

$$ 4 + 4 = (\hat{x_1}-\hat{x_0})^2 + (sin\alpha-sin\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + (\hat{y_1}-\hat{y_0})^2 + (cos\alpha-cos\beta)^2 + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{x_1}-\hat{x_0})^2 + (\hat{y_1}-\hat{y_0})^2 + (sin\alpha-sin\beta)^2 + (cos\alpha-cos\beta)^2 - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{d})^2 + (sin\alpha)^2+(sin\beta)^2-2sin\alpha sin\beta + (cos\alpha)^2+(cos\beta)^2-2cos\alpha cos\beta - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$ 8 = (\hat{d})^2 + [(sin\alpha)^2+(cos\alpha)^2]+[(sin\beta)^2+(cos\beta)^2]-2(sin\alpha sin\beta+cos\alpha cos\beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\hat{p}^2 = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$


Let us assume $$d_n = (\hat{d})^2 + 2 - 2cos(\alpha - \beta) - 2(\hat{x_1}-\hat{x_0})(sin\alpha-sin\beta) + 2(\hat{y_1}-\hat{y_0})(cos\alpha-cos\beta)$$

$$\therefore \hat{p} = \sqrt{d_n}$$ 

After dividing equation(2) by (1), we get

$$ tan(\alpha-\hat{t}) = \frac{(\hat{y_1}-\hat{y_0})+(cos\alpha-cos\beta)}{(\hat{x_1}-\hat{x_0})-(sin\alpha-sin\beta)} = k$$
where, k is a constant

$$(\alpha-\hat{t})= \arctan{k} = rec $$ 

$$\hat{t} = (\alpha - rec)mod2\pi $$

$$\hat{q} = (-\beta + rec)mod2\pi $$


```python
def RLR(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0

    if abs(rec) > 1.0:
        return None, None, None, ["R", "L", "R"]

    p_rlr = mod2pi(2 * math.pi - math.acos(rec))
    t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))
    q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))

    return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
```


```python
def LRL(alpha, beta, dist, dx, dy):
    sin_a = math.sin(alpha)
    sin_b = math.sin(beta)
    cos_a = math.cos(alpha)
    cos_b = math.cos(beta)
    cos_a_b = math.cos(alpha - beta)

    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0

    if abs(rec) > 1.0:
        return None, None, None, ["L", "R", "L"]

    p_lrl = mod2pi(2 * math.pi - math.acos(rec))
    t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
    q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))

    return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
```


```python
def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
    # for straight path computing the next wayponts' state values
    # l is length of a single part out of three
    # maxc is maximum curvature of car in radian
    # note that straight 
    if m == "S":
        px[ind] = ox + l / maxc * math.cos(oyaw)
        py[ind] = oy + l / maxc * math.sin(oyaw)
        pyaw[ind] = oyaw
    # for curve path computing the next waypoints' state values 
    # why sin(l) ?
    else:
        ldx = math.sin(l) / maxc
        if m == "L":
            ldy = (1.0 - math.cos(l)) / maxc
        elif m == "R":
            ldy = (1.0 - math.cos(l)) / (-maxc)

        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
        px[ind] = ox + gdx
        py[ind] = oy + gdy

    # for left curve tangent's slope or car's yaw angle value is increasing via 1 that means rotating in anti clockwise direction
    # for right curve tangent's slope or car's yaw angle value is decreasing via 1 that means rotating in clockwise direction
    if m == "L":
        pyaw[ind] = oyaw + l
    elif m == "R":
        pyaw[ind] = oyaw - l

    if l > 0.0:
        directions[ind] = 1
    else:
        directions[ind] = -1

    return px, py, pyaw, directions
```


```python
def generate_local_course(total_length, lengths, mode, max_curv, step_size):
    # point_num specifies the number of intermediate points between start and goal waypoints
    point_num = int(total_length / step_size) + len(lengths)  

    # for index variable '_' in range of point_num
    # all values in the arrays like px, py, pyaw and directions are initialized to 0 or 0.0
    # px, py denotes the coordinates of intermediate waypoints
    # pyaw specifies the yaw angle of the car at an intermdiate waypoint
    # direction represents the direction taken by car at an intermediate waypoint
    px = [0.0 for _ in range(point_num)]
    py = [0.0 for _ in range(point_num)]
    pyaw = [0.0 for _ in range(point_num)]
    directions = [0 for _ in range(point_num)]
    ind = 1

    # direction[i] = 0 means car is moving along straight line
    # direction[i] = 1 means left steering by car
    # direction[i] = -1 means right steering by car
    if lengths[0] > 0.0:
        directions[0] = 1
    else:
        directions[0] = -1

    # length[i] = +ve means distance is going to be increased in positive side
    # length[i] = -ve means distance is going to be increased in negaitive side
    if lengths[0] > 0.0:
        d = step_size
    else:
        d = -step_size

    ll = 0.0

    for m, l, i in zip(mode, lengths, range(len(mode))):
        # this lengths array consists of the lengths of three parts of the dubin path between two consecutive waypoints
        # l[i] denotes the length of any one part
        # while car is moving in straight direction or taking a left steering
        if l > 0.0:       
            d = step_size
        # while car is taking a right steering    
        else:            
            d = -step_size
    
        # storing x, y and yaw values of the starting point of any one specific part 
        # any part may be a right or left curve or may be a staright line
        # index value bocomes 0 from 1
        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
        ind -= 1
        
        # while two consecutive mode combination is straight-left or left-straight
        # we will get a positive value for multiplication of their lengths
        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
            pd = -d - ll
        else:
            pd = d - ll
  
        # call interpolate() to generate all the equally spaced intermediate watpoints for any single part
        # interpolation is a process of determining the unknown values that lie in between the known data points. 
        # It is mostly used to predict the unknown values for any geographical related data points such as noise level, rainfall, elevation, and so on.
        while abs(pd) <= abs(l):
            ind += 1
            px, py, pyaw, directions = interpolate(ind, pd, m, max_curv, ox, oy, oyaw, px, py, pyaw, directions)
            pd += d
       
        # caluculation of remaining length    
        ll = l - pd - d 
        # Now we use inerpolate() to generate the accute path for one part via using those generated waypoints
        ind += 1
        px, py, pyaw, directions = interpolate(ind, l, m, max_curv, ox, oy, oyaw, px, py, pyaw, directions)
    
    if len(px) <= 1:
        return [], [], [], []

    # remove unused data
    while len(px) >= 1 and px[-1] == 0.0:
        px.pop()
        py.pop()
        pyaw.pop()
        directions.pop()
  
    return px, py, pyaw, directions


```


```python
def planning_from_origin(dx, dy, dyaw, curv, radius, step_size):
    # origin is first waypoint
    # Prior Python 3.8, hypot() is used previously to find the hypotenuse of a right-angled triangle: sqrt(x*x + y*y)
    # From Python 3.8, this method is used to calculate the Euclidean norm as well.
    # For n-dimensional cases, the coordinates passed are assumed to be like (x1, x2, x3, ..., xn) 
    # So Euclidean length from the origin is calculated by sqrt(x1*x1 + x2*x2 +x3*x3 .... xn*xn).) & stored in dist variable
    # radius of the circle or curvature path along which robot car is moving
    dist = math.hypot(dx, dy)
    dist_hat = dist/radius
    dx_hat = dx/radius
    dy_hat = dy/radius

    # The math.atan2() method returns the arc tangent of y/x, in radians where, x and y are the coordinates of a point (x,y).
    # The arctan function is the inverse of the tangent function.
    # It returns the angle whose tangent is a given number.
    theta = mod2pi(math.atan2(dy, dx))
    alpha = mod2pi(-theta)
    beta = mod2pi(dyaw - theta)
 
    # store function or methods names in planners list
    planners = [LSL, RSR, LSR, RSL, RLR, LRL]

    # best_cost value is set to positive infinity
    # None is null value
    best_cost = float("inf")
    bt, bp, bq, best_mode = None, None, None, None

    # call each planner function in the list
    # any dubin path between two waypoints is divided into three parts
    # distances of those parts are stored in t,p,q variable respectively
    # mode is an array of direction elements
    for planner in planners:
        t, p, q, mode = planner(alpha, beta, dist_hat, dx_hat, dy_hat)

        if t is None:
            continue

        # total cost or distance of the dubin path 
        # we want the best directions or mode with the minimum distance 
        # store the length of three parts of the path with minimum cost into bt, bp, bq
        cost = (abs(t) + abs(p) + abs(q))
        if best_cost > cost:
            bt, bp, bq, best_mode = t, p, q, mode
            best_cost = cost
    lengths = [bt, bp, bq]

    # call the generate_local_course()
    # returning the x values, y values and yaw values with directions that are needed to compute the dubin path 
    x_list, y_list, yaw_list, directions = generate_local_course(sum(lengths), lengths, best_mode, curv, step_size)

    return x_list, y_list, yaw_list, best_mode, best_cost
```


```python
def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, radius, step_size=0.1):
    # step_size = 0.1 means 
    # calculate the differences between source and goal positions 
    dx = gx - sx
    dy = gy - sy

    # robot car is moving in xy plane and steering along z axis or direction 
    # from_euler() method rotates our car along z axix using syaw angle and as_dcm() converts a car's 3D rotation into a direction cosine matrix 
    # l_rot stores the (3X3) direction cosine matrix as a (2X2) matrix
    # stack() method makes a matrix of dimension (1X2)
    # T denotes the traonspose matrix with dimension(1X2) of the previous matrix  and @ denotes the matrix multiplication 
    # multiplication result is stored in 1e_xy (1x2) matrix and now car's rotation is complete
    l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]
    le_xy = np.stack([dx, dy]).T @ l_rot
    le_yaw = gyaw - syaw

    # call planning_from_origin() method with below arguments 
    lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(le_xy[0], le_xy[1], le_yaw, curv, radius, step_size)
    
    # 
    rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]
    converted_xy = np.stack([lp_x, lp_y]).T @ rot
    x_list = converted_xy[:, 0] + sx
    y_list = converted_xy[:, 1] + sy
    yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]

    return PATH(lengths, mode, x_list, y_list, yaw_list)
```


```python
def main():
    # choose robot vehicle's states pairs: (x coordinate, y coordinate, yaw in degree
    # Yaw happens when the weight of your vehicle shifts from its center of gravity to the left or the right.
    # This is a shift you will definitely feel when you’re inside your vehicle.
    # The times this will happen is when you suddenly steer, brake, or accelerate.
    # This might cause your vehicle to spin around its center of gravity.
    # states is a list that stores multiple other lists with triplets like x, y and yaw angle values
    # simulation-1
    states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),(35, -5, 30), (25, -10, -120), (15, -15, 100), (15, -25, 90)]

    # simulation-2
    #states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),(35, 10, 180), (32, -10, 180), (5, -12, 90)]

    # max curvature of robot vehicle in radian
    max_curv = 0.25
    
    # radius of the curve along which the car is moving
    radius = 4.15
    
    # Initialization of arrays for x, y and yaw values
    path_x, path_y, yaw = [], [], []

    # start, goal denote the source and goal states or two consecutive waypoints respectively
    # deg2rad() coverts given yaw degree value into radian
    for i in range(len(states) - 1):
        start_x = states[i][0]
        start_y = states[i][1]
        start_yaw = np.deg2rad(states[i][2])
        goal_x = states[i + 1][0]
        goal_y = states[i + 1][1]
        goal_yaw = np.deg2rad(states[i + 1][2])

        # call calc_dubins_path() method with arguments like x, y and yaw values of source and goal waypoints and maximum curvature
        # compute the dubin path between two consecutive states or waypoints
        path_i = calc_dubins_path(start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, max_curv, radius)

        # append all the x, y and yaw values between two consecutive waypoints to the path_x, path_y and yaw arrays respectively
        for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):
            path_x.append(x)
            path_y.append(y)
            yaw.append(iyaw)

    # show the animation of computed dubin path
    # Interactive mode is on
    plt.ion()

    # plot a new figure
    plt.figure()

    # for each waypoint
    # clf() function clears the current figure
    # plot() function show the dubin curve
    for i in range(len(path_x)):
        plt.clf()
        plt.plot(path_x, path_y, linewidth=1, color='gray')

        # for each waypoint or state draw an arrow with given arguments like x, y, yaw, length and color values
        for x, y, theta in states:
            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')

        # for each state draw a continuously moving car with arguments like x, y, yaw , width and length values
        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)

        # plot the figure with equal length axis
        # setting the x axis to -10 to 42 and y axis to -20 to 20 via axis() function
        # draw() function updates a figure that has been altered
        # pause() function show the figure for given interval seconds
        plt.axis("equal")
        plt.title("Simulation of Dubins Path")
        plt.axis([-10, 42, -35, 20])
        plt.draw()
        plt.pause(0.001)

    plt.pause(1)


if __name__ == '__main__':
    main()
```

4eb73f9b708a0584f42af2d6928b2d09c7065773
ipynb
Jupyter Notebook
_posts/2D_Dubin_Path_Modified.ipynb
abbhicse/contrast
56af86b07877305d1ba6286177a30beee04dbe2a
Unlicense
_posts/2D_Dubin_Path_Modified.ipynb
abbhicse/contrast
56af86b07877305d1ba6286177a30beee04dbe2a
Unlicense
_posts/2D_Dubin_Path_Modified.ipynb
abbhicse/contrast
56af86b07877305d1ba6286177a30beee04dbe2a
Unlicense
Qwen/Qwen-72B
1. YES
2. YES