# Exercise 3: formation control


```python
# header to start
%matplotlib notebook
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib as mp
import math
import pickle
import IPython
import scipy.linalg
```


```python
def getLaplacian(E, n_vertices, directed=False):
    L = np.zeros((n_vertices, n_vertices))
    for e in E:
        if directed:
            L[e[1]][e[1]] += 1
            L[e[1]][e[0]] = -1
        else:
            L[e[1]][e[1]] += 1
            L[e[0]][e[0]] += 1
            L[e[1]][e[0]] = -1
            L[e[0]][e[1]] = -1
    return L

def getIncidence(E, n_vertices):
    D = np.zeros((n_vertices, len(E)))
    for i, e in enumerate(E):
        D[e[0]][i] = -1
        D[e[1]][i] = 1
    return D

def getMSD(x, y, E):
    MSD = np.zeros((len(E),))
    for i in range(len(E)):
        e = E[i]
        x1 = x[e[0]]
        x2 = x[e[1]]
        y1 = y[e[0]]
        y2 = y[e[1]]
        
        d = np.power(x1-x2, 2) + np.power(y1-y2, 2)
        d = np.sqrt(d)
        MSD[i] = d
        
    return MSD
```

## Helper function to display results

This function can be used to display the behavior of the robots in 2D


```python
def make_animation(plotx,E,xl=(-2,2),yl=(-2,2),inter=25, display=False):
    '''
    takes a graph and motion of vertexes in 2D and returns an animation
    E: list of edges (each edge is a pair of vertexes)
    plotx: a matrix of states ordered as (x1, y1, x2, y2, ..., xn, yn) in the rows and time in columns
    xl and yl define the display boundaries of the graph
    inter is the interval between each point in ms
    '''
    fig = mp.figure.Figure()
    mp.backends.backend_agg.FigureCanvasAgg(fig)
    ax = fig.add_subplot(111, autoscale_on=False, xlim=xl, ylim=yl)
    ax.grid()

    list_of_lines = []
    for i in E: #add as many lines as there are edges
        line, = ax.plot([], [], 'o-', lw=2)
        list_of_lines.append(line)

    def animate(i):
        for e in range(len(E)):
            vx1 = plotx[2*E[e][0],i]
            vy1 = plotx[2*E[e][0]+1,i]
            vx2 = plotx[2*E[e][1],i]
            vy2 = plotx[2*E[e][1]+1,i]
            list_of_lines[e].set_data([vx1,vx2],[vy1,vy2])
        return list_of_lines
    
    def init():
        return animate(0)


    ani = animation.FuncAnimation(fig, animate, np.arange(0, len(plotx[0,:])),
        interval=inter, blit=True, init_func=init)
    plt.close(fig)
    plt.close(ani._fig)
    if(display==True):
        IPython.display.display_html(IPython.core.display.HTML(ani.to_html5_video()))
    return ani
```

We wish the control the formation of 4 robots randomly distributed in the environment to keep the formation shown in the figure of Exercise 2. 

## Question 1
Assume each agent has state space dynamics $\dot{\mathbf{p}}_i =\mathbf{u}_i$, with $\mathbf{u}_i$ in $\mathbb{R}^2$ and $\mathbf{u} = [\mathbf{u}_1, \mathbf{u}_2, \mathbf{u}_3, \mathbf{u}_4]$.

Implement the second order linear control law seen in the class
$$\mathbf{u} = -k \mathbf{L} \mathbf{x} + k \mathbf{D z}_{ref}$$

where $k>0$ is a positive gain and $\mathbf{D}$ is the incidence matrix of the graph.


Simulate the control law for several random initial conditions of the agents (in 2D). What do you observe? How does it compare to the same control law but for a framework with a complete graph?


```python
with open('example_animation.pickle', 'rb') as f:
    data = pickle.load(f)
E = data['E']
n_vertices = 4
L = getLaplacian(E, n_vertices, directed=False)
D = getIncidence(E, n_vertices)

Px_des = np.array([0, 1, 1, (2-math.sqrt(14))/4]).T
Py_des = np.array([0, 0, -1, -(math.sqrt(14)+2)/4]).T

x = np.array([0, 0.5, -0.3, -0.1]).T
y = np.array([0.1, 0.2, 0.3, 0.1]).T

K = 2 * np.eye(4)

xs = x
ys = y
ts = [0]
t = 0
dt = 0.001

while t < 10:
    zx_ref = np.matmul(D.T, Px_des)
    dx = -np.matmul(K, np.matmul(L, x)) + np.matmul(K, np.matmul(L, Px_des))
    zy_ref = np.matmul(D.T, Py_des)
    dy = -np.matmul(K, np.matmul(L, y)) + np.matmul(K, np.matmul(L, Py_des))
    x += dt*dx
    y += dt*dy
    xs = np.vstack((xs, x))
    ys = np.vstack((ys, y))
    t += dt
    ts.append(t)

data = np.zeros((0, xs.shape[0]))
for i in range(xs.shape[1]):
    data = np.vstack((data, xs[:, i].T))
    data = np.vstack((data, ys[:, i].T))

plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```








    <matplotlib.animation.FuncAnimation at 0x7fb15a96acf8>



### Complete Graph


```python
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3], [1, 3]]
n_vertices = 4
L = getLaplacian(E, n_vertices, directed=False)
D = getIncidence(E, n_vertices)

Px_des = np.array([0, 1, 1, (2-math.sqrt(14))/4]).T
Py_des = np.array([0, 0, -1, -(math.sqrt(14)+2)/4]).T

x = np.array([0, 0.5, -0.3, -0.1]).T
y = np.array([0.1, 0.2, 0.3, 0.1]).T

K = 2 * np.eye(4)

xs = x
ys = y
ts = [0]
t = 0
dt = 0.001

while t < 10:
    zx_ref = np.matmul(D.T, Px_des)
    dx = -np.matmul(K, np.matmul(L, x)) + np.matmul(K, np.matmul(L, Px_des))
    zy_ref = np.matmul(D.T, Py_des)
    dy = -np.matmul(K, np.matmul(L, y)) + np.matmul(K, np.matmul(L, Py_des))
    x += dt*dx
    y += dt*dy
    xs = np.vstack((xs, x))
    ys = np.vstack((ys, y))
    t += dt
    ts.append(t)

data = np.zeros((0, xs.shape[0]))
for i in range(xs.shape[1]):
    data = np.vstack((data, xs[:, i].T))
    data = np.vstack((data, ys[:, i].T))

plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```








    <matplotlib.animation.FuncAnimation at 0x7fb17e3d5860>



The control law for both the incomplete and complete graph renders the same result.

## Question 2
Assume each agent has state space dynamics $\dot{\mathbf{p}}_i =\mathbf{u}_i$, with $\mathbf{u}_i$ in $\mathbb{R}^2$ and $\mathbf{u} = [\mathbf{u}_1, \mathbf{u}_2, \mathbf{u}_3, \mathbf{u}_4]$.

We now consider the following control law
$$\begin{equation}
\mathbf{u} = \mathbf{R}_\mathcal{G}^T(\mathbf{p}) (\mathbf{g}_d - \mathbf{g}_\mathcal{G}(\mathbf{p}))
\end{equation}$$
where $\mathbf{R}_\mathcal{G}$ is the rigidity matrix associated to the graph of the framework, $\mathbf{g}_d$ is the vector of desired square distance between agents and $\mathbf{g}_\mathcal{G}$ is the measured square distance between each agent.

Simulate the control law for several random initial conditions of the agents (in 2D). What do you observe? How does it compare to the same control law but for a framework with a complete graph?


```python
gd = np.array([1, 1, math.sqrt(2), 1.5, 1.5])
R = np.array([[-2, 0, 2, 0, 0, 0, 0, 0],
                  [0, 0, 0, 2, 0, -2, 0, 0],
                  [-2, 2, 0, 0, 2, -2, 0, 0],
                  [(math.sqrt(14)-2)/2, (math.sqrt(14)+2)/2, 0, 0,
                   0, 0, (2-math.sqrt(14))/2, -(math.sqrt(14)+2)/2],
                  [0, 0, 0, 0, (2+math.sqrt(14))/2, (math.sqrt(14)-2)/2, 
                   -(2+math.sqrt(14))/2, (2-math.sqrt(14))/2]])

p = np.array([0, 0.1, 0.5, 0.2, -0.3, 0.3, -0.1, 0.1]).T
ps = p
ts = [0]
t = 0
dt = 0.001
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3]]

while t < 10:
    x = [p[0], p[2], p[4], p[6]]
    y = [p[1], p[3], p[5], p[7]]
    u = np.matmul(R.T, gd-getMSD(x, y, E))
    p += dt*u
    ps = np.vstack((ps, p))
    t += dt
    ts.append(t)

data = ps.T
plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```








    <matplotlib.animation.FuncAnimation at 0x7fb140f85ef0>




```python
### Complete Graph
```


```python
gd = np.array([1, 1, math.sqrt(2), 1.5, 1.5, math.sqrt(2)*(math.sqrt(14)+2)/4])
R = np.array([[-2, 0, 2, 0, 0, 0, 0, 0],
              [0, 0, 0, 2, 0, -2, 0, 0],
              [-2, 2, 0, 0, 2, -2, 0, 0],
              [(math.sqrt(14)-2)/2, (math.sqrt(14)+2)/2, 0, 0,
               0, 0, (2-math.sqrt(14))/2, -(math.sqrt(14)+2)/2],
              [0, 0, 0, 0, (2+math.sqrt(14))/2, (math.sqrt(14)-2)/2, 
               -(2+math.sqrt(14))/2, (2-math.sqrt(14))/2],
              [0, 0, (math.sqrt(14)+2)/4, (math.sqrt(14)+2)/4, 0, 0, -(math.sqrt(14)+2)/4, -(math.sqrt(14)+2)/4]])
p = np.array([0, 0.1, 0.5, 0.2, -0.3, 0.3, -0.1, 0.1]).T
ps = p
ts = [0]
t = 0
dt = 0.001
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3], [1, 3]]

while t < 10:
    x = [p[0], p[2], p[4], p[6]]
    y = [p[1], p[3], p[5], p[7]]
    u = np.matmul(R.T, gd-getMSD(x, y, E))
    p += dt*u
    ps = np.vstack((ps, p))
    t += dt
    ts.append(t)

data = ps.T
plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```








    <matplotlib.animation.FuncAnimation at 0x7fb140ebe358>



## Question 3

How would you compare both control laws? What are the pros and cons of each of them?

The first controller is only rotation invariant, while the second one is neither. This can be seen from the controller definition, we see that for the first controller it can also be written as,
$$\dot{P}_x = -KLP_x - KLP_x^{\mathrm{des}}$$
$$\dot{P}_y = -KLP_y - KLP_y^{\mathrm{des}}$$
we can see that as long as the relative $x$ and $y$ distance are the same as desired the controller will output 0, which disregards the abosolute translation of the system.

For the second controller for the stable system all required is the distance between vertices are equal to the desired distance thus it is rotation and translation invariant.


```python
# for example assume that you have simulated a formation control in 2D and stored the data in a file
# we load the data needed for the display
with open('example_animation.pickle', 'rb') as f:
    data = pickle.load(f)

# this is the list of edges (as we usually define them for an undirected graph)
E = data['E']
print('the list of edges is:')
print(E)

# this is the time of simulation
t = data['t']

# this is an array containing the evolution of the states of the robot
# x[0,:] contains the time evolution of the x variable of robot 1
# x[1,:] contains the time evolution of the y variable of robot 1
# x[2,:] contains the time evolution of the x variable of robot 2
# etc
x = data['x']

# since we simulated with a small delta t = 0.001, we want to subsample for display
# we just take data every 50ms
plotx = x[:,::50]

make_animation(plotx, E, inter=50, display=True)

# a video showing the behavior of the robots and the connection between the robots should be displayed below
```

    the list of edges is:
    [[0, 1], [0, 2], [0, 3], [1, 2], [2, 3]]









    <matplotlib.animation.FuncAnimation at 0x7fb140ec5b70>



8df8fd06056394cf900cd12d4cb37303f8062364
ipynb
Jupyter Notebook
series3/Exercise3.ipynb
BolunDai0216/ConsensusControl
12f36fa3a70897b9e6cbcdab19734ca8360211a5
MIT
series3/Exercise3.ipynb
BolunDai0216/ConsensusControl
12f36fa3a70897b9e6cbcdab19734ca8360211a5
MIT
series3/Exercise3.ipynb
BolunDai0216/ConsensusControl
12f36fa3a70897b9e6cbcdab19734ca8360211a5
MIT
Qwen/Qwen-72B
1. YES
2. YES