def plot_SS(X,T, title = 'State Space'):
fig, ax = plt.subplots()
plt.axis('equal')
# plot agents
for i in range(param.get('ni')):
P_i = np.dot( \
util.get_p_i(i), np.transpose(X)).transpose()
if i < param.get('na'):
color = param.get('FreeAgentColor')
else:
color = param.get('ControlAgentColor')
# plot trajectory
ax.plot( P_i[:,0], P_i[:,1], color = color)
ax.scatter( P_i[0,0], P_i[0,1], color = color,
marker = param.get('start_marker'))
ax.scatter( P_i[-1,0], P_i[-1,1], color = color,
marker = param.get('stop_marker'))
# plot desired
ax.plot( param.get('pd')[:,0], param.get('pd')[:,1],
color = param.get('DesiredTrajectoryColor'))
# plot centroid
P_bar = util.get_P_bar( X,T)
ax.plot( P_bar[:,0], P_bar[:,1], color = param.get('CentroidColor'))
ax.plot( P_bar[0,0], P_bar[0,1], color = param.get('CentroidColor'),
marker = param.get('start_marker'))
ax.plot( P_bar[-1,0], P_bar[-1,1], color = param.get('CentroidColor'),
marker = param.get('stop_marker'))
plt.title(title)
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 plot_SS(X,T, title = None, fig = None, ax = None):
if fig is None:
fig, ax = plt.subplots()
plt.axis('equal')
if title is not None:
plt.title(title)
# plot agents
for i in range(param.get('na')):
P_i = np.dot( \
util.get_p_i(i), X).transpose()
# plot trajectory
ax.plot( P_i[:,0], P_i[:,1], color = param.get('AgentColor'))
ax.scatter( P_i[0,0], P_i[0,1], color = param.get('AgentColor'),
marker = param.get('start_marker'))
ax.scatter( P_i[-1,0], P_i[-1,1], color = param.get('AgentColor'),
marker = param.get('stop_marker'))
def plot_ss(x,t, title = None, fig = None, ax = None):
if fig is None:
fig, ax = plt.subplots()
plt.axis('equal')
if title is not None:
plt.title(title)
# plot agents
for i in range(param.get('na')):
p_i = np.dot( \
util.get_p_i(i), x)
# plot trajectory
ax.scatter( p_i[0], p_i[1], color = param.get('AgentColor'),
marker = param.get('stop_marker'))
p_i = dynamics.get_xl(x,t)[0:param.get('nd')]
ax.scatter( p_i[0], p_i[1], color = param.get('LeaderColor'),
marker = param.get('stop_marker'))
p_i = dynamics.get_xb(x,t)[0:param.get('nd')]
ax.scatter( p_i[0], p_i[1], color = param.get('BasisColor'),
marker = param.get('stop_marker'))
def get_cij(x, t):
# euclidean distance heuristic
pl = dynamics.get_xl(x, t)[0:param.get('nd')]
pb = dynamics.get_xb(x, t)[0:param.get('nd')]
T = dynamics.get_T(x, t)
c = np.zeros((param.get('ni'), param.get('ni')))
for i in range(param.get('ni')):
# agent positions
pi = np.dot(util.get_p_i(i), x)
for j in range(param.get('ni')):
# extract configuration
my_1 = np.zeros((param.get('ni'), 1))
my_1[j] = 1
my_1 = np.kron(my_1, np.eye(param.get('nd')))
Tj = np.dot(np.dot(my_1.T, T), my_1)
# desired positions
pj = pl + \
np.dot(Tj,pb)
c[j, i] = np.linalg.norm(pi - pj)
return c
def plot_scp_iteration_state( Xnl, tX, tX_prev, T, title = None):
fig, ax = plt.subplots()
plt.axis('equal')
color_na_nl = 'b'
color_nb_nl = 'g'
color_na_txt = 'r'
color_nb_txt = 'c'
color_na_txtm1 = 'm'
color_nb_txtm1 = 'y'
for i in range(param.get('ni')):
P_i = np.squeeze(np.dot( \
util.get_p_i(i), Xnl)).transpose()
if i < param.get('na'):
color = color_na_nl
else:
color = color_nb_nl
# plot trajectory
ax.plot( P_i[:,0], P_i[:,1], color = color)
ax.scatter( P_i[0,0], P_i[0,1], color = color,
marker = param.get('start_marker'))
ax.scatter( P_i[-1,0], P_i[-1,1], color = color,
marker = param.get('stop_marker'))
ax.plot( np.nan, np.nan, color = color_na_nl, label = 'Nonlinear X Free')
ax.plot( np.nan, np.nan, color = color_nb_nl, label = 'Nonlinear X Control')
for i in range(param.get('ni')):
P_i = np.squeeze(np.dot( \
util.get_p_i(i), tX)).transpose()
if i < param.get('na'):
color = color_na_txt
else:
color = color_nb_txt
# plot trajectory
ax.plot( P_i[:,0], P_i[:,1], color = color)
ax.scatter( P_i[0,0], P_i[0,1], color = color,
marker = param.get('start_marker'))
ax.scatter( P_i[-1,0], P_i[-1,1], color = color,
marker = param.get('stop_marker'))
ax.plot( np.nan, np.nan, color = color_na_txt, label = 'Linear X Free')
ax.plot( np.nan, np.nan, color = color_nb_txt, label = 'Linear X Control')
for i in range(param.get('ni')):
P_i = np.squeeze(np.dot( \
util.get_p_i(i), tX_prev)).transpose()
if i < param.get('na'):
color = color_na_txtm1
else:
color = color_nb_txtm1
# plot trajectory
ax.plot( P_i[:,0], P_i[:,1], color = color)
ax.scatter( P_i[0,0], P_i[0,1], color = color,
marker = param.get('start_marker'))
ax.scatter( P_i[-1,0], P_i[-1,1], color = color,
marker = param.get('stop_marker'))
ax.plot( np.nan, np.nan, color = color_na_txtm1, label = 'Prev X Free')
ax.plot( np.nan, np.nan, color = color_nb_txtm1, label = 'Prev X Control')
plt.legend()
plt.title( title)