def __init__(self, mJ_true=np.array([None])):
print('[astrobee.py::__init__] Initializing Astrobee.')
# true dynamics
if mJ_true.any()!=None:
self.mass = mJ_true[0]
self.J = np.diag(mJ_true[1:])
self.Jinv = np.linalg.inv(self.J)
# get symbolic dynamics
(self.f_dt, self.A_dt, self.B_dt,
self.f_A_dx, self.f_A_du,
self.f_B_dx, self.f_B_du,
self.f_dw,
self.f_w_dx, self.f_w_du) = self.get_equations_dt()
# spherical obstacles [(x,y,z),r]
self.obstacles = [
[[11.3,3.8,4.8], 0.3],
[[10.5,5.5,5.5], 0.4],
]
print('Initializing the ISS.')
keepin_zones, keepout_zones = get_ISS_zones()
self.poly_obstacles = keepout_zones
# additional obstacles
center, width = np.array([10.8,0.,5.]), 0.85*np.ones(3)
self.poly_obstacles.append(PolyObs(center,width))
center, width = np.array([11.2,1.75,4.85]), np.array([0.5,0.6,0.65])
self.poly_obstacles.append(PolyObs(center,width))
print('nb spherical obs:', len(self.obstacles))
print('nb polygonal obs:', len(self.poly_obstacles))
def plot(X_in, U_in, V_in, model):
global figures_N
figures_N = X_in.shape[0]
figures_i = figures_N - 1
global X, U, V, obstacles, poly_obs
X = X_in
U = U_in
V = V_in
obstacles, poly_obs = model.obstacles, model.poly_obstacles
global keepin_zones
keepin_zones, keepout_zones = get_ISS_zones()
fig = plt.figure(figsize=(10, 12))
ax = my_plot(fig, figures_i)
ax = plot_obstacles(ax, obstacles, keepin_zones, poly_obs)
cid = fig.canvas.mpl_connect('key_press_event', key_press_event)
# Plot controls
# plt.figure(2)
# for ui in range(U_in.shape[1]):
# plt.subplot(00ui)
# plt.plot(range(U_in[ui,:]),U_in[ui,:])
U_end = U_in[-1, :, :]
print(U_end.shape)
fig, axs = plt.subplots(U_end.shape[0])
for ui in range(U_end.shape[0]):
axs[ui].plot(list(range(0, U_end.shape[1])), U_end[ui, :])
axs[ui].set_title(str(ui) + "-th control of last SCP iter.")
plt.draw()
plt.figure(1)
X_end = X_in[-1, :, :]
print(X_end.shape)
fig, axs = plt.subplots(X_end.shape[0])
for xi in range(X_end.shape[0]):
axs[xi].plot(list(range(0, X_end.shape[1])), X_end[xi, :])
axs[xi].set_title(str(xi) + "-th state of last SCP iter.")
plt.draw()
plt.figure(1)
lims_btm, lims_up = np.array([5., -3.5, 3.]), np.array([13., 8.5, 6.5])
plt.xlim([lims_btm[0], lims_up[0]])
plt.ylim([lims_btm[1], lims_up[1]])
plt.show()
def plot(X_in, U_in, V_in, m, Xs_true, Us_true):
lims_btm, lims_up = np.array([8.8, -2., 3.5]), np.array([12.2, 8., 6.5])
rcParams['font.family'] = 'serif'
rcParams['font.size'] = 14
rc('text', usetex=True)
X, U, V = X_in[-1, :, :], U_in[-1, :, :], V_in[-1, :, :, :]
V = np.moveaxis(V, -1, 0) # (N, n_x,n_x)
obstacles, poly_obs = m.obstacles, m.poly_obstacles[-2:]
keepin_zones, keepout_zones = get_ISS_zones()
# *********************************************
# TRAJECTORY - X-Y
idx = [0, 1]
fig, ax = plt.subplots(figsize=(6, 10))
plot_mean_traj_with_gaussian_uncertainty_ellipses(
X.T,
V,
additional_radius=m.robot_radius,
color='blue',
probability=m.prob,
idx=idx,
alpha=0.2)
ax = plot_obstacles(ax, obstacles, keepin_zones, poly_obs, idx)
ax.text(10.45, -0.2, r'$\mathcal{X}_{\textrm{obs}}$', fontsize=24)
ax.text(9.2, 1.8, r'$\mathcal{X}_{\textrm{obs}}$', fontsize=24)
# Plot start & goal positions
ax.plot(m.x_init[idx[0]],
m.x_init[idx[1]],
'+',
markersize=16,
markeredgewidth=3,
color='k')
ax.plot(m.x_final[idx[0]],
m.x_final[idx[1]],
'+',
markersize=16,
markeredgewidth=3,
color='k')
init_text_pos = [m.x_init[idx[0]] - 0., m.x_init[idx[1]] - 0.4]
goal_text_pos = [m.x_final[idx[0]] + 0., m.x_final[idx[1]] + 0.1]
ax.text(init_text_pos[0] - 0.,
init_text_pos[1] - 0.3,
r'$\mathbf{x}_{\textrm{init}}$',
fontsize=22)
ax.text(goal_text_pos[0] - 0.4,
goal_text_pos[1] + 0.4,
r'$\mathbf{x}_{\textrm{goal}}$',
fontsize=22)
# ----------------
ax.tick_params("both", labelsize=24)
ax.set_xlabel('X', fontsize=24)
ax.set_ylabel('Y', rotation="horizontal", fontsize=24)
plt.xlim([lims_btm[idx[0]], lims_up[idx[0]]])
plt.ylim([lims_btm[idx[1]], lims_up[idx[1]]])
# *********************************************
# *********************************************
# TRAJECTORY - Z-Y
idx = [2, 1]
fig, ax = plt.subplots(figsize=(4, 10))
plot_mean_traj_with_gaussian_uncertainty_ellipses(
X.T,
V,
additional_radius=m.robot_radius,
color='blue',
probability=m.prob,
idx=idx,
alpha=0.2)
ax = plot_obstacles(ax, obstacles, keepin_zones, poly_obs, idx)
# Plot start & goal positions
ax.plot(m.x_init[idx[0]],
m.x_init[idx[1]],
'+',
markersize=16,
markeredgewidth=3,
color='k')
ax.plot(m.x_final[idx[0]],
m.x_final[idx[1]],
'+',
markersize=16,
markeredgewidth=3,
color='k')
init_text_pos = [m.x_init[idx[0]] - 0., m.x_init[idx[1]] - 0.7]
goal_text_pos = [m.x_final[idx[0]] + 0.12, m.x_final[idx[1]] + 0.1]
ax.text(init_text_pos[0] - 0.2,
init_text_pos[1] - 0.1,
r'$\mathbf{x}_{\textrm{init}}$',
fontsize=22)
ax.text(goal_text_pos[0] - 0.4,
goal_text_pos[1] + 0.35,
r'$\mathbf{x}_{\textrm{goal}}$',
fontsize=22)
# ----------------
ax.tick_params("both", labelsize=24)
ax.set_xlabel('Z', fontsize=24)
plt.xlim([lims_btm[idx[0]], lims_up[idx[0]]])
plt.ylim([lims_btm[idx[1]], lims_up[idx[1]]])
# *********************************************
# *********************************************
# CONTROLS
fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(list(range(0, U.shape[1])), U[0, :], '--o', color='tab:green')
ax.plot(list(range(0, U.shape[1])), U[1, :], '--o', color='tab:orange')
ax.plot(list(range(0, U.shape[1])), U[2, :], '--o', color='tab:blue')
if m.B_feedback:
Sigmas_u = compute_variances_controls(m, V)
plot_mean_var(U[0, :], Sigmas_u[:, 0, 0], prob=0.9, color='tab:green')
plot_mean_var(U[1, :], Sigmas_u[:, 1, 1], prob=0.9, color='tab:orange')
plot_mean_var(U[2, :], Sigmas_u[:, 2, 2], prob=0.9, color='tab:blue')
ax.legend([r'$F_x$', r'$F_y$', r'$F_z$'], fontsize=24)
ax.tick_params("both", labelsize=24)
ax.set_xlabel('k', fontsize=24)
ax.set_ylabel('F', rotation="horizontal", fontsize=24)
fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(list(range(0, U.shape[1])), U[3, :], '--o', color='tab:green')
ax.plot(list(range(0, U.shape[1])), U[4, :], '--o', color='tab:orange')
ax.plot(list(range(0, U.shape[1])), U[5, :], '--o', color='tab:blue')
if m.B_feedback:
Sigmas_u = compute_variances_controls(m, V)
plot_mean_var(U[3, :], Sigmas_u[:, 3, 3], prob=0.9, color='tab:green')
plot_mean_var(U[4, :], Sigmas_u[:, 4, 4], prob=0.9, color='tab:orange')
plot_mean_var(U[5, :], Sigmas_u[:, 5, 5], prob=0.9, color='tab:blue')
ax.legend([r'$M_x$', r'$M_y$', r'$M_z$'], fontsize=24)
ax.tick_params("both", labelsize=24)
ax.set_xlabel('k', fontsize=24)
ax.set_ylabel('M', rotation="horizontal", fontsize=24)
ax.ticklabel_format(axis='y', style='sci', scilimits=(0, 0))
plt.draw()
plt.show()
import sys, os
sys.path.append('../src/utils/')
sys.path.append('../exps/Models/')
import numpy as np
import matplotlib as plt
from polygonal_obstacles import PolygonalObstacle as PolyObs
from viz import *
from ISS import get_ISS_zones
lims_btm, lims_up = np.array([5.,-3.5, 3.]), np.array([13., 8.5, 6.5])
keepin_zones, keepout_zones = get_ISS_zones()
# --------------------------------------------
plt.figure(1)
ax = plt.gca()
# --------------------------------------------
for obs in keepin_zones:
center, widths = obs.c, 2*np.array([obs.dx,obs.dy,obs.dz])
plot_rectangle(ax, center[:2], widths[:2], color='g')
for obs in keepout_zones:
center, widths = obs.c, 2*np.array([obs.dx,obs.dy,obs.dz])
plot_rectangle(ax, center[:2], widths[:2], color='r')
plt.xlim([lims_btm[0], lims_up[0]])
plt.ylim([lims_btm[1], lims_up[1]])
plt.draw()
plt.show()