def shape(fig, alpha, color, edge_c, edge_w, grid, sides, edges, multi_pi,
figcolor, rotation, rotmagt, rotmagp, save):
plt.clf()
# Definition of x
def x_(u, v):
x = (cos(u) *
(Fraction(1, 3) * sqrt(2) * cos(u) * cos(2 * v) + Fraction(2, 3) *
sin(u) * cos(v))) / (1 - sqrt(2) * sin(u) * cos(u) * sin(3 * v))
return x
# Definition of y
def y_(u, v):
y = (cos(u) *
(Fraction(1, 3) * sqrt(2) * cos(u) * sin(2 * v) - Fraction(2, 3) *
sin(u) * sin(v))) / (1 - sqrt(2) * sin(u) * cos(u) * sin(3 * v))
return y
# Definition of z
def z_(u, v):
z = cos(u)**2 / (1 - sqrt(2) * sin(u) * cos(u) * sin(3 * v)) - 1
return z
# Value of the angles
u = linspace(0, multi_pi * pi, sides + 1)
v = linspace(0, multi_pi * pi, edges)
u, v = meshgrid(u, v)
# Symbolic representation
x = x_(u, v)
y = y_(u, v)
z = z_(u, v)
# Figure Properties
ax = p3.Axes3D(fig)
ax.set_facecolor(figcolor) # Figure background turns black
# Axis Properties
plt.axis(grid) # Turns off the axis grid
plt.axis('equal')
# Axis Limits
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
# Surface Plot
boys_surf = ax.plot_surface(x, y, z)
boys_surf.set_alpha(alpha) # Transparency of figure
boys_surf.set_edgecolor(edge_c) # Edge color of the lines on the figure
boys_surf.set_linewidth(edge_w) # Line width of the edges
boys_surf.set_facecolor(color) # General color of the figure
def rot_on():
def animate(i):
ax.view_init(azim=rotmagt * i, elev=rotmagp * i)
if save == "MP4":
# Animate
ani = FuncAnimation(fig,
animate,
frames=500,
interval=100,
save_count=50) # frames=100)#, repeat=True)
Writer = writers['ffmpeg']
writer = Writer(fps=30, bitrate=1800)
ani.save('{}.mp4'.format(name), writer=writer)
else:
#save = None
# Animate
ani = FuncAnimation(fig, animate, interval=1,
save_count=50) # frames=100)#, repeat=True)
pass
plt.ion()
plt.show()
time.sleep(0)
plt.close()
if rotation == "On":
rot_on()
elif rotation == "Off":
pass
#Based on http://matplotlib.org/examples/animation/simple_3danim.html
import numpy
import matplotlib.pyplot as mplot
import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.animation as animation
import xml.etree.ElementTree as ET
fig = mplot.figure()
axes = p3.Axes3D(fig)
plotType = 'line'
#plotType = 'scatter'
def readData():
#read data
xml_data = ET.parse('particles.xml')
root = xml_data.getroot()
particle_interval = int(root.get('particle_interval'))
n_iterations = int(int(root.get('n_iterations')) / (particle_interval)) + 1
n_particles = int(root.get('n_particles'))
its = root.findall('iteration')
if plotType == 'line':
data = [numpy.empty((3, n_iterations)) for index in range(n_particles)]
elif plotType == 'scatter':
data = [numpy.empty((3, n_particles)) for index in range(n_iterations)]
if plotType == 'line':
for p in range(n_particles):
step = ((np.random.rand(dims) - 0.5) * 0.1)
lineData[:, index] = lineData[:, index-1] + step
return lineData
def update_lines(num, dataLines, lines):
for line, data in zip(lines, dataLines):
# NOTE: there is no .set_data() for 3 dim data...
line.set_data(data[0:2, :num])
line.set_3d_properties(data[2, :num])
return lines
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
# Fifty lines of random 3-D lines
data = [Gen_RandLine(25, 3) for index in range(50)]
# Creating fifty line objects.
# NOTE: Can't pass empty arrays into 3d version of plot()
lines = [ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1])[0] for dat in data]
# Setting the axes properties
ax.set_xlim3d([0.0, 1.0])
ax.set_xlabel('X')
ax.set_ylim3d([0.0, 1.0])
ax.set_ylabel('Y')
def rotation_matrix(axis, theta):
axis = axis / np.sqrt(np.dot(axis, axis))
a = np.cos(theta / 2)
b, c, d = axis * np.sin(theta / 2)
return np.array([[
a * a + b * b - c * c - d * d, 2 * (b * c - a * d), 2 * (b * d + a * c)
], [
2 * (b * c + a * d), a * a + c * c - b * b - d * d, 2 * (c * d - a * b)
], [
2 * (b * d - a * c), 2 * (c * d + a * b), a * a + d * d - b * b - c * c
]])
########
fig3 = pyplot.figure(3, figsize=(3, 3), dpi=100)
ax3 = p3.Axes3D(fig3)
ax3.view_init(elev=25, azim=25)
ax3.set_color_cycle('b')
########
########
line3x, = ax3.plot([0, 9], [0, 0], [0, 0])
line3x.set_linewidth(2)
ax3.text(9,
0,
0,
r'$x_{lab}$',
fontsize=14,
verticalalignment='top',
horizontalalignment='center')
line3x.set_color('k')
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d as plt3d
fig = plt.figure(num=15)
fig3d = plt3d.Axes3D(fig)
X = np.arange(-5, 5, 0.25)
Y = np.arange(-5, 5, 0.25)
X, Y = np.meshgrid(X, Y)
R = np.sqrt(X ** 2 + Y ** 2)
Z = np.sin(R)
fig3d.plot_surface(X, Y, Z, \
rstride=1, cstride=1, \
cmap=plt.get_cmap('rainbow'))
fig3d.contourf(X, Y, Z, \
offset=-2, zdir='z', cmap=plt.get_cmap('rainbow'))
plt.show()
def animate(lr: float = 0.02,
n_iter: int = 180,
bounds: list = None,
init_point: list = None,
with_tf: bool = False,
output_path: str = None):
fig = plt.figure(dpi=120, figsize=(5, 8))
ax = axes3d.Axes3D(fig)
ax.w_xaxis.gridlines.set_alpha(0.5)
ax.w_yaxis.gridlines.set_alpha(0.5)
ax.w_zaxis.gridlines.set_alpha(0.5)
ax.w_xaxis.pane.fill = False
ax.w_yaxis.pane.fill = False
ax.w_zaxis.pane.fill = False
ax = plot_saddle_plane(ax)
if not init_point:
init_point = [1.2, -0.001]
if with_tf:
tracks, labels = run_tracks_with_tf(lr=lr,
n_iter=n_iter,
init_point=init_point)
else:
tracks, labels = run_tracks(lr=lr,
n_iter=n_iter,
init_point=init_point)
_, n_tracks, iters = tracks.shape
c = ['red', 'orange', 'yellow', 'green']
lines = [
ax.plot(tracks[0, i, 0:1],
tracks[1, i, 0:1],
tracks[2, i, 0:1],
linewidth=2,
c=c[i],
label=labels[i],
markevery=[-1])[0] for i in range(n_tracks)
]
leads = [
ax.plot(tracks[0, i, 0:1],
tracks[1, i, 0:1],
tracks[2, i, 0:1],
marker='o',
c=c[i])[0] for i in range(n_tracks)
]
def update(num, data, lines, leads):
rot_speed = 1.5
azim = (num * rot_speed) % 360 - 180
ax.view_init(elev=45, azim=azim)
frame_speed = 1
for i, line in enumerate(lines):
lines[i].set_data(data[0:2, i, :num * frame_speed])
lines[i].set_3d_properties(data[2, i, :num * frame_speed])
for i, sct in enumerate(leads):
leads[i].set_data(data[0:2, i, num * frame_speed - 1])
leads[i].set_3d_properties(data[2, i, num * frame_speed - 1])
return lines, leads
if not bounds:
bounds = [[-1.2, 1.2], [-1.2, 1.2], [-1.2, 1.2]]
# Setting the axes properties
ax.set_xlim3d(bounds[0])
ax.set_xlabel('X')
ax.set_ylim3d(bounds[1])
ax.set_ylabel('Y')
ax.set_zlim3d(bounds[2])
ax.set_zlabel('Z')
ani = animation.FuncAnimation(fig,
update,
frames=iters,
fargs=(tracks, lines, leads),
interval=50,
blit=False)
plt.legend()
if output_path:
ani.save(output_path, writer='imagemagick')
else:
plt.show()
plt.close()
def get_3d_axes():
fig = plt.figure(figsize=(5, 5))
ax = p3.Axes3D(fig)
return fig, ax
def draw_scene(panel_ors, sat):
import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.animation as animation
import math
def make_triangles(alpha=90 / 180 * math.pi, transformation=0):
#alpha = 90/180 * math.pi
#print("Painting triangle with "+str(alpha)+"rads")
tria = np.zeros((3, 3))
h = 3**(1 / 2) / 2 * l
z_height = -math.sin(
alpha) * h # Height of the piece's ends above XY plane.
h_xy = math.cos(alpha) * h
tria[0] = [0, 3**(1 / 2) / 3 * l, 0]
tria[1] = [(-math.cos(1 / 6 * math.pi) * h_xy - 1 / 4 * l),
math.sin(1 / 6 * math.pi) * h_xy + 3**(1 / 2) / 12 * l,
z_height]
tria[2] = [-l / 2, -3**(1 / 2) / 3 * 0.5 * l, 0]
while transformation != 0:
theta = 120 / 180 * math.pi
c, s = np.cos(theta), np.sin(theta)
R = np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))
tria[0] = np.dot(R, tria[0])
tria[1] = np.dot(R, tria[1])
tria[2] = np.dot(R, tria[2])
transformation -= 1
piece = p3.art3d.Poly3DCollection([tria])
piece.set_color("w")
piece.set_edgecolor("r")
return piece
def update_lines(num, panel_ors):
while len(ax.collections) > 1:
ax.collections.pop(1)
for i in panel_ors:
piece = make_triangles(panel_ors[i].y_axis_theta[num], int(i) - 1)
if panel_ors[i].state == 0:
piece.set_color("cornflowerblue")
elif panel_ors[i].state == 1:
piece.set_color("blue")
ax.add_collection3d(piece)
timeElapsed = "{:.2f}".format(
panel_ors["1"].x_axis[num]).zfill(timeStampLen)
timeLabel.set_text("t = " + timeElapsed + " seconds")
frameRef = animation_id + "_" + str(str(num).zfill(4))
fname = "fourth\\frames\\" + frameRef + ".png"
'''
if not os.path.isfile(fname):
print("Saving file "+fname)
fig.savefig(fname)
'''
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
# hub triangle
hubcoor = np.zeros((3, 3))
l = panel_ors["1"].dimensions[0]
hubcoor[0] = [0, 3**(1 / 2) / 3 * l, 0]
hubcoor[1] = [-l / 2, -3**(1 / 2) / 3 * 0.5 * l, 0]
hubcoor[2] = [l / 2, -3**(1 / 2) / 3 * 0.5 * l, 0]
hub = p3.art3d.Poly3DCollection([hubcoor])
if panel_ors["1"].hubState == 1:
hub.set_color("blue")
else:
hub.set_color("cornflowerblue")
hub.set_edgecolor("k")
ax.add_collection3d(hub)
for i in panel_ors:
piece = make_triangles(panel_ors[i].y_axis_theta[-1], int(i) - 1)
#ax.add_collection3d(piece)
if panel_ors[i].state == 0:
piece.set_color("cornflowerblue")
elif panel_ors[i].state == 1:
piece.set_color("blue")
ax.add_collection3d(piece)
# Setting the axes properties
ax.set_xlim3d([-l, l])
ax.set_xlabel('X')
ax.set_ylim3d([-l, l])
ax.set_ylabel('Y')
ax.set_zlim3d([-l, l])
ax.set_zlabel('Z')
ax.set_title('3D Test')
timeLabel = ax.text2D(0.05,
0.95,
"t = {:.2f}".format(0.00),
transform=ax.transAxes)
timeStampLen = len(
str(int(panel_ors["1"].x_axis[-1] // 1 + 1))
) + 3 # number of digits of last timestamp rounded up to the highest unit, plus one period plus 2 decimal digits.
#fig.savefig("fourth\\frames\\test frame.png")
# Creating the Animation object
line_ani = animation.FuncAnimation(fig,
update_lines,
panel_ors["1"].iterations,
fargs=(panel_ors, ),
interval=50,
blit=False)
plt.show()
import numpy as np
#import matplotlib as mpl
#mpl.use('pgf')
from matplotlib import pyplot
import mpl_toolkits.mplot3d.axes3d as p3
from mpl_toolkits.mplot3d import proj3d
import sys
sys.path.append(
r'C:\Users\user\Desktop\xelatex_Transend\drawing_code\python\3D_geometry_annotate_program'
)
import annotate_program as tool
#### The plotting of a vector-based graphics using the above points location information.
fig2 = pyplot.figure(2, figsize=(4, 3), dpi=100)
ax2 = p3.Axes3D(fig2)
ax2.view_init(elev=40, azim=-40)
ax2.set_color_cycle('b')
po = np.array([0, 0, 0]) #origin
px = np.array([1, 0, 0])
py = np.array([0, 1, 0])
pz = np.array([0, 0, 1])
pA = np.array([0.3, 0.3, 1])
#xyz axes
farrowx = tool.Arrow3D(*zip(po, px),
mutation_scale=16,
lw=2,
arrowstyle="-|>",
color="b")
def draw_comic(frames,
angles=None,
figsize=None,
window_size=0.45,
dot_size=20,
lw=2.5,
zcolor=None,
cmap='cool_r'):
fig = plt.figure(figsize=figsize)
ax = p3.Axes3D(fig)
ax.view_init(30, 0)
shift_size = window_size
ax.set_xlim(-window_size, window_size)
ax.set_ylim(-window_size, len(frames) * window_size)
ax.set_zlim(-0.1, 0.6)
ax.set_xticklabels([])
ax.set_yticklabels([])
ax.set_zticklabels([])
ax.get_xaxis().set_ticks([])
ax.get_yaxis().set_ticks([])
cm = matplotlib.cm.get_cmap(cmap)
if angles is not None:
vR = 0.15
zidx = point_labels.index("CLAV")
X = frames[:, zidx, 0]
Y = frames[:, zidx, 1]
dX, dY = vR * np.cos(angles), vR * np.sin(angles)
Z = frames[:, zidx, 2]
#Z = frames[:,2,2]
for iframe, frame in enumerate(frames):
ax.scatter(frame[:, 0],
frame[:, 1] + iframe * shift_size,
frame[:, 2],
alpha=0.3,
c=zcolor,
cmap=cm,
s=dot_size,
depthshade=True)
if angles is not None:
ax.quiver(X[iframe],
iframe * shift_size + Y[iframe],
Z[iframe],
dX[iframe],
dY[iframe],
0,
color='black')
for i, (g1, g2) in enumerate(skeleton_lines):
g1_idx = [point_labels.index(l) for l in g1]
g2_idx = [point_labels.index(l) for l in g2]
if zcolor is not None:
color = cm(0.5 *
(zcolor[g1_idx].mean() + zcolor[g2_idx].mean()))
else:
color = None
x1 = np.mean(frame[g1_idx], axis=0)
x2 = np.mean(frame[g2_idx], axis=0)
ax.plot(np.linspace(x1[0], x2[0], 10),
np.linspace(x1[1], x2[1], 10) + iframe * shift_size,
np.linspace(x1[2], x2[2], 10),
color=color,
lw=lw)
def animate(list_of_frames, animation_title):
J = np.matrix(
[[20, 1, 2, 1, 8, 10, 2, 9, 11, 3, 4, 7, 7, 5, 6, 14, 15, 16, 17],
[3, 3, 3, 8, 10, 12, 9, 11, 13, 4, 7, 5, 6, 14, 15, 16, 17, 18, 19]])
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
# Setting the axes properties
ax.set_xlim3d([-100.0, 400.0])
ax.set_xlabel('X')
ax.set_ylim3d([-100.0, 100.0])
ax.set_ylabel('Y')
ax.set_zlim3d([-100.0, 100.0])
ax.set_zlabel('Z')
title = ax.set_title(animation_title)
#lines = [ax.plot(dat[0, : ], dat[1, : ], dat[2, : ])[0] for dat in data]
sequence_number = 0
S = list_of_frames[sequence_number]
X_VECTOR = S[:, 0]
Z_VECTOR = np.subtract(np.full((len(S)), 100), S[:, 1])
Y_VECTOR = S[:, 2] / 4
joints, = ax.plot(X_VECTOR, Y_VECTOR, Z_VECTOR, linestyle="", marker=".")
lines = []
for i in range(FRAME_DATA_POINT_COUNT - 1):
c1 = J[0, i] - 1
c2 = J[1, i] - 1
line, = ax.plot([X_VECTOR[c1], X_VECTOR[c2]],
[Y_VECTOR[c1], Y_VECTOR[c2]],
[Z_VECTOR[c1], Z_VECTOR[c2]])
lines.append(line)
def update(sequence_number):
S = list_of_frames[sequence_number]
X_VECTOR = S[:, 0]
Z_VECTOR = np.subtract(np.full((len(S)), 100), S[:, 1])
Y_VECTOR = S[:, 2] / 4
joints.set_data(X_VECTOR, Y_VECTOR)
joints.set_3d_properties(Z_VECTOR)
for i in range(FRAME_DATA_POINT_COUNT - 1):
c1 = J[0, i] - 1
c2 = J[1, i] - 1
lines[i].set_data([X_VECTOR[c1], X_VECTOR[c2]],
[Y_VECTOR[c1], Y_VECTOR[c2]])
lines[i].set_3d_properties([Z_VECTOR[c1], Z_VECTOR[c2]])
title.set_text(animation_title +
' sequence={}'.format(sequence_number))
return tuple(lines) + (title, joints)
#return title, joints
# Creating the Animation object
#line_ani = animation.FuncAnimation(fig, update, len(list_of_frames), fargs=(frames, lines),interval=400, blit=True)
line_ani = animation.FuncAnimation(fig,
update,
len(list_of_frames),
interval=400,
blit=True)
line_ani.save('action_recognition.mp4',
fps=30,
extra_args=['-vcodec', 'libx264'])
plt.show()
def animate_stick(seq,
ghost=None,
ghost_shift=0,
figsize=None,
zcolor=None,
pointer=None,
ax_lims=(-0.4, 0.4),
speed=45,
dot_size=20,
dot_alpha=0.5,
lw=2.5,
cmap='cool_r',
pointer_color='black',
cloud=False):
if zcolor is None:
zcolor = np.zeros(seq.shape[1])
fig = plt.figure(figsize=figsize)
ax = p3.Axes3D(fig)
# The following lines eliminate background lines/axes:
ax.axis('off')
ax.xaxis.set_visible(False)
ax.yaxis.set_visible(False)
ax.set_frame_on(False)
# set figure background opacity (alpha) to 0:
fig.patch.set_alpha(0.)
if ghost_shift and ghost is not None:
seq = seq.copy()
ghost = ghost.copy()
seq[:, :, 0] -= ghost_shift
ghost[:, :, 0] += ghost_shift
cm = matplotlib.cm.get_cmap(cmap)
pts = ax.scatter(seq[0, :, 0],
seq[0, :, 1],
seq[0, :, 2],
c=zcolor,
s=dot_size,
cmap=cm,
alpha=dot_alpha)
ghost_color = 'blue'
if ghost is not None:
pts_g = ax.scatter(ghost[0, :, 0],
ghost[0, :, 1],
ghost[0, :, 2],
c=ghost_color,
s=dot_size,
alpha=dot_alpha)
if ax_lims:
ax.set_xlim(*ax_lims)
ax.set_ylim(*ax_lims)
ax.set_zlim(0, ax_lims[1] - ax_lims[0])
plt.close(fig)
xline, colors = get_line_segments(seq, zcolor, cm)
lines = put_lines(ax, xline[0], colors, lw=lw, alpha=0.9, cloud=cloud)
if ghost is not None:
xline_g = get_line_segments(ghost)
lines_g = put_lines(ax,
xline_g[0],
ghost_color,
lw=lw,
alpha=1.0,
cloud=cloud)
if pointer is not None:
vR = 0.15
dX, dY = vR * np.cos(pointer), vR * np.sin(pointer)
zidx = point_labels.index('CLAV')
X = seq[:, zidx, 0]
Y = seq[:, zidx, 1]
Z = seq[:, zidx, 2]
quiv = ax.quiver(X[0],
Y[0],
Z[0],
dX[0],
dY[0],
0,
color=pointer_color)
ax.quiv = quiv
def update(t):
pts._offsets3d = juggle_axes(seq[t, :, 0], seq[t, :, 1], seq[t, :, 2],
'z')
for i, l in enumerate(lines):
l.set_data(xline[t, i, :2])
l.set_3d_properties(xline[t, i, 2])
if ghost is not None:
pts_g._offsets3d = juggle_axes(ghost[t, :, 0], ghost[t, :, 1],
ghost[t, :, 2], 'z')
for i, l in enumerate(lines_g):
l.set_data(xline_g[t, i, :2])
l.set_3d_properties(xline_g[t, i, 2])
if pointer is not None:
ax.quiv.remove()
ax.quiv = ax.quiver(X[t],
Y[t],
Z[t],
dX[t],
dY[t],
0,
color=pointer_color)
return animation.FuncAnimation(
fig,
update,
len(seq),
interval=speed,
blit=False,
)
def animated_plot(x, *args, **kwargs):
"""
Implements animated trajectory plot
INPUTS:
-numpy array(s)
-list of numpy arrays
OUTPUTS:
-returns fig, ax, data, line_ani. fig, ax and line_ani are matplotlib figure, axis, and animation handles,
respectively. Data is a numpy array of (reduced) data. (Optional, to use set return_data=True)
"""
assert x[0].shape[
1] > 2, "Hypertools currently only supports animation for data with > 2 dims."
## HYPERTOOLS-SPECIFIC ARG PARSING ##
if 'zoom' in kwargs:
zoom = kwargs['zoom']
del kwargs['zoom']
else:
zoom = 0
if 'chemtrails' in kwargs:
chemtrails = kwargs['chemtrails']
del kwargs['chemtrails']
else:
chemtrails = False
if 'rotations' in kwargs:
rotations = kwargs['rotations']
del kwargs['rotations']
else:
rotations = 2
if 'duration' in kwargs:
duration = kwargs['duration']
del kwargs['duration']
else:
duration = 30
if 'frame_rate' in kwargs:
frame_rate = kwargs['frame_rate']
del kwargs['frame_rate']
else:
frame_rate = 50
if 'tail_duration' in kwargs:
tail_duration = kwargs['tail_duration']
del kwargs['tail_duration']
else:
tail_duration = 2
if 'return_data' in kwargs:
return_data = kwargs['return_data']
del kwargs['return_data']
else:
return_data = False
if 'legend' in kwargs:
legend = True
legend_data = kwargs['legend']
del kwargs['legend']
else:
legend = False
if 'color_palette' in kwargs:
palette = kwargs['color_palette']
del kwargs['color_palette']
if 'save_path' in kwargs:
save = True
save_path = kwargs['save_path']
del kwargs['save_path']
else:
save = False
# handle show flag
if 'show' in kwargs:
show = kwargs['show']
del kwargs['show']
matplotlib.use('Agg')
else:
show = True
import matplotlib.pyplot as plt
##SUB FUNCTIONS##
def plot_cube(scale, const=1):
cube = {
"top": ([[-1, 1], [-1, 1]], [[-1, -1], [1, 1]], [[1, 1], [1, 1]]),
"bottom": ([[-1, 1], [-1, 1]], [[-1, -1], [1, 1]], [[-1, -1],
[-1, -1]]),
"left": ([[-1, -1], [-1, -1]], [[-1, 1], [-1, 1]], [[-1, -1],
[1, 1]]),
"right": ([[1, 1], [1, 1]], [[-1, 1], [-1, 1]], [[-1, -1], [1,
1]]),
"front": ([[-1, 1], [-1, 1]], [[-1, -1], [-1, -1]], [[-1, -1],
[1, 1]]),
"back": ([[-1, 1], [-1, 1]], [[1, 1], [1, 1]], [[-1, -1], [1, 1]])
}
plane_list = []
for side in cube:
(Xs, Ys, Zs) = (np.asarray(cube[side][0]) * scale * const,
np.asarray(cube[side][1]) * scale * const,
np.asarray(cube[side][2]) * scale * const)
plane_list.append(
ax.plot_wireframe(Xs,
Ys,
Zs,
rstride=1,
cstride=1,
color='black',
linewidth=2))
return plane_list
def update_lines(num, data_lines, lines, trail_lines, cube_scale,
tail_duration):
if hasattr(update_lines, 'planes'):
for plane in update_lines.planes:
plane.remove()
update_lines.planes = plot_cube(cube_scale)
ax.view_init(elev=10,
azim=rotations * (360 * (num / data_lines[0].shape[0])))
ax.dist = 8 - zoom
for line, data, trail in zip(lines, data_lines, trail_lines):
if num <= tail_duration:
line.set_data(data[0:num + 1, 0:2].T)
line.set_3d_properties(data[0:num + 1, 2])
else:
line.set_data(data[num - tail_duration:num + 1, 0:2].T)
line.set_3d_properties(data[num - tail_duration:num + 1, 2])
if chemtrails:
trail.set_data(data[0:num + 1, 0:2].T)
trail.set_3d_properties(data[0:num + 1, 2])
return lines, trail_lines
args_list = parse_args(x, args)
kwargs_list = parse_kwargs(x, kwargs)
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
if type(x) is not list:
x = [x]
interp_val = frame_rate * duration / (x[0].shape[0] - 1)
x = interp_array_list(x, interp_val=interp_val)
x = center(x)
x = scale(x)
if tail_duration == 0:
tail_duration = 1
else:
tail_duration = int(frame_rate * tail_duration)
lines = [
ax.plot(dat[0, 0:1],
dat[1, 0:1],
dat[2, 0:1],
linewidth=3,
*args_list[idx],
**kwargs_list[idx])[0] for idx, dat in enumerate(x)
]
trail = [
ax.plot(dat[0, 0:1],
dat[1, 0:1],
dat[2, 0:1],
alpha=.3,
linewidth=3,
*args_list[idx],
**kwargs_list[idx])[0] for idx, dat in enumerate(x)
]
ax.set_axis_off()
# Get cube scale from data
cube_scale = 1
# Setting the axes properties
ax.set_xlim3d([-cube_scale, cube_scale])
ax.set_ylim3d([-cube_scale, cube_scale])
ax.set_zlim3d([-cube_scale, cube_scale])
#add legend
if legend:
proxies = [
plt.Rectangle((0, 0), 1, 1, fc=palette[idx])
for idx, label in enumerate(legend_data)
]
ax.legend(proxies, legend_data)
# Creating the Animation object
line_ani = animation.FuncAnimation(fig,
update_lines,
x[0].shape[0],
fargs=(x, lines, trail, cube_scale,
tail_duration),
interval=1000 / frame_rate,
blit=False,
repeat=False)
if save:
Writer = animation.writers['ffmpeg']
writer = Writer(fps=frame_rate, bitrate=1800)
line_ani.save(save_path, writer=writer)
if show:
plt.show()
if return_data:
return fig, ax, x, line_ani
def shape(fig, alpha, color, edge_c, edge_w, grid, color2, figcolor, rotation,
rotmagt, rotmagp, save):
plt.clf()
C0 = (1 + sqrt(5)) / 4
C1 = (3 + sqrt(5)) / 4
C2 = (1 + sqrt(5)) / 2
points = array([
[0.0, 0.0, C2],
[0.0, 0.0, -C2],
[C2, 0.0, 0.0],
[-C2, 0.0, 0.0],
[0.0, C2, 0.0],
[0.0, -C2, 0.0],
[0.5, C0, C1],
[0.5, C0, -C1],
[0.5, -C0, C1],
[0.5, -C0, -C1],
[-0.5, C0, C1],
[-0.5, C0, -C1],
[-0.5, -C0, C1],
[-0.5, -C0, -C1],
[C1, 0.5, C0],
[C1, 0.5, -C0],
[C1, -0.5, C0],
[C1, -0.5, -C0],
[-C1, 0.5, C0],
[-C1, 0.5, -C0],
[-C1, -0.5, C0],
[-C1, -0.5, -C0],
[C0, C1, 0.5],
[C0, C1, -0.5],
[C0, -C1, 0.5],
[C0, -C1, -0.5],
[-C0, C1, 0.5],
[-C0, C1, -0.5],
[-C0, -C1, 0.5],
[-C0, -C1, -0.5],
])
P = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
Z = zeros((30, 3))
for i in range(30):
Z[i, :] = dot(points[i, :], P)
ax = p3.Axes3D(fig)
ax.set_facecolor(figcolor)
plt.axis(grid)
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
verts = [
[Z[0], Z[8], Z[16], Z[14], Z[6]],
[Z[0], Z[10], Z[18], Z[20], Z[12]],
[Z[1], Z[7], Z[15], Z[17], Z[9]],
[Z[1], Z[13], Z[21], Z[19], Z[11]],
[Z[2], Z[15], Z[23], Z[22], Z[14]],
[Z[2], Z[16], Z[24], Z[25], Z[17]],
[Z[3], Z[18], Z[26], Z[27], Z[19]],
[Z[3], Z[21], Z[29], Z[28], Z[20]],
[Z[4], Z[23], Z[7], Z[11], Z[27]],
[Z[4], Z[26], Z[10], Z[6], Z[22]],
[Z[5], Z[24], Z[8], Z[12], Z[28]],
[Z[5], Z[29], Z[13], Z[9], Z[25]],
[Z[0], Z[6], Z[10]],
[Z[0], Z[12], Z[8]],
[Z[1], Z[9], Z[13]],
[Z[1], Z[11], Z[7]],
[Z[2], Z[14], Z[16]],
[Z[2], Z[17], Z[15]],
[Z[3], Z[19], Z[21]],
[Z[3], Z[20], Z[18]],
[Z[4], Z[22], Z[23]],
[Z[4], Z[27], Z[26]],
[Z[5], Z[25], Z[24]],
[Z[5], Z[28], Z[29]],
[Z[6], Z[14], Z[22]],
[Z[7], Z[23], Z[15]],
[Z[8], Z[24], Z[16]],
[Z[9], Z[17], Z[25]],
[Z[10], Z[26], Z[18]],
[Z[11], Z[19], Z[27]],
[Z[12], Z[20], Z[28]],
[Z[13], Z[29], Z[21]],
]
fc = [color if i in range(0, 12) else color2 for i in range(len(verts))]
sdode = Poly3DCollection(verts)
sdode.set_edgecolor(edge_c)
sdode.set_linewidth(edge_w)
sdode.set_alpha(alpha)
sdode.set_facecolor(fc)
ax.add_collection3d(sdode)
def rot_on():
def animate(i):
ax.view_init(azim=rotmagt * i, elev=rotmagp * i)
if save == "MP4":
# Animate
ani = FuncAnimation(fig,
animate,
frames=500,
interval=100,
save_count=50) # frames=100)#, repeat=True)
Writer = writers['ffmpeg']
writer = Writer(fps=30, bitrate=1800)
ani.save('{}.mp4'.format(name), writer=writer)
else:
# save = None
# Animate
ani = FuncAnimation(fig, animate, interval=1,
save_count=50) # frames=100)#, repeat=True)
pass
plt.ion()
plt.show()
time.sleep(0)
plt.close()
if rotation == "On":
rot_on()
elif rotation == "Off":
pass
def PLOT_ANIMATION(dCellList,
tCellsX,
tCellsY,
tCellsZ,
arrivalTimes,
save=False):
fig = plt.figure()
ax = p3.Axes3D(fig)
lines = [ax.plot([], [], [], 'o', c='r')[0]
for _ in range(numTCells)] # lines to animate
time_text = ax.text(0.02, 0.95, 0.95, 'time = 0', transform=ax.transAxes)
lines.append(time_text)
# Set the axes properties
ax.set_xlim3d([-500, 500])
ax.set_xlabel('X')
ax.set_ylim3d([-500, 500])
ax.set_ylabel('Y')
ax.set_zlim3d([-500, 500])
ax.set_zlabel('Z')
ax.set_title('Lymph Node Visualization')
def init():
for i in range(numDCells):
coords = dCellList[i].posn
ax.scatter(coords[0], coords[1], coords[2], c='b')
for line in lines[:-1]:
line.set_data([], [])
line.set_3d_properties([])
return lines
def update_lines(num):
x = 0
for line in lines[:-1]:
if tCellsX[x, num] == tCellsX[x, num - 1]:
# cell must have been activates
line.set_color("black")
line.set_data(np.vstack((tCellsX[x, num], tCellsY[x, num])))
line.set_3d_properties(tCellsZ[x, num])
x += 1
lines[-1].set_text("time = " + str(num + firstDCArrival))
return lines
ani = animation.FuncAnimation(fig,
update_lines,
int(numTimeSteps -
firstDCArrival / timeStep),
init_func=init,
interval=50,
blit=True,
repeat=False)
if save:
ani.save('animation.gif', writer='imagemagick', fps=30)
plt.show()
def __init__(self):
np.random.seed(19680801)
self._dtheta = 0.05
self._dx = 10
self._link_L = 160
self._link_N = 12
self._N = 200
# self.Kp_curve = 0.00041
# moved to reset_sim()
# self.robot = Robot()
# self.joint_cmd = MultiDOFJointState()
self.reset_sim()
rospy.Subscriber('/joy', Joy, self.joy_update)
self.pub_joint_cmd = rospy.Publisher('/robot_snake_sim/joint_ang_cmd/', MultiDOFJointState, queue_size=10)
### initiate plot
fig = plt.figure()
ax = p3.Axes3D(fig)
ax.view_init(elev=35, azim=-150)
# data = [self.Gen_RandLine(25, 3)]
# print(data)
data = self.robot.head_axis
# Creating fifty line objects.
# NOTE: Can't pass empty arrays into 3d version of plot()
axes_color = ["red", "green", "blue"]
head_lines = [ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1])[0] for dat in data]
for color, line in zip(axes_color, head_lines):
line.set_color(color)
path = self.robot.path
path_lines, = ax.plot(path[0, :], path[1, :], path[2, :], 'k.', markersize=0.1)
joint_lines, = ax.plot(path[0, :], path[1, :], path[2, :], '--or')
# joint_recon_lines, = ax.plot(path[0, :], path[1, :], path[2, :], 'sg')
ax.set_xlim3d([0, 2000])
ax.set_ylim3d([-1000, 1000])
ax.set_zlim3d([-1000, 1000])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Test')
# line_ani = animation.FuncAnimation(fig, func=self.update_lines, frames=100, fargs=(head_lines, path_lines, joint_lines, joint_recon_lines), interval=1, blit=False)
line_ani = animation.FuncAnimation(fig, func=self.update_lines, frames=100, fargs=(head_lines, path_lines, joint_lines), interval=5, blit=False)
plt.show()
'''
self.pub_vec = [rospy.Publisher('/snake_10/linear_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint1_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint2_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint3_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint4_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint5_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint6_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint7_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint8_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint9_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/snake_10/joint10_position_controller/command', Float64, queue_size=10)]
self.pub = rospy.Publisher(_cmd_topic, Twist, queue_size=10)
'''
# self.reset_sim()
# self.joint_vals = Float32MultiArray()
# self.joint_vals.data[0] = [0]
# for i in range(self._link_N):
# self.joint_vals.data[i + 1] = 0
# self.angle_range = np.linspace(-np.pi / 2, np.pi, _N)
# self.head = Twist()
# self.head.linear.x = 0
# self.head.angular.z = 0
# self.x_move = 0
# self.theta_move = 0
while not rospy.is_shutdown():
# self.head.linear.x = 0
# self.head.angular.z = 0
# self.head.linear.x += self.x_move
# self.head.angular.z += self.theta_move
# self.pub.publish(self.head)
#
# self.pub_vec[0].publish(self.head.linear.x)
# self.pub_vec[1].publish(self.head.angular.z)
self.rate.sleep()
def shape(fig, alpha, color, edge_c, edge_w, grid, sides, edges, multi_pi,
radiusm, radiusa, height, figcolor, rotation, rotmagt, rotmagp,
save):
plt.clf()
# Definition of x
def x_(u, v):
x = a * cos(v)
return x
# Definition of y
def y_(u, v):
y = b * sin(v)
return y
# Definition of z
def z_(u, v):
z = u
return z
# Height
h = height
# Radius
a = radiusm
b = radiusa
# Number of edges on the base
s = sides
# Value of the angles
u = linspace(0, h, edges)
v = linspace(0, 2 * pi, s + 1)
u, v = meshgrid(u, v)
# Symbolic representation
x = x_(u, v)
y = y_(u, v)
z = z_(u, v)
# Figure Properties
ax = p3.Axes3D(fig)
ax.set_facecolor(figcolor) # Figure background turns black
# Axis Properties
plt.axis(grid) # Turns off the axis grid
plt.axis('equal')
# Axis Limits
# ax.set_xlim(-1 * a,a)
# ax.set_ylim(-1 * a,a)
# ax.set_zlim(0,h)
#Surface Plot
prism = ax.plot_surface(x, y, z)
prism.set_alpha(alpha) # Transparency of figure
prism.set_edgecolor(edge_c) # Edge color of the lines on the figure
prism.set_linewidth(edge_w) # Line width of the edges
prism.set_facecolor(color) # General color of the figure
def rot_on():
def animate(i):
ax.view_init(azim=rotmagt * i, elev=rotmagp * i)
if save == "MP4":
# Animate
ani = FuncAnimation(fig,
animate,
frames=500,
interval=100,
save_count=50) # frames=100)#, repeat=True)
Writer = writers['ffmpeg']
writer = Writer(fps=30, bitrate=1800)
ani.save('{}.mp4'.format(name), writer=writer)
else:
#save = None
# Animate
ani = FuncAnimation(fig, animate, interval=1,
save_count=50) # frames=100)#, repeat=True)
pass
plt.ion()
plt.show()
time.sleep(0)
plt.close()
if rotation == "On":
rot_on()
elif rotation == "Off":
pass
def track_animation(data):
import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.animation as animation
# Fixing random state for reproducibility
np.random.seed(19680801)
def update_lines(num, dataLines, lines):
for line, data in zip(lines, dataLines):
# NOTE: there is no .set_data() for 3 dim data...
line.set_data(data[0:2, :num])
line.set_3d_properties(data[2, :num])
return lines
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
# Fifty lines of random 3-D lines
cell_ids = list(set(data[:, keys.index('cell_id')]))
cell_tracks = []
print(len(cell_ids))
for cell in cell_ids[::10]:
cell_track = np.array([
data[data[:, keys.index('cell_id')] == cell,
keys.index('X')], data[data[:, keys.index('cell_id')] == cell,
keys.index('Y')],
data[data[:, keys.index('cell_id')] == cell,
keys.index('Z')]
]).transpose()
# print(cell, cell_track.shape)
if cell_track.shape[0] > 10:
cell_tracks.append(cell_track.transpose())
# Creating fifty line objects.
# NOTE: Can't pass empty arrays into 3d version of plot()
lines = [
ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1])[0]
for dat in cell_tracks
]
# Setting the axes properties
# ax.set_xlim3d([0.0, 1.0])
# ax.set_xlabel('X')
# ax.set_ylim3d([0.0, 1.0])
# ax.set_ylabel('Y')
# ax.set_zlim3d([0.0, 1.0])
# ax.set_zlabel('Z')
# ax.set_title('3D Test')
# Creating the Animation object
line_ani = animation.FuncAnimation(fig,
update_lines,
25,
fargs=(cell_tracks, lines),
interval=50,
blit=False)
plt.show()
return
def splot(X, A):
import mpl_toolkits.mplot3d.axes3d as p3
fig = plt.figure()
ax = p3.Axes3D(fig)
xv, yv = np.meshgrid(X, X, sparse=False, indexing='ij')
ax.plot_wireframe(xv, yv, A)
def plot_function_3d(x, y, function, **kwargs):
"""Plot values of a function of two variables in 3D.
More on 3D plotting in pylab:
http://www.scipy.org/Cookbook/Matplotlib/mplot3D
Parameters
----------
x : array_like of float
The plotting range on X axis.
y : array_like of float
The plotting range on Y axis.
function : function
The function to plot.
plot_type : {'surface', 'wireframe', 'scatter', 'contour', 'contourf'}, keyword only, optional
The type of a plot, see
`scipy cookbook <http://www.scipy.org/Cookbook/Matplotlib/mplot3D>`_
for examples. The default value is 'surface'.
num_contours : int
The number of contours to plot, 50 by default.
xlabel : str, keyword only, optional
The X axis label. Empty by default.
ylabel : str, keyword only, optional
The Y axis label. Empty by default.
zlabel : str, keyword only, optional
The Z axis label. Empty by default.
title : str, keyword only, optional
The title. Empty by default.
**kwargs
Passed to the respective plotting function.
"""
import mpl_toolkits.mplot3d.axes3d as pylab3d
ax = pylab3d.Axes3D(pylab.gcf())
ax.set_xlabel(kwargs.pop('xlabel', ''))
ax.set_ylabel(kwargs.pop('ylabel', ''))
ax.set_zlabel(kwargs.pop('zlabel', ''))
ax.set_title(kwargs.pop('title', ''))
X, Y = np.meshgrid(x, y)
Z = []
for y_value in y:
Z.append([])
for x_value in x:
Z[-1].append(function(x_value, y_value))
Z = np.array(Z)
plot_type = kwargs.pop('plot_type', 'surface')
if plot_type == 'surface':
ax.plot_surface(X,
Y,
Z,
rstride=kwargs.pop('rstride', 1),
cstride=kwargs.pop('cstride', 1),
cmap=kwargs.pop('cmap', pylab.cm.jet),
**kwargs)
elif plot_type == 'wireframe':
ax.plot_wireframe(X,
Y,
Z,
cmap=kwargs.pop('cmap', pylab.cm.jet),
**kwargs)
elif plot_type == 'scatter':
ax.scatter3D(np.ravel(X), np.ravel(Y), np.ravel(Z), **kwargs)
elif plot_type == 'contour':
num_contours = kwargs.pop('num_contours', 50)
ax.contour3D(X,
Y,
Z,
num_contours,
cmap=kwargs.pop('cmap', pylab.cm.jet),
**kwargs)
elif plot_type == 'contourf':
num_contours = kwargs.pop('num_contours', 50)
ax.contourf3D(X,
Y,
Z,
num_contours,
cmap=kwargs.pop('cmap', pylab.cm.jet),
**kwargs)
else:
raise PyteomicsError('Unknown plot type: {}'.format(plot_type))
def plotAxis(
translation,
rotation,
label=None,
ax=None,
figsize=(5, 5),
point_color="black",
normalize_translation=False,
cameraSize=1,
):
"""
Plots a camera's coordinates in 3D.
Args:
translation: a 3x1 translation vector.
rotation: a 3x3 rotation matrix.
label: string; a label for the trajectory to be drawn.
ax: Axes3D instance; if this drawing is part of a bigger drawing function, this share the parent's axis (so that more than function could draw on the same canvas).
figsize: Tuple[int, int]; If new axis is created, this instructs the figure size to the function-- i.e. `figsize = (5,5)`.
point_color: string; This decides the color of the camera center.
normalize_translation: boolean; Whether or not to normalize the the camera center (i.e. the translation passed to this function).
"""
if not ax:
fig = plt.figure(figsize=figsize)
ax = p3.Axes3D(fig)
# Setting the axes properties
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
r1 = rotation[:, 0] / np.linalg.norm(rotation[:, 0])
r2 = rotation[:, 1] / np.linalg.norm(rotation[:, 1])
r3 = rotation[:, 2] / np.linalg.norm(rotation[:, 2])
if normalize_translation:
translation = translation / np.linalg.norm(translation)
d1 = np.array([translation, (translation + cameraSize * r1)])
d2 = np.array([translation, (translation + cameraSize * r2)])
d3 = np.array([translation, (translation + cameraSize * r3)])
d_center = np.array([d1, d2, d3]).mean(axis=0)
# Plot center
ax.scatter(translation[0],
translation[1],
translation[2],
c=point_color,
s=35)
# Plot axes
ax.plot(d1[:, 0], d1[:, 1], d1[:, 2], color="red")
ax.plot(d2[:, 0], d2[:, 1], d2[:, 2], color="green")
ax.plot(d3[:, 0], d3[:, 1], d3[:, 2], color="blue")
ax.plot(d_center[:, 0], d_center[:, 1], d_center[:, 2], color="black")
# Plot triangles to show camera direction
face_vertices = [d1[1], d2[1], d3[1]]
verts = [face_vertices]
ax.add_collection3d(Poly3DCollection(verts, alpha=0.1))
if label:
ax.text(
translation[0],
translation[1],
translation[2],
label,
size=20,
zorder=1,
color="k",
)
return ax
cax = imshow(disparity, interpolation='nearest', vmin=dmin, vmax=dmax)
colorbar(cax, shrink=.5)
if plot_wireframe:
## Plot wireframe
## Split the xyz 3 "channels" into three images with proper shape.
x, y, z = [
xx.T for xx in sqmesh.xyz.reshape(*(list(sqmesh.disparity.shape) +
[3])).T
]
## Get the estimated model coordinates
p = surf.coordinates()
fig = figure()
ax = p3.Axes3D(fig, aspect='equal')
title('Square mesh on 3D space', fontsize=20, fontweight='bold')
ax.axis('equal')
ax.plot_wireframe(x, y, z, color='#8888ff')
ax.plot_wireframe(surf.q[:, 0].reshape(Nl, Nk),
surf.q[:, 1].reshape(Nl, Nk),
surf.q[:, 2].reshape(Nl, Nk),
color='g')
ax.plot_wireframe(p[:, 0].reshape(Nl, Nk),
p[:, 1].reshape(Nl, Nk),
p[:, 2].reshape(Nl, Nk),
color='r')
mrang = max([x.max() - x.min(),
y.max() - y.min(),
# Maintenant, on va quand même utiliser les facilités de Numpy pour
# effectuer le calcul
import numpy as np
positions = np.array(simulation_verlet(deltat, n))
# Et à présent, les animations proprement dites
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as p3
from matplotlib import animation # Pour l'animation progressive
fig = plt.figure(figsize=(10, 10))
ax = p3.Axes3D(fig) # Attaching 3D axis to the figure
# Creating line objects.
# NOTE: Can't pass empty arrays into 3d version of plot()
data = [positions[:, i, :] for i in range(len(m))]
lignes = [ax.plot(dat[:, 0], dat[:, 1], dat[:, 2])[0] for dat in data]
cote = 3
ax.set_xlim3d([-cote, cote])
ax.set_xlabel('X')
ax.set_ylim3d([-cote, cote])
ax.set_ylabel('Y')
ax.set_zlim3d([-cote, cote])
ax.set_zlabel('Z')
ax.set_title("Petit amas stellaire, $t={}$".format(tmax))
from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt import numpy as np ax = axes3d.Axes3D(plt.figure()) i = np.arange(-1, 1, 0.01) X, Y = np.meshgrid(i, i) Z = X**2 - Y**2 ax.plot_wireframe(X, Y, Z, rstride=5, cstride=10) plt.show()
def sameAxisAnimationN(config, myData, traj, quadList, obsPF, myColour='blue'):
# Initialize list for objects
# ---------------------------
t_all = []
pos_all = []
quat_all = []
sDes_tr_all = []
waypoints = []
xyzType = []
yawType = []
x = []
y = []
z = []
xDes = []
yDes = []
zDes = []
x_wp = []
y_wp = []
z_wp = []
line1 = []
line2 = []
line3 = []
params = []
# shared
Ts = config.Ts
ifsave = config.ifsave
Po = obsPF.Po[:, 0:
-1] # cut off last one, as that represents the position of vechicle 1
obsRad = obsPF.obsRad
# # first vehicle
# t_all = myData.t_all
# pos_all = myData.pos_all
# quat_all = myData.quat_all
# sDes_tr_all = myData.sDes_traj_all
# waypoints = traj.wps
# xyzType = traj.xyzType
# yawType = traj.yawType
# # second vehicle
# t_all2 = myData2.t_all
# pos_all2 = myData2.pos_all
# quat_all2 = myData2.quat_all
# sDes_tr_all2 = myData2.sDes_traj_all
# waypoints2 = traj2.wps
# xyzType2 = traj2.xyzType
# yawType2 = traj2.yawType
# For the number of vehicles
# --------------------------
for iVeh in range(0, config.nVeh):
t_all.append(myData[iVeh].t_all)
pos_all.append(myData[iVeh].pos_all)
quat_all.append(myData[iVeh].quat_all)
sDes_tr_all.append(myData[iVeh].sDes_traj_all)
waypoints.append(traj[iVeh].wps)
xyzType.append(traj[iVeh].xyzType)
yawType.append(traj[iVeh].yawType)
params.append(quadList[iVeh].params)
# # first vehicle
# x = pos_all[:,0]
# y = pos_all[:,1]
# z = pos_all[:,2]
# xDes = sDes_tr_all[:,0]
# yDes = sDes_tr_all[:,1]
# zDes = sDes_tr_all[:,2]
# x_wp = waypoints[:,0]
# y_wp = waypoints[:,1]
# z_wp = waypoints[:,2]
# if (config.orient == "NED"):
# z = -z
# zDes = -zDes
# z_wp = -z_wp
# # second vehicle
# x2 = pos_all2[:,0]
# y2 = pos_all2[:,1]
# z2 = pos_all2[:,2]
# xDes2 = sDes_tr_all2[:,0]
# yDes2 = sDes_tr_all2[:,1]
# zDes2 = sDes_tr_all2[:,2]
# x_wp2 = waypoints2[:,0]
# y_wp2 = waypoints2[:,1]
# z_wp2 = waypoints2[:,2]
# if (config.orient == "NED"):
# z2 = -z2
# zDes2 = -zDes2
# z_wp2 = -z_wp2
# For the number of vehicles
# --------------------------
for iVeh in range(0, config.nVeh):
x.append(pos_all[iVeh][:, 0])
y.append(pos_all[iVeh][:, 1])
z.append(pos_all[iVeh][:, 2])
xDes.append(sDes_tr_all[iVeh][:, 0])
yDes.append(sDes_tr_all[iVeh][:, 1])
zDes.append(sDes_tr_all[iVeh][:, 2])
x_wp.append(waypoints[iVeh][:, 0])
y_wp.append(waypoints[iVeh][:, 1])
z_wp.append(waypoints[iVeh][:, 2])
if (config.orient == "NED"):
z[iVeh] = -z[iVeh]
zDes[iVeh] = -zDes[iVeh]
z_wp[iVeh] = -z_wp[iVeh]
fig = plt.figure()
ax = p3.Axes3D(fig)
# # first vehicle
# line1, = ax.plot([], [], [], lw=2, color=myColour)
# line2, = ax.plot([], [], [], lw=2, color='gray')
# line3, = ax.plot([], [], [], '--', lw=1, color=myColour)
# # second vehicle
# line1b, = ax.plot([], [], [], lw=2, color=myColour2)
# line2b, = ax.plot([], [], [], lw=2, color='gray')
# line3b, = ax.plot([], [], [], '--', lw=1, color=myColour2)
# For the number of vehicles
# --------------------------
for iVeh in range(0, config.nVeh):
line1.append([])
line2.append([])
line3.append([])
line1[iVeh], = ax.plot([], [], [], lw=2, color=myColour)
line2[iVeh], = ax.plot([], [], [], lw=2, color='gray')
line3[iVeh], = ax.plot([], [], [], '--', lw=1, color=myColour)
# Setting the axes properties
extraEachSide = 0.5
# find maxes from vehicles
# xmax = np.maximum(x.max(),x2.max())
# ymax = np.maximum(y.max(),y2.max())
# zmax = np.maximum(z.max(),z2.max())
# xmin = np.minimum(x.min(),x2.min())
# ymin = np.minimum(y.min(),x2.min())
# zmin = np.minimum(z.min(),x2.min())
xmax = np.max(x)
ymax = np.max(y)
zmax = np.max(z)
xmin = np.min(x)
ymin = np.min(y)
zmin = np.min(z)
# maxRange = 0.5*np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max() + extraEachSide
# mid_x = 0.5*(x.max()+x.min())
# mid_y = 0.5*(y.max()+y.min())
# mid_z = 0.5*(z.max()+z.min())
maxRange = 0.5 * np.array([xmax - xmin, ymax - ymin, zmax - zmin
]).max() + extraEachSide
mid_x = 0.5 * (xmax + xmin)
mid_y = 0.5 * (ymax + ymin)
mid_z = 0.5 * (zmax + zmin)
ax.set_xlim3d([mid_x - maxRange, mid_x + maxRange])
ax.set_xlabel('x-direction')
if (config.orient == "NED"):
ax.set_ylim3d([mid_y + maxRange, mid_y - maxRange])
elif (config.orient == "ENU"):
ax.set_ylim3d([mid_y - maxRange, mid_y + maxRange])
ax.set_ylabel('y-direction')
ax.set_zlim3d([mid_z - maxRange, mid_z + maxRange])
ax.set_zlabel('Altitude')
titleTime = ax.text2D(0.05, 0.95, "", transform=ax.transAxes)
#titleVeh1 = ax.text2D(0.05, 0.91, "vehicle 1", color=myColour, transform=ax.transAxes)
#titleVeh2 = ax.text2D(0.05, 0.87, "vehicle 2", color=myColour2, transform=ax.transAxes)
# # first vehicle
# trajType = ''
# yawTrajType = ''
# if (xyzType == 0):
# trajType = 'Hover'
# else:
# ax.scatter(x_wp, y_wp, z_wp, color=myColour, alpha=1, marker = 'o', s = 25)
# if (xyzType == 1 or xyzType == 12):
# trajType = 'Simple Waypoints'
# else:
# ax.plot(xDes, yDes, zDes, ':', lw=1.3, color=myColour)
# if (xyzType == 2):
# trajType = 'Simple Waypoint Interpolation'
# elif (xyzType == 3):
# trajType = 'Minimum Velocity Trajectory'
# elif (xyzType == 4):
# trajType = 'Minimum Acceleration Trajectory'
# elif (xyzType == 5):
# trajType = 'Minimum Jerk Trajectory'
# elif (xyzType == 6):
# trajType = 'Minimum Snap Trajectory'
# elif (xyzType == 7):
# trajType = 'Minimum Acceleration Trajectory - Stop'
# elif (xyzType == 8):
# trajType = 'Minimum Jerk Trajectory - Stop'
# elif (xyzType == 9):
# trajType = 'Minimum Snap Trajectory - Stop'
# elif (xyzType == 10):
# trajType = 'Minimum Jerk Trajectory - Fast Stop'
# elif (xyzType == 1):
# trajType = 'Minimum Snap Trajectory - Fast Stop'
# if (yawType == 0):
# yawTrajType = 'None'
# elif (yawType == 1):
# yawTrajType = 'Waypoints'
# elif (yawType == 2):
# yawTrajType = 'Interpolation'
# elif (yawType == 3):
# yawTrajType = 'Follow'
# elif (yawType == 4):
# yawTrajType = 'Zero'
# # second vehicle
# trajType2 = ''
# yawTrajType2 = ''
# if (xyzType2 == 0):
# trajType2 = 'Hover'
# else:
# ax.scatter(x_wp2, y_wp2, z_wp2, color=myColour2, alpha=1, marker = 'o', s = 25)
# if (xyzType2 == 1 or xyzType2 == 12):
# trajType2 = 'Simple Waypoints'
# else:
# ax.plot(xDes2, yDes2, zDes2, ':', lw=1.3, color=myColour2)
# if (xyzType2 == 2):
# trajType2 = 'Simple Waypoint Interpolation'
# elif (xyzType2 == 3):
# trajType2 = 'Minimum Velocity Trajectory'
# elif (xyzType2 == 4):
# trajType2 = 'Minimum Acceleration Trajectory'
# elif (xyzType2 == 5):
# trajType2 = 'Minimum Jerk Trajectory'
# elif (xyzType2 == 6):
# trajType2 = 'Minimum Snap Trajectory'
# elif (xyzType2 == 7):
# trajType2 = 'Minimum Acceleration Trajectory - Stop'
# elif (xyzType2 == 8):
# trajType2 = 'Minimum Jerk Trajectory - Stop'
# elif (xyzType2 == 9):
# trajType2 = 'Minimum Snap Trajectory - Stop'
# elif (xyzType2 == 10):
# trajType2 = 'Minimum Jerk Trajectory - Fast Stop'
# elif (xyzType2 == 1):
# trajType2 = 'Minimum Snap Trajectory - Fast Stop'
# if (yawType2 == 0):
# yawTrajType2 = 'None'
# elif (yawType2 == 1):
# yawTrajType2 = 'Waypoints'
# elif (yawType2 == 2):
# yawTrajType2 = 'Interpolation'
# elif (yawType2 == 3):
# yawTrajType2 = 'Follow'
# elif (yawType2 == 4):
# yawTrajType2 = 'Zero'
# n'th vehicle
trajType = ''
yawTrajType = ''
if (xyzType[0] == 0):
trajType = 'Hover'
else:
#for iVeh in range(0,config.nVeh):
#ax.scatter(x_wp[iVeh], y_wp[iVeh], z_wp[iVeh], color=myColour, alpha=1, marker = 'o', s = 25)
if (xyzType == 1 or xyzType == 12):
trajType = 'Simple Waypoints'
else:
#for iVeh in range(0,config.nVeh):
#ax.plot(xDes[iVeh], yDes[iVeh], zDes[iVeh], ':', lw=1.3, color=myColour)
if (xyzType == 2):
trajType = 'Simple Waypoint Interpolation'
elif (xyzType == 3):
trajType = 'Minimum Velocity Trajectory'
elif (xyzType == 4):
trajType = 'Minimum Acceleration Trajectory'
elif (xyzType == 5):
trajType = 'Minimum Jerk Trajectory'
elif (xyzType == 6):
trajType = 'Minimum Snap Trajectory'
elif (xyzType == 7):
trajType = 'Minimum Acceleration Trajectory - Stop'
elif (xyzType == 8):
trajType = 'Minimum Jerk Trajectory - Stop'
elif (xyzType == 9):
trajType = 'Minimum Snap Trajectory - Stop'
elif (xyzType == 10):
trajType = 'Minimum Jerk Trajectory - Fast Stop'
elif (xyzType == 1):
trajType = 'Minimum Snap Trajectory - Fast Stop'
if (yawType == 0):
yawTrajType = 'None'
elif (yawType == 1):
yawTrajType = 'Waypoints'
elif (yawType == 2):
yawTrajType = 'Interpolation'
elif (yawType == 3):
yawTrajType = 'Follow'
elif (yawType == 4):
yawTrajType = 'Zero'
# travis add
#o1 = np.array([-2, -1, -3]) # obstacle 1 (x,y,z)
#o2 = np.array([3, -2, 1]) # obstacle 2 (x,y,z)
#Po = np.vstack((o1,o2)).transpose() # stack obstacles
ax.scatter(np.squeeze(Po[0, :]),
np.squeeze(Po[1, :]),
-np.squeeze(Po[2, :]),
color='red',
alpha=1,
marker='o',
s=100 * obsRad)
#titleType1 = ax.text2D(0.95, 0.95, trajType, transform=ax.transAxes, horizontalalignment='right')
#titleType2 = ax.text2D(0.95, 0.91, 'Yaw: '+ yawTrajType, transform=ax.transAxes, horizontalalignment='right')
def updateLines(i):
# # first vehicle
# # -------------
# time = t_all[i*numFrames]
# pos = pos_all[i*numFrames]
# x = pos[0]
# y = pos[1]
# z = pos[2]
# x_from0 = pos_all[0:i*numFrames,0]
# y_from0 = pos_all[0:i*numFrames,1]
# z_from0 = pos_all[0:i*numFrames,2]
# dxm = params["dxm"]
# dym = params["dym"]
# dzm = params["dzm"]
# quat = quat_all[i*numFrames]
# if (config.orient == "NED"):
# z = -z
# z_from0 = -z_from0
# quat = np.array([quat[0], -quat[1], -quat[2], quat[3]])
# R = utils.quat2Dcm(quat)
# motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm], [-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym, dzm]])
# motorPoints = np.dot(R, np.transpose(motorPoints))
# motorPoints[0,:] += x
# motorPoints[1,:] += y
# motorPoints[2,:] += z
# line1.set_data(motorPoints[0,0:3], motorPoints[1,0:3])
# line1.set_3d_properties(motorPoints[2,0:3])
# line2.set_data(motorPoints[0,3:6], motorPoints[1,3:6])
# line2.set_3d_properties(motorPoints[2,3:6])
# line3.set_data(x_from0, y_from0)
# line3.set_3d_properties(z_from0)
#titleTime.set_text(u"Time = {:.2f} s".format(time))
# # second vehicle
# # -------------
# time2 = t_all2[i*numFrames]
# pos2 = pos_all2[i*numFrames]
# x2 = pos2[0]
# y2 = pos2[1]
# z2 = pos2[2]
# x_from02 = pos_all2[0:i*numFrames,0]
# y_from02 = pos_all2[0:i*numFrames,1]
# z_from02 = pos_all2[0:i*numFrames,2]
# dxm2 = params2["dxm"]
# dym2 = params2["dym"]
# dzm2 = params2["dzm"]
# quat2 = quat_all2[i*numFrames]
# if (config.orient == "NED"):
# z2 = -z2
# z_from02 = -z_from02
# quat2 = np.array([quat2[0], -quat2[1], -quat2[2], quat2[3]])
# R2 = utils.quat2Dcm(quat2)
# motorPoints2 = np.array([[dxm2, -dym2, dzm2], [0, 0, 0], [dxm2, dym2, dzm2], [-dxm2, dym2, dzm2], [0, 0, 0], [-dxm2, -dym2, dzm2]])
# motorPoints2 = np.dot(R2, np.transpose(motorPoints2))
# motorPoints2[0,:] += x2
# motorPoints2[1,:] += y2
# motorPoints2[2,:] += z2
# line1b.set_data(motorPoints2[0,0:3], motorPoints2[1,0:3])
# line1b.set_3d_properties(motorPoints2[2,0:3])
# line2b.set_data(motorPoints2[0,3:6], motorPoints2[1,3:6])
# line2b.set_3d_properties(motorPoints2[2,3:6])
# line3b.set_data(x_from02, y_from02)
# line3b.set_3d_properties(z_from02)
# #titleTime.set_text(u"Time = {:.2f} s".format(time))
# n'th vehicle
# -------------
for iVeh in range(0, config.nVeh):
time2 = t_all[iVeh][i * numFrames]
pos2 = pos_all[iVeh][i * numFrames]
x2 = pos2[0]
y2 = pos2[1]
z2 = pos2[2]
x_from02 = pos_all[iVeh][0:i * numFrames, 0]
y_from02 = pos_all[iVeh][0:i * numFrames, 1]
z_from02 = pos_all[iVeh][0:i * numFrames, 2]
dxm2 = params[iVeh]["dxm"]
dym2 = params[iVeh]["dym"]
dzm2 = params[iVeh]["dzm"]
quat2 = quat_all[iVeh][i * numFrames]
if (config.orient == "NED"):
z2 = -z2
z_from02 = -z_from02
quat2 = np.array([quat2[0], -quat2[1], -quat2[2], quat2[3]])
R2 = utils.quat2Dcm(quat2)
motorPoints2 = np.array([[dxm2, -dym2, dzm2], [0, 0, 0],
[dxm2, dym2, dzm2], [-dxm2, dym2, dzm2],
[0, 0, 0], [-dxm2, -dym2, dzm2]])
motorPoints2 = np.dot(R2, np.transpose(motorPoints2))
motorPoints2[0, :] += x2
motorPoints2[1, :] += y2
motorPoints2[2, :] += z2
line1[iVeh].set_data(motorPoints2[0, 0:3], motorPoints2[1, 0:3])
line1[iVeh].set_3d_properties(motorPoints2[2, 0:3])
line2[iVeh].set_data(motorPoints2[0, 3:6], motorPoints2[1, 3:6])
line2[iVeh].set_3d_properties(motorPoints2[2, 3:6])
line3[iVeh].set_data(x_from02, y_from02)
line3[iVeh].set_3d_properties(z_from02)
titleTime.set_text(u"Time = {:.2f} s".format(time2))
#return line1, line2, line1b, line2b
return line1, line2
def ini_plot():
# # vehicle 1
# line1.set_data([], [])
# line1.set_3d_properties([])
# line2.set_data([], [])
# line2.set_3d_properties([])
# line3.set_data([], [])
# line3.set_3d_properties([])
# # vehicle 2
# line1b.set_data([], [])
# line1b.set_3d_properties([])
# line2b.set_data([], [])
# line2b.set_3d_properties([])
# line3b.set_data([], [])
# line3b.set_3d_properties([])
# n'th vehicle
# -------------
for iVeh in range(0, config.nVeh):
line1[iVeh].set_data([], [])
line1[iVeh].set_3d_properties([])
line2[iVeh].set_data([], [])
line2[iVeh].set_3d_properties([])
line3[iVeh].set_data([], [])
line3[iVeh].set_3d_properties([])
#return line1, line2, line3, line1b, line2b, line3b
return line1, line2, line3
#my add (init_func messes up, made a new one here)
#line_ani = animation.FuncAnimation(fig, updateLines, blit=False, frames=len(t_all[0:-2:numFrames]), interval=(Ts*1000*numFrames))
line_ani = animation.FuncAnimation(fig,
updateLines,
blit=False,
frames=len(t_all[0][0:-2:numFrames]),
interval=(Ts * 1000 * numFrames))
# Creating the Animation object
# ORIGINAL line_ani = animation.FuncAnimation(fig, updateLines, init_func=ini_plot, frames=len(t_all[0:-2:numFrames]), interval=(Ts*1000*numFrames), blit=False)
if (ifsave):
# ORIGINAL line_ani.save('Gifs/Raw/animation_{0:.0f}_{1:.0f}.gif'.format(xyzType,yawType), dpi=80, writer='imagemagick', fps=25)
line_ani.save('Gifs/Raw/animation_multi_{0}_and_{1}.gif'.format(
myColour, myColour),
writer=writer) #my add
plt.show()
return line_ani
def save_simulation_movie(individuals, output_folder,\
n_individuals,n_timepoints,black_background=True, verbose=False):
"""Save an .ffmpg move of the simulated community change"""
#TODO: standardize these and put them up above
import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.animation as animation
#The code for writing animation files is essentially identical to the
#matplotlib tutorial here: http://matplotlib.org/examples/animation/basic_example_writer.html
Writer = animation.writers['ffmpeg']
writer = Writer(fps=15,
metadata=dict(artist=str(__author__)),
bitrate=1800)
# Attaching 3D axis to the figure
fig = plt.figure()
ax = p3.Axes3D(fig)
data = get_timeseries_data(individuals)
colors = [i.BaseParams["color"] for i in individuals]
if verbose:
print("Individual colors:", colors)
print("Movie raw data:", data)
ids = []
for item in individuals:
ids.append(item.SubjectId)
save_simulation_data(data, ids, output_folder)
# NOTE: Can't pass empty arrays into 3d version of plot()
linestyle = '-'
pointstyle = 'o' #cheat to use lines to represent points
lines = [ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1],linestyle,\
c=colors[i],alpha=0.20)[0] for i,dat in enumerate(data)]
points = [ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1],pointstyle,\
c=colors[i],alpha=1.0)[0] for i,dat in enumerate(data)]
# Setting the axes properties
ax.set_xlim3d([-1.0, 1.0])
ax.set_xlabel('PC1')
ax.set_ylim3d([-1.0, 1.0])
ax.set_ylabel('PC2')
ax.set_zlim3d([-1.0, 1.0])
ax.set_zlabel('PC3')
ax.set_title('Simulation Results')
if black_background:
#ax.set_axis_bgcolor('black') deprecated
ax.set_facecolor('black')
fig.patch.set_facecolor('black')
dull_red = (0.50, 0, 0, 1)
ax.w_xaxis.set_pane_color((0.0, 0.0, 0.0, 1))
ax.w_yaxis.set_pane_color((0.0, 0.0, 0.0, 1))
ax.w_zaxis.set_pane_color((0.0, 0.0, 0.0, 1))
#Set 3d background grid color to a dull red
new_grid_params = ax.w_xaxis._axinfo['grid']
new_grid_params.update({'color': dull_red, 'linewidth': 1.0})
if verbose:
print(new_grid_params)
ax.w_xaxis._axinfo.update({'grid': new_grid_params})
ax.w_yaxis._axinfo.update({'grid': new_grid_params})
ax.w_zaxis._axinfo.update({'grid': new_grid_params})
ax.spines['bottom'].set_color(dull_red)
ax.spines['left'].set_color(dull_red)
ax.xaxis.label.set_color(dull_red)
ax.yaxis.label.set_color(dull_red)
ax.zaxis.label.set_color(dull_red)
ax.tick_params(axis='x', colors=dull_red)
ax.tick_params(axis='y', colors=dull_red)
ax.tick_params(axis='z', colors=dull_red)
# Creating the Animation object
line_ani = animation.FuncAnimation(fig, update_3d_plot, n_timepoints, fargs=(data,ax,lines,points),\
interval=100, blit=False)
line_ani.save(join(output_folder, 'simulation_video.mp4'), writer=writer)
def __init__(self):
self._dtheta = 0.01 # Dimention in rad
self._dx = 0.1 # Dimention in mm
self._link_L = 160
self._link_N = 10
# self._N = 200
self._head_init_x = self._link_L * (self._link_N - 1)
self._head_init_y = 0
self._joint_limit = np.deg2rad(40)
self._joy_dead_zone = 0.12
self._linear_cmd_offset = self._link_L * (self._link_N - 1)
self.reset_sim()
rospy.Subscriber('/joy', Joy, self.joy_update)
# self.pub_joint_cmd = rospy.Publisher('/robot_snake_sim/joint_ang_cmd/', MultiDOFJointState, queue_size=10)
self.pub_joint_cmd = rospy.Publisher('/robot_snake_10/joint_cmd/',
Float32MultiArray,
queue_size=10)
self.pub_linear_cmd = rospy.Publisher('/robot_snake_10/linear_cmd/',
Float32,
queue_size=10)
# initiate plot
fig = plt.figure()
ax = p3.Axes3D(fig)
ax.view_init(elev=35, azim=-150)
headAxis_1 = self.robot.head_axis_1
headAxis_2 = self.robot.head_axis_2
# Creating fifty line objects.
# NOTE: Can't pass empty arrays into 3d version of plot()
axes_color = ["red", "green", "blue"]
head_lines_1 = [
ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1])[0]
for dat in headAxis_1
]
head_lines_2 = [
ax.plot(dat[0, 0:1], dat[1, 0:1], dat[2, 0:1])[0]
for dat in headAxis_2
]
for color, line_1, line_2 in zip(axes_color, head_lines_1,
head_lines_2):
line_1.set_color(color)
line_2.set_color(color)
# for color, line_1 in zip(axes_color, head_lines_1):
# line_1.set_color(color)
# for color, line_2 in zip(axes_color, head_lines_2):
# line_2.set_color(color)
path_lines, = ax.plot(self.robot.path[0, :],
self.robot.path[1, :],
self.robot.path[2, :],
'k.',
markersize=0.01)
joint_lines, = ax.plot(self.robot.path[0, :], self.robot.path[1, :],
self.robot.path[2, :], '--or')
joint_lines_odd, = ax.plot(self.robot.joint_pos[0, :],
self.robot.joint_pos[1, :],
self.robot.joint_pos[2, :], '--og')
joint_lines_even, = ax.plot(self.robot.joint_pos[0, :],
self.robot.joint_pos[1, :],
self.robot.joint_pos[2, :], '--og')
# joint_recon_lines, = ax.plot(path[0, :], path[1, :], path[2, :], 'sg')
ax.set_xlim3d([0, 2000])
ax.set_ylim3d([-1000, 1000])
ax.set_zlim3d([-1000, 1000])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Test')
print("start")
if _PLOT:
line_ani = animation.FuncAnimation(
fig,
func=self.update_lines,
frames=100,
fargs=(head_lines_1, head_lines_2, path_lines, joint_lines,
joint_lines_odd, joint_lines_even),
interval=5,
blit=False)
plt.show()
while not rospy.is_shutdown():
self.pub_joint_cmd.publish(
Float32MultiArray(data=self.robot.joint_cmd[1:]))
self.pub_linear_cmd.publish(
Float32(data=(self.robot.joint_cmd[0] +
self._linear_cmd_offset)))
self.rate.sleep()
clf() plot(x,stats.beta.pdf(x,1,0.9)) #[Out]# [<matplotlib.lines.Line2D object at 0x11b3da910>] plot(x,stats.beta.pdf(x,1,0.99)) #[Out]# [<matplotlib.lines.Line2D object at 0x11b84ccd0>] clf() plot(x,1/sqrt(x) ) #[Out]# [<matplotlib.lines.Line2D object at 0x11b86a550>] stats.import mpl_toolkits.mplot3d.axes3d as p3 import mpl_toolkits.mplot3d.axes3d as p3 fig=p.figure() fig=p.figure() fig=p3.figure() fig=figure() ax = p3.Axes3D(fig) #numpy.random.dirichlet #?numpy.random.dirichlet (x,y,z)=numpy.random.dirichlet([0.5,3,3], 3) (x,y,z)=numpy.random.dirichlet([0.5,3,3], 5) s=numpy.random.dirichlet([0.5,3,3], 5) s #[Out]# array([[ 0.08619444, 0.62634005, 0.28746551], #[Out]# [ 0.12159434, 0.54585735, 0.33254831], #[Out]# [ 0.3518952 , 0.5364836 , 0.11162121], #[Out]# [ 0.30257971, 0.2858588 , 0.41156149], #[Out]# [ 0.02345192, 0.17496258, 0.8015855 ]]) s[1:] #[Out]# array([[ 0.12159434, 0.54585735, 0.33254831], #[Out]# [ 0.3518952 , 0.5364836 , 0.11162121], #[Out]# [ 0.30257971, 0.2858588 , 0.41156149],
from mpl_toolkits.mplot3d import axes3d
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
def generate(X, Y, phi):
R = 1 - np.sqrt(X**2 + Y**2)
return np.cos(2 * np.pi * X + phi) * R
fig = plt.figure()
ax = axes3d.Axes3D(fig)
xs = np.linspace(-1, 1, 50)
ys = np.linspace(-1, 1, 50)
X, Y = np.meshgrid(xs, ys)
Z = generate(X, Y, 0.0)
wframe = ax.plot_wireframe(X, Y, Z, rstride=2, cstride=2)
ax.set_zlim(-1, 1)
def update(i, ax, fig):
ax.cla()
phi = i * 360 / 2 / np.pi / 100
Z = generate(X, Y, phi)
wframe = ax.plot_wireframe(X, Y, Z, rstride=2, cstride=2)
ax.set_zlim(-1, 1)
return wframe,
def animate_ghost(df_real,
df_pred,
edges=[],
axes=None,
frames=50,
speed=45,
figsize=(7, 5),
colors=None,
ghost_shift=0.3):
'''
General function that can plot numpy arrays in either of two shapes.
@param numpy.ndarray df: a 2D numpy array with shape d[vert][time][dimension]
@param numpy.ndarray edges: 2 2D numpy array with shape [[i, j]] where i and j
are indices into the `vertices` in X
@param dict axes: dict that maps {x, y, z} keys to (min_axis_val, max_axis_val)
@param int frames: the number of time slices to animate
@param int speed: the temporal duration of each frame. Increase to boost fps
@param tuple figsize: the size of the figure to render
@param {str|list}: string or list of color values (if list, one per edge to be drawn)
'''
if axes is None:
axes = {'x': (0, 1), 'y': (0, 1), 'z': (0, 1.5)}
fig = plt.figure(figsize=figsize)
ax = p3.Axes3D(fig)
ax.set_xlim(*axes['x'])
ax.set_ylim(*axes['y'])
ax.set_zlim(*axes['z'])
plt.close(fig)
if edges:
params = {'edges': edges}
callback = update_lines_ghost
lines_real, geoms_real = [[df_real[i, 0, :], df_real[j, 0, :]]
for i, j in edges], []
for idx, i in enumerate(lines_real):
c = 'black'
# if colors and isinstance(colors, list): c = colors[idx]
# elif colors: c = colors
# else: c = plt.cm.RdYlBu(idx/len(lines_real))
geoms_real.append(
ax.plot([i[0][0], i[1][0]], [i[0][1], i[1][1]],
[i[0][2], i[1][2]],
color=c))
lines_pred, geoms_pred = [[df_pred[i, 0, :], df_pred[j, 0, :]]
for i, j in edges], []
for idx, i in enumerate(lines_pred):
if colors and isinstance(colors, list): c = colors[idx]
elif colors: c = colors
else: c = plt.cm.RdYlBu(idx / len(lines_pred))
geoms_pred.append(
ax.plot([i[0][0], i[1][0]], [i[0][1], i[1][1]],
[i[0][2], i[1][2]],
color=c))
else:
params = None
callback = update_points_ghost
geoms_real = ax.scatter(df_real[:, 0, 0],
df_real[:, 0, 1],
df_real[:, 0, 2],
depthshade=False) # x,y,z vals
geoms_pred = ax.scatter(df_pred[:, 0, 0],
df_pred[:, 0, 1],
df_pred[:, 0, 2],
depthshade=False) # x,y,z vals
return animation.FuncAnimation(fig,
callback,
frames,
interval=speed,
fargs=(geoms_real, df_real, geoms_pred,
df_pred, params, ghost_shift),
blit=False).to_html5_video()
