-
Notifications
You must be signed in to change notification settings - Fork 1
/
simulation_closedform_linsys.py
162 lines (126 loc) · 5 KB
/
simulation_closedform_linsys.py
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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
import matplotlib.pyplot as plt
import math
import numpy as np
from numpy import linalg as LA
from scipy.integrate import solve_ivp
from gurobipy import *
"""
Created on Mon Oct 24 9:18 PM
@author: Guang Yang
This code is a rewritten version of the original CBF-RRT paper with
linear dynamics
"""
class Obstacle_Sphere(object):
def __init__(self, center, radius):
self.T = 1.0 #Integration Length
self.N = 100 # Number of Control Updates
self.center=center
self.radius=radius
def h(self,x):
return LA.norm(x-self.center,2)**2-self.radius**2
def gradh(self,x):
return 2*(x-self.center)
def hdot(self,x,xdot):
return self.gradh(x).transpose().dot(xdot)
def fun_derivative_trajectory(x,dx,f,gradf):
fx=np.apply_along_axis(f,0,x)
gradfx=np.apply_along_axis(gradf,0,x)
dfx=np.sum(gradfx*dx,0)
#plot fx and dfx
t_span = np.linspace(0,7.0,100)
fig, ax = plt.subplots()
ax.plot(t_span, -fx,'r',label="-h")
#ax.plot(t_span, gradfx[0,:],'g')
ax.plot(t_span, dfx,'b',label="h_dot")
ax.set_xlabel("Time")
ax.legend()
plt.show()
class CBF_RRT:
def __init__(self, initial_state, obstacle_list):
self.t0 = 0 # Starting time
self.T = 1.0 #Integration Length
self.N = 100 # Number of Control Updates
self.y0 = initial_state
self.k_cbf = 1.0 #CBF coefficient
self.p_cbf = 1 #CBF constraint power
self.x_obstacle = obstacle_list
self.u1_lower_lim = -5
self.u1_upper_lim = 5
self.u2_lower_lim = -5
self.u2_upper_lim = 5
self.u1_traj = np.zeros(shape=(0,0))
self.u2_traj = np.zeros(shape=(0,0))
self.x1_traj = np.zeros(shape=(0,0))
self.x2_traj = np.zeros(shape=(0,0))
self.cbf_traj = np.zeros(shape=(0,0))
self.hdot_traj = np.zeros(shape=(0,0))
self.h_traj = np.zeros(shape=(0,0))
def QP_controller(self,x_current,u_ref):
self.m = Model("CBF_CLF_QP")
x1 = x_current[0]
x2 = x_current[1]
self.m.remove(self.m.getConstrs())
u1_ref = u_ref[0]
u2_ref = u_ref[1]
# Control Acutuator Constraints
self.u1 = self.m.addVar(lb=self.u1_lower_lim, ub=self.u1_upper_lim,
vtype=GRB.CONTINUOUS, name="velocity_constraint_x1")
self.u2 = self.m.addVar(lb=self.u2_lower_lim, ub=self.u2_upper_lim,
vtype=GRB.CONTINUOUS, name="velocity_constraint_x2")
# Initialize Cost Function
self.cost_func = (self.u1-u1_ref)*(self.u1-u1_ref)+(self.u2-u2_ref)*(self.u2-u2_ref)
self.m.setObjective(self.cost_func, GRB.MINIMIZE)
# CBF function
for i in range(0,len(self.x_obstacle)):
h = (x1-self.x_obstacle[i][0])**2+(x2-self.x_obstacle[i][1])**2-self.x_obstacle[i][2]**2
lgh = 2*(x1-self.x_obstacle[i][0])*self.u1+2*(x2-self.x_obstacle[i][1])*self.u2
self.m.addConstr((lgh+self.k_cbf*h**self.p_cbf)>=0)
#Stop optimizer from publsihing results to console - remove if desired
self.m.Params.LogToConsole = 0
#Solve the optimization problem
self.m.optimize()
self.solution = self.m.getVars()
#self.m.write("Safe_RRT_Forward.lp")
# get the results of decision variables
u1 = self.solution[0].x
u2 = self.solution[1].x
'''
h_temp = (x1-self.x_obstacle[i][0])**2+(x2-self.x_obstacle[i][1])**2-self.x_obstacle[i][2]**2
lgh_temp = 2*(x1-self.x_obstacle[i][0])*u1+2*(x2-self.x_obstacle[i][1])*u2
self.cbf_traj = np.append(self.cbf_traj,(lgh_temp+self.k_cbf*h_temp**self.p_cbf))
self.hdot_traj = np.append(self.hdot_traj,(lgh_temp))
self.h_traj = np.append(self.h_traj,(self.k_cbf*h_temp**1))
'''
return np.array([[u1],[u2]])
def motion_planning(self,u_ref):
x_current = self.y0
x = np.zeros((2,0))
u = np.zeros((2,0))
delta_t = self.T/self.N
for i in range(0,self.N):
x=np.hstack((x,x_current))
u_current = self.QP_controller(x_current[:,0],u_ref)
u=np.hstack((u,u_current))
x_current=x_current+delta_t*u_current
return (x,u)
def plot_traj(self,x,u):
#t_span = np.linspace(0,self.T,self.N)
fig, ax = plt.subplots()
circle = plt.Circle((obstacle_list[0][0], obstacle_list[0][1]),
obstacle_list[0][2], color='r',alpha=0.2)
ax.add_artist(circle)
ax.plot(x[0,:], x[1,:])
ax.set_xlim(-1,5)
ax.set_ylim(-1,5)
ax.set_xlabel("x1")
ax.set_ylabel("x2")
plt.show()
if __name__ == "__main__":
initial_state = np.array([[1.0],[1.0]])
obstacle_list = [[2.9,2.6,0.5]]
u_ref = [0.5,0.5]
CBFRRT_Planning = CBF_RRT(initial_state, obstacle_list)
x, u= CBFRRT_Planning.motion_planning(u_ref)
CBFRRT_Planning.plot_traj(x,u)
#Sphere = Obstacle_Sphere([obstacle_list[0][0],obstacle_list[0][1]],obstacle_list[0][2])
#fun_derivative_trajectory(x,u,Sphere.h,Sphere.gradh)