Optimal-Control-Non-Linear-Programming/utils.py at main · ncussonn/Optimal-Control-Non-Linear-Programming · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
import numpy as np
from casadi import *
from numpy import sin, cos, pi
import matplotlib.pyplot as plt
from matplotlib import animation
from time import time

def visualize(car_states, ref_traj, obstacles, t, time_step, save=False):
    init_state = car_states[0,:]
    def create_triangle(state=[0,0,0], h=0.5, w=0.25, update=False):
        x, y, th = state
        triangle = np.array([
            [h, 0   ],
            [0,  w/2],
            [0, -w/2],
            [h, 0   ]
        ]).T
        rotation_matrix = np.array([
            [cos(th), -sin(th)],
            [sin(th),  cos(th)]
        ])

        coords = np.array([[x, y]]) + (rotation_matrix @ triangle).T
        if update == True:
            return coords
        else:
            return coords[:3, :]

    def init():
        return path, current_state, target_state,

    def animate(i):
        # get variables
        x = car_states[i,0]
        y = car_states[i,1]
        th = car_states[i,2]

        # update path
        if i == 0:
            path.set_data(np.array([]), np.array([]))
        x_new = np.hstack((path.get_xdata(), x))
        y_new = np.hstack((path.get_ydata(), y))
        path.set_data(x_new, y_new)

        # update horizon
        #print(car_states[0, :, i])
        #x_new = car_states[0, :, i]
        #y_new = car_states[1, :, i]
        #horizon.set_data(x_new, y_new)

        # update current_state
        current_state.set_xy(create_triangle([x, y, th], update=True))

        # update current_target
        x_ref = ref_traj[i,0]
        y_ref = ref_traj[i,1]
        th_ref = ref_traj[i,2]
        target_state.set_xy(create_triangle([x_ref, y_ref, th_ref], update=True))

        # update target_state
        # xy = target_state.get_xy()
        # target_state.set_xy(xy)

        return path, current_state, target_state,
    circles = []
    for obs in obstacles:
        circles.append(plt.Circle((obs[0], obs[1]), obs[2], color='r', alpha = 0.5))
    # create figure and axes
    fig, ax = plt.subplots(figsize=(6, 6))
    min_scale_x = min(init_state[0], np.min(ref_traj[:,0])) - 1.5
    max_scale_x = max(init_state[0], np.max(ref_traj[:,0])) + 1.5
    min_scale_y = min(init_state[1], np.min(ref_traj[:,1])) - 1.5
    max_scale_y = max(init_state[1], np.max(ref_traj[:,1])) + 1.5
    ax.set_xlim(left = min_scale_x, right = max_scale_x)
    ax.set_ylim(bottom = min_scale_y, top = max_scale_y)
    for circle in circles:
        ax.add_patch(circle)
    # create lines:
    #   path
    path, = ax.plot([], [], 'k', linewidth=2)

    #   current_state
    current_triangle = create_triangle(init_state[:3])
    current_state = ax.fill(current_triangle[:, 0], current_triangle[:, 1], color='r')
    current_state = current_state[0]
    #   target_state
    target_triangle = create_triangle(ref_traj[0,0:3])
    target_state = ax.fill(target_triangle[:, 0], target_triangle[:, 1], color='b')
    target_state = target_state[0]

    #   reference trajectory
    ax.scatter(ref_traj[:,0], ref_traj[:,1], marker='x')

    sim = animation.FuncAnimation(
        fig=fig,
        func=animate,
        init_func=init,
        frames=len(t),
        interval=time_step*100,
        blit=True,
        repeat=True
    )
    plt.show()

    if save == True:
        sim.save('./fig/animation' + str(time()) +'.gif', writer='ffmpeg', fps=15)

    return


# Constraints
def motionModel(cur_ref_traj, next_ref_traj, e, u):

    delta = 0.5

    v = u[0]
    w = u[1]
    theta_tilde = e[2]

    # r and alpha
    x = cur_ref_traj[0]
    y = cur_ref_traj[1]
    alpha = cur_ref_traj[2]

    # r_next and alpha_next
    x_next = next_ref_traj[0]
    y_next = next_ref_traj[1]
    alpha_next = next_ref_traj[2]

    # Next Error must be within bound
    e_constraint_0 = e[0] + delta*cos(theta_tilde + alpha)*v + (x - x_next)
    e_constraint_1 = e[1] + delta*sin(theta_tilde + alpha)*v + (y - y_next)
    e_constraint_2 = e[2] + delta*w + (alpha - alpha_next)

    e_new = vertcat(e_constraint_0, e_constraint_1, e_constraint_2)

    return e_new