def get_clf_controller( x,t):
LgV = dynamics.get_LgV( x,t)
LfV = dynamics.get_LfV( x,t)
V = dynamics.get_V( x,t)
lambda_v = util.get_stabilization_rate()
# cvxpy
u = cp.Variable(param.get('m'))
delta_v = cp.Variable()
constraints = [
LfV + LgV*u + lambda_v * V <= delta_v,
cp.abs( u) <= param.get('control_max'),
delta_v >= 0
]
obj = cp.Minimize( \
cp.sum_squares(u) + \
param.get('p_v') * delta_v \
)
prob = cp.Problem(obj, constraints)
prob.solve(verbose = False, solver = cp.GUROBI)
u = np.array(u.value)
return u
def debug_scp_iteration_plot( tx_next, u_next, xbar, ubar, x0, T, i_iter):
unl = u_next
x_curr = x0
Xnl = []
Vnl_nlx = []
Vnl_lx = []
tV_nlx = []
tV_lx = []
for k,t in enumerate(T):
x_next = x_curr + dynamics.get_dxdt( x_curr, unl[:,k], t) * param.get('dt')
R_k, w_k = dynamics.get_linear_lyapunov( xbar[:,k], ubar[:,k], t)
Vnl_nlx.append( dynamics.get_V( x_curr, t))
Vnl_lx.append( dynamics.get_V( tx_next[:,k],t))
tV_nlx.append( np.matmul( R_k, x_curr) + w_k )
tV_lx.append( np.matmul( R_k, tx_next[:,k]) + w_k)
Xnl.append( x_curr)
x_curr = x_next
Xnl = np.asarray(Xnl)
Vnl_nlx = np.asarray(Vnl_nlx)
Vnl_lx = np.asarray(Vnl_lx)
tV_nlx = np.asarray(tV_nlx)
tV_lx = np.asarray(tV_lx)
plot_scp_iteration_state( Xnl, np.transpose(tx_next,(1,0,2)), \
np.transpose(xbar,(1,0,2)), T, title = str(param.get('controller')) + ' State' + \
'\nIteration: ' + str(i_iter) + '\nTime: ' + str(T[0]))
plot_scp_iteration_lyapunov( np.squeeze(Vnl_nlx), np.squeeze(Vnl_lx), np.squeeze( tV_nlx), \
np.squeeze( tV_lx), T, title = str(param.get('controller')) + ' Lyapunov' + \
'\nIteration: ' + str(i_iter) + '\nTime: ' + str(T[0]))
def get_linear_dynamics( xbar, ubar, t):
def prod(dgdx, u):
s = 0
for i in range(len(u)):
s += dgdx[:,i,:]*u[i]
return np.squeeze(s)
dfdx = get_dfdx(xbar,t)
dgdx = get_dgdx(xbar)
g = get_g(xbar)
f = get_f(xbar)
# continuous time
F = dfdx + prod(dgdx,ubar)
B = g
d = f + np.dot( g, ubar) \
- np.dot( dfdx, xbar) \
- np.dot( prod( dgdx, ubar), xbar) \
- np.dot( g, ubar)
# discrete time
F_k = np.eye(param.get('n')) + F*param.get('dt')
B_k = B*param.get('dt')
d_k = d*param.get('dt')
return F_k, B_k, d_k
def get_T(x,t):
# add phase shift for all agents
T = param.get('radius_d')[0]*util.get_R(param.get('phase_d')[0])
for i in range(1,param.get('na')):
r_i = param.get('radius_d')[i]
phi_i = param.get('phase_d')[i]
T = block_diag(T, r_i*util.get_R(phi_i))
return T
def get_f(x,t):
# single integrator dynamics
pi_pv = util.permute_to_pv()
I = np.eye(param.get('nd')*param.get('ni'))
F = np.vstack((
np.hstack((0.*I,1.*I)),
np.hstack((0.*I,0.*I))))
return np.dot(np.dot(np.dot(
pi_pv.T, F), pi_pv), x)
def permute_to_pv():
pi = np.zeros((2*param.get('ni'),2*param.get('ni')))
for i in range(param.get('ni')):
p1_idx = 2*i
v1_idx = p1_idx + 1
p2_idx = i
v2_idx = i + param.get('ni')
pi[p2_idx, p1_idx] = 1
pi[v2_idx, v1_idx] = 1
return np.kron( pi, np.eye(param.get('nd')))
def get_min_dist(x):
min_dist = np.inf
for i in range(param.get('ni')):
pose_i = np.dot(get_p_i(i), x)
for j in range(param.get('ni')):
if i is not j:
pose_j = np.dot(get_p_i(j), x)
dist = np.linalg.norm(pose_i - pose_j)
if dist < min_dist:
min_dist = dist
return min_dist
def get_pd_t(t):
if param.get('case_xd') == 0:
return np.ones((2, 1))
elif param.get('case_xd') == 1:
return t * np.ones((2, 1))
elif param.get('case_xd') == 2:
return np.array([
np.cos(2 * np.pi / param.get('tf') * t),
np.sin(2 * np.pi / param.get('tf') * t)
])
return
def get_plot_lim(X, T):
xmin = np.inf
ymin = np.inf
xmax = -np.inf
ymax = -np.inf
# check agents
for t in range(len(T)):
x = X[t]
for i in range(param.get('ni')):
pose_i = get_p(x, i)
if xmin > pose_i[0]:
xmin = pose_i[0]
if xmax < pose_i[0]:
xmax = pose_i[0]
if ymin > pose_i[1]:
ymin = pose_i[1]
if ymax < pose_i[1]:
ymax = pose_i[1]
# check desired
for t in range(len(T)):
pose_d = param.get('pd')[t, :]
if xmin > pose_d[0]:
xmin = pose_d[0]
if xmax < pose_d[0]:
xmax = pose_d[0]
if ymin > pose_d[1]:
ymin = pose_d[1]
if ymax < pose_d[1]:
ymax = pose_d[1]
# add buffer
if xmin < 0:
xmin = xmin * (1 + param.get('plot_buffer'))
else:
xmin = xmin * (1 - param.get('plot_buffer'))
if ymin < 0:
ymin = ymin * (1 + param.get('plot_buffer'))
else:
ymin = ymin * (1 - param.get('plot_buffer'))
if xmax < 0:
xmax = xmax * (1 - param.get('plot_buffer'))
else:
xmax = xmax * (1 + param.get('plot_buffer'))
if ymax < 0:
ymax = ymax * (1 - param.get('plot_buffer'))
else:
ymax = ymax * (1 + param.get('plot_buffer'))
return xmin, xmax, ymin, ymax
def main():
device = "cpu"
model = GainsNet2()
train_dataset = make_dataset()
test_dataset = make_dataset()
for epoch in range(param.get('gains_epochs')):
print('Epoch: ', epoch + 1)
train(model, train_dataset)
test(model, test_dataset)
torch.save(model, param.get('gains_model_fn'))
def train(model, loader):
optimizer = torch.optim.Adam(model.parameters(), lr=param.get('gains_lr'))
loss_func = torch.nn.MSELoss() # this is for regression mean squared loss
epoch_loss = 0
for step, (b_x, b_y) in enumerate(loader): # for each training step
prediction = model(b_x) # input x and predict based on x
loss = loss_func(prediction, b_y) # must be (1. nn output, 2. target)
optimizer.zero_grad() # clear gradients for next train
loss.backward() # backpropagation, compute gradients
optimizer.step() # apply gradients
epoch_loss += loss
print(' Train Epoch Loss: ',
epoch_loss / step / param.get('gains_batch_size'))
def make_gif(X,T):
fig, ax = plt.subplots()
xmin,xmax,ymin,ymax = util.get_plot_lim(X,T)
def animate(i):
plt.cla()
ax.set_xlim(xmin,xmax)
ax.set_ylim(ymin,ymax)
plt.title('State Space at t = ' + str(np.round(T[i],2)))
return plot_ss(X[i],i,scat),
scat = plt.scatter([],[])
step = np.round(len(T)/param.get('nframes_gif'))
anim = FuncAnimation(fig, animate, np.arange(0,len(T),step,int))
anim.save( param.get('fn_gif'))
def plot_U(U,T, title = 'Control Input'):
fig, ax = plt.subplots()
for i in range(param.get('m')):
ax.plot(T,U[:,i],label = 'U' + str(i))
plt.title(title)
plt.legend()
def get_dynamics_u(x,t):
# get
A = dynamics.get_A(x)
pi_pv = util.permute_to_pv()
# formation task assignment
pi_ta_nd = np.kron(ta.get_centralized_ta(x,t), \
np.eye(param.get('nd')))
pi_ta_dof = np.kron(ta.get_centralized_ta(x,t), \
np.eye(param.get('dof')))
T = np.dot(np.dot(
pi_ta_nd.T, dynamics.get_T(x,t)), pi_ta_nd)
Tv = np.dot(np.dot(
pi_ta_dof.T, dynamics.get_Tv(x,t)), pi_ta_dof)
# leader/basis
x_l = dynamics.get_xl(x,t)
x_b = dynamics.get_xb(x,t)
my_1 = np.kron( np.ones((param.get('ni'),1)),\
np.eye(param.get('dof')))
# transform state into global coordinate frame
z = x - np.dot(my_1,x_l) - \
np.dot(np.dot(Tv, my_1),x_b)
# Laplacian
L = dynamics.get_L(x)
L = np.dot(
np.kron(L, np.eye(param.get('nd'))), T)
I = np.eye(param.get('ni')*param.get('nd'))
U = np.dot(np.dot(
np.hstack((-param.get('k1')*(L+I), -param.get('k2')*(L+I))),
pi_pv), z)
# collision avoidance
# not implemented
# for i in range(param.get('ni')):
# p_i = np.dot(util.get_p_i(i),x)
# for j in range(param.get('ni')):
# p_j = np.dot(util.get_p_i(j),x)
# dist = np.linalg.norm( p_i - p_j)
# idx = i*param.get('nd') + np.arange(0, param.get('nd'))
# U[idx] = U[idx] - \
# 0*(param.get('k_c')*(p_j-p_i)/(dist*(dist - param.get('R_safe'))))
return U
def get_A(x):
# Adjacency matrix
P = np.dot(util.get_p(),x)
X = np.dot(
np.ones((param.get('ni'),1)),
util.to_vec(P[np.mod(np.arange(0,len(P)),2)==False]).T)
Y = np.dot(
np.ones((param.get('ni'),1)),
util.to_vec(P[np.mod(np.arange(0,len(P)),2)==True]).T)
D = np.sqrt(
np.power( X - X.T + 1e-9,2) +
np.power( Y - Y.T + 1e-9,2))
A = np.exp( -param.get('lambda_a')*D) * \
1.0 * (D < param.get('R_comm'))
A = A / np.linalg.norm(A, ord=2, axis=1)
return A
def get_vdot_a( x):
for i in range(param.get('na')):
try:
vdot_a = np.vstack( (vdot_a, reynolds( x,i)))
except:
vdot_a = reynolds( x,i)
return vdot_a
def save_figs():
fn = os.path.join( os.getcwd(), param.get('fn_plots'))
pp = PdfPages(fn)
for i in plt.get_fignums():
pp.savefig(plt.figure(i))
plt.close(plt.figure(i))
pp.close()
def plot_ss(x,k,scat):
scat_x = []
scat_y = []
scat_c = []
# plot agents
for i in range(param.get('ni')):
p_i = util.get_p(x,i)
scat_x.append(p_i[0])
scat_y.append(p_i[1])
if i < param.get('na'):
scat_c.append(param.get('FreeAgentColor'))
else:
scat_c.append(param.get('ControlAgentColor'))
# plot centroid
my_1 = util.get_my_1()
p_a = util.get_p_a(x)
p_c = np.matmul( np.transpose( my_1), p_a)
scat_x.append( p_c[0])
scat_y.append( p_c[1])
scat_c.append( param.get('CentroidColor'))
# plot desired
xd = param.get('pd')[k,:]
scat_x.append( xd[0])
scat_y.append( xd[1])
scat_c.append( param.get('DesiredTrajectoryColor'))
return plt.scatter(np.asarray(scat_x), np.asarray(scat_y),
color = scat_c)
def make_dataset():
model = torch.load(param.get('rl_model_fn'))
states = []
actions = []
for _ in range(param.get('gains_n_data')):
state = array((
param.get('sys_pos_bounds') * uniform(-1., 1.),
param.get('sys_angle_bounds_deg') * uniform(-1., 1.),
2. * uniform(-1., 1.),
2. * uniform(-1., 1.),
))
prob = model.pi(torch.from_numpy(state).float())
m = Categorical(prob)
action = array(param.get('sys_actions')[m.sample().item()], ndmin=1)
states.append(state)
actions.append(action)
return torch.tensor(states).float(), torch.tensor(actions).float()
def test(model, loader):
loss_func = torch.nn.MSELoss() # this is for regression mean squared loss
epoch_loss = 0
for step, (b_x, b_y) in enumerate(loader): # for each training step
prediction = model(b_x) # input x and predict based on x
loss = loss_func(prediction, b_y) # must be (1. nn output, 2. target)
epoch_loss += loss
print(' Test Epoch Loss: ',
epoch_loss / step / param.get('gains_batch_size'))
def plot_V(V,T,title = 'Lyapunov Convergence'):
fig, ax = plt.subplots()
Vdot = np.gradient(V, param.get('dt'), axis = 0)
ax.plot(T,np.squeeze(V),label = 'V')
ax.plot(T,np.squeeze(Vdot),label = 'Vdot')
plt.title(title)
plt.legend()
ax.grid(True)
def reynolds( x, i):
A = util.get_A(x)
p_i = np.dot( util.get_p_i(i), x)
v_i = np.dot( util.get_v_i(i), x)
a_i = np.zeros((param.get('nd'),1))
for j in range(param.get('ni')):
if i is not j:
p_j = np.dot( util.get_p_i(j), x)
v_j = np.dot( util.get_v_i(j), x)
r_ij = p_j - p_i
dist = np.linalg.norm(r_ij)
a_i = a_i + A[i,j]*( \
param.get('kv')*(v_j - v_i) + \
param.get('kx')*r_ij*(1 - param.get('R_des')/dist)
)
return a_i
def get_f(x):
# drift dynamics
if param.get('model') is 'reynolds':
# free agents
for i in range(param.get('na')):
v_i = np.dot( util.get_v_i(i), x)
a_i = reynolds( x, i)
try:
f = np.vstack( (f, v_i, reynolds( x, i)))
except:
f = np.vstack( (v_i, reynolds( x, i)))
# control agents
for i in range(param.get('nb')):
v_i = np.dot( util.get_v_i(i + param.get('na')), x)
a_i = np.zeros([param.get('nd'),1])
f = np.vstack( (f, v_i, a_i))
return f
def get_A(x):
A = np.ones([param.get('ni'), param.get('ni')])
# A = np.zeros([param.get('ni'), param.get('ni')])
# for i in range(param.get('ni')):
# p_i = np.dot( get_p_i(i), x)
# for j in range(param.get('ni')):
# p_j = np.dot( get_p_i(j), x)
# dist = np.linalg.norm( p_i - p_j)
# if dist < param.get('R_comm'):
# print('Adj')
# print(np.shape(x))
# print(np.shape(p_i))
# print(np.shape(p_j))
# print(np.shape(dist))
# print(param.get('lambda_a'))
# A[i,j] = np.exp( -param.get('lambda_a')*dist)
# print(A)
return A
def get_xd():
pd = []
vd = []
ad = []
for t in param.get('T'):
pd.append(get_pd_t(t))
vd.append(get_vd_t(t))
ad.append(get_ad_t(t))
param['pd'] = np.asarray(pd)
param['vd'] = np.asarray(vd)
param['ad'] = np.asarray(ad)
def eval(self, x, t):
if param.get('system').get('name') is 'CartPole':
xd = array([0, 0, 0, 0])
error = xd - x
derivative = 0
if self.error_prev is not None:
derivative = (error - self.error_prev) / param.get('dt')
integral = 0
if self.integral is not None:
integral = self.integral + error * param.get('dt')
pid = param.get('kp')*error \
+ param.get('kv')*derivative \
+ param.get('ki')*integral
K = ones((1, 4))
u = dot(K, pid)
self.integral = integral
self.error_prev = error
return u
def plot_cost( C, title = 'Objective Value'):
fig, ax = plt.subplots()
prop_cycle = plt.rcParams['axes.prop_cycle']
colors = prop_cycle.by_key()['color']
for k,u in enumerate(param.get('controllers')):
ax.axhline( C[k], label = u, c = colors[k])
plt.title(title)
plt.xlabel('Iteration')
plt.ylabel('Objective')
plt.legend()
def permute_eta_rows():
perm_mat = np.zeros( (param.get('nd')*param.get('gamma'), \
param.get('nd')*param.get('gamma')))
gamma = param.get('gamma') # relative degree
for dim_idx in range(param.get('nd')):
row_idx = dim_idx * gamma
for gamma_idx in range(gamma):
col_idx = gamma_idx * param.get('nd') + dim_idx
perm_mat[row_idx, col_idx] = 1
row_idx += 1
return perm_mat
def test(model, dataset):
Loss = 0
loss_func = torch.nn.MSELoss()
for batch_idx, (data, target) in enumerate(dataset):
data = torch.from_numpy(data).float()
target = torch.from_numpy(target).float()
# prediction = model.calc_action(data)
prediction = model(data)
loss = loss_func(prediction,
target) # must be (1. nn output, 2. target)
if batch_idx % param.get('gains_log_interval') == 0:
print(' loss: ', loss)
def fn5():
util.get_x0()
util.get_xd()
x0 = param.get('x0')
t0 = param.get('T')[0]
# param['autograd_on'] = True
# print( dynamics.get_detadx(x0,t0))
# param['autograd_on'] = False
# print( dynamics.get_detadx(x0,t0))
# param['autograd_on'] = True
# print( dynamics.get_dgdx(x0))
# param['autograd_on'] = False
# print( dynamics.get_dgdx(x0))
print(dynamics.get_dfdx(x0, t0))
print('\n')
print(dynamics_v2.get_dfdx(x0, t0))
print(np.shape(dynamics.get_dfdx(x0, t0)))
print(np.shape(dynamics_v2.get_dfdx(x0, t0)))
