def show_robot(link_lengths, beta, thetas):
a_vec, beta_vec, d_vec, theta_vec = get_table(link_lengths, beta,
rad_to_grad(thetas))
t_01 = build_transformation_matrix(a_vec[0], beta_vec[0], d_vec[0],
theta_vec[0])
t_02 = np.dot(
t_01,
build_transformation_matrix(a_vec[1], beta_vec[1], d_vec[1],
theta_vec[1]))
t_03 = np.dot(
t_02,
build_transformation_matrix(a_vec[2], beta_vec[2], d_vec[2],
theta_vec[2]))
t_04 = np.dot(
t_03,
build_transformation_matrix(a_vec[3], beta_vec[3], d_vec[3],
theta_vec[3]))
x_points = [0, t_01[0, 3], t_02[0, 3], t_03[0, 3], t_04[0, 3]]
y_points = [0, t_01[1, 3], t_02[1, 3], t_03[1, 3], t_04[1, 3]]
z_points = [0, t_01[2, 3], t_02[2, 3], t_03[2, 3], t_04[2, 3]]
fig = plt.figure()
ax = fig.gca(projection='3d')
Axes3D.plot(ax, xs=x_points, ys=y_points, zs=z_points)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.scatter(x_points, y_points, z_points, color='orange', marker='o')
plt.xlim(0, 2)
plt.ylim(0, 2)
ax.set_zlim(0, 3)
plt.show()
def plot3D(show=True):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# ax = fig.gca(projection='3d')
predicted = sess.run(inference(X), feed_dict={X: train_X})
x = np.squeeze(np.asarray(train_X1))
y = np.squeeze(np.asarray(train_X2))
z = np.squeeze(np.asarray(train_Y))
z1 = np.squeeze(np.asarray(predicted))
ax.plot(x, y, zs=z, c='b', label='Original')
ax.plot(x, y, zs=z1, c='r', label='Fit')
# ax.scatter(train_X1, train_X2, train_Y, c='y', marker='s')
# ax.scatter(train_X1, train_X2, predicted, c='r', marker='v')
ax.set_xlabel('X1')
ax.set_ylabel('X2')
ax.set_zlabel('Y')
ax.legend()
buf = io.BytesIO()
if show:
# If want to view image in Tensorboard, do not show plot => Strange
# bug!!!
plt.show()
else:
# plt.savefig("/tmp/test.png", format='png')
plt.savefig(buf, format='png')
buf.seek(0)
return buf
def draw_axis(extracted_waypoints_rel,x_axis,y_axis,z_axis): fig = plt.figure() ax = fig.add_subplot(111, projection='3d') size=0.8 ax.set_xlim3d(-size, size) ax.set_ylim3d(-size, size) ax.set_zlim3d(-size, size) for i in range(len(extracted_waypoints_rel)): VecStart_x=extracted_waypoints_rel[i][0] VecStart_y=extracted_waypoints_rel[i][1] VecStart_z=extracted_waypoints_rel[i][2] VecEnd_x=x_axis[i][0] VecEnd_y=x_axis[i][1] VecEnd_z=x_axis[i][2] ax.plot([VecStart_x, VecEnd_x], [VecStart_y,VecEnd_y],zs=[VecStart_z,VecEnd_z],c="r") for i in range(len(extracted_waypoints_rel)): VecStart_x=extracted_waypoints_rel[i][0] VecStart_y=extracted_waypoints_rel[i][1] VecStart_z=extracted_waypoints_rel[i][2] VecEnd_x=y_axis[i][0] VecEnd_y=y_axis[i][1] VecEnd_z=y_axis[i][2] ax.plot([VecStart_x, VecEnd_x], [VecStart_y,VecEnd_y],zs=[VecStart_z,VecEnd_z],c="g") for i in range(len(extracted_waypoints_rel)): VecStart_x=extracted_waypoints_rel[i][0] VecStart_y=extracted_waypoints_rel[i][1] VecStart_z=extracted_waypoints_rel[i][2] VecEnd_x=z_axis[i][0] VecEnd_y=z_axis[i][1] VecEnd_z=z_axis[i][2] ax.plot([VecStart_x, VecEnd_x], [VecStart_y,VecEnd_y],zs=[VecStart_z,VecEnd_z],c="b") plt.show() Axes3D.plot()
def plotFrame(frame : LieGroup, style='-', ax : Axes3D = None):
if not isinstance(frame, (SIM3, SE3, SO3)):
raise TypeError("The frame must be of one of the types: SIM3, SE3, SO3.")
if ax is None:
ax = plt.gca()
if not isinstance(ax, Axes3D):
raise TypeError("The axes must be of 3D type.")
if isinstance(frame, SO3):
mat = np.eye(4)
mat[0:3,0:3] = frame.as_matrix()
else:
mat = frame.as_matrix()
Q = mat[0:3,0:3]
t = mat[0:3,3:4]
frame_axes = [[],[],[]]
for a in range(3):
for i in range(3):
frame_axes[a] += [t[a,0], t[a,0]+Q[a,i], np.nan]
# result = ax.plot(frame_axes[0], frame_axes[1], frame_axes[2])
colors = ['r','g','b']
colors = [c+style for c in colors]
result = []
for a in range(3):
temp = ax.plot([t[0,0], t[0,0]+Q[0,a]], [t[1,0], t[1,0]+Q[1,a]], [t[2,0], t[2,0]+Q[2,a]], colors[a])
result.append(temp)
temp = ax.plot(t[0,0], t[1,0], t[2,0], 'ko')
result.append(temp)
return result
def plotData(figNum, ts, sx, sy, vx, vy, yaw, vmag, acceleration):
# plot results
fig = plt.figure(figNum)
ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, sx, sy, ts, c='r', marker='o')
plt.ylabel('Position/Velocity y')
plt.xlabel('Position/Velocity x')
ax.set_zlabel('time')
# fig = plt.figure(3)
# ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, vx, vy, ts, c='b', marker='.')
plt.figure(figNum + 1)
plt.subplot(3, 1, 1)
plt.plot(ts, vmag, 'r-')
plt.ylabel('v_mag')
plt.subplot(3, 1, 2)
plt.plot(ts, np.multiply(yaw, 1 / math.pi), 'r-')
plt.ylabel('yaw')
plt.subplot(3, 1, 3)
plt.plot(ts, acceleration, 'o-')
plt.ylabel('acceleration')
def _connect_points_3d(ax_3d: Axes3D, point_a: array_like, point_b: array_like,
**kwargs) -> None:
"""
Plot a line between two 3D points.
Parameters
----------
ax_3d : Axes3D
Instance of :class:`~mpl_toolkits.mplot3d.axes3d.Axes3D`.
point_a, point_b : array_like
The two 3D points to be connected.
kwargs : dict, optional
Additional keywords passed to :meth:`~mpl_toolkits.mplot3d.axes3d.Axes3D.plot`.
Raises
------
ValueError
If the axis is not an instance of Axes3D.
"""
if not isinstance(ax_3d, Axes3D):
raise ValueError("Axis must be instance of class Axes3D.")
xs = [point_a[0], point_b[0]]
ys = [point_a[1], point_b[1]]
zs = [point_a[2], point_b[2]]
ax_3d.plot(xs, ys, zs, **kwargs)
def _draw_line(ax: Axes3D,
a: np.ndarray,
b: np.ndarray,
color: str = "blue",
line_style: str = "-",
label: str = None):
x = np.array([a[0], b[0]])
y = np.array([a[1], b[1]])
z = np.array([a[2], b[2]])
ax.plot(x, y, z, color=color, linestyle=line_style, label=label)
def plot_sample_list(sample_list,lim_val=10):
sample_mat = np.array(sample_list)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(sample_mat[:,0], sample_mat[:,1], sample_mat[:,2], c='r', marker='o')
ax.set_xlim(-lim_val, lim_val)
ax.set_ylim(-lim_val, lim_val)
ax.set_zlim(-lim_val, lim_val)
plt.show()
Axes3D.plot()
def _draw_ray(ax: Axes3D,
ray: Ray,
length: float,
color: str = "blue",
line_style: str = "-",
label: str = None):
end_point = ray.point(length)
x = np.array([ray.position[0], end_point[0]])
y = np.array([ray.position[1], end_point[1]])
z = np.array([ray.position[2], end_point[2]])
ax.plot(x, y, z, color=color, linestyle=line_style, label=label)
def plot(msg):
global figure
global plt
global lines
p_1, p_2, p_3 = dynamics.fk(msg.position)
xdata = [p_1[0], p_2[0], p_3[0]]
ydata = [p_1[1], p_2[1], p_3[1]]
zdata = [p_1[2], p_2[2], p_3[2]]
Axes3D.plot(xdata, ydata, zs=zdata)
plt.draw()
def makeequator( ax ) :
npts = 256
x = numpy.zeros( npts )
y = numpy.zeros( npts )
z = numpy.zeros( npts )
n = 0
for phi in numpy.arange( 0., 2*math.pi, 2*math.pi/npts ) :
x[n] = math.cos(phi)
y[n] = math.sin(phi)
n = n+1
q = Axes3D.plot(ax, x, y, zs=z, c='gray' )
q = Axes3D.plot(ax, x, z, zs=y, c='gray' )
q = Axes3D.plot(ax, z, y, zs=x, c='gray' )
def lineplot(deg): import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import numpy as np fig = plt.figure() ax = fig.add_subplot(111, projection='3d') arr = np.array(deg) #for n in range(5): # for val in deg: ax.plot( range(0,len(arr)), arr, 0 ) plt.show() Axes3D.plot()
def plotTrajectory(self):
try:
print("printing trajectory?")
ts = self.opt.d.time * self.opt.tf.value[0]
# Switch to right figure
self.fig
# plt.subplot(2, 1, 1)
Axes3D.plot(self.ax,
self.opt.sx.value,
self.opt.sy.value,
ts,
c='r',
marker='o')
plt.ylabel('Position/Velocity y')
plt.xlabel('Position/Velocity x')
self.ax.set_zlabel('time')
# fig = plt.figure(3)
# self.ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(self.ax,
self.opt.vx.value,
self.opt.vy.value,
ts,
c='b',
marker='.')
self.fig2
plt.clf()
plt.subplot(3, 1, 1)
plt.plot(ts, self.opt.a, 'r-')
plt.ylabel('acceleration')
plt.subplot(3, 1, 2)
plt.plot(ts, np.multiply(self.opt.yaw, 180 / math.pi), 'r-')
plt.ylabel('turning input')
plt.subplot(3, 1, 3)
plt.plot(ts, self.opt.v_mag, 'b-')
plt.ylabel('vmag')
plt.ion()
plt.show()
plt.pause(0.001)
except Exception as e:
self.PrintException()
def plot(self, axes: Axes3D, highlight_max=True):
# Plot all unpicked relay positions
unpicked = list(
filter(lambda x: x not in self.relays, self.input.relays))
rx = list(map(lambda r: r.x, unpicked))
ry = list(map(lambda r: r.y, unpicked))
rz = list(map(lambda r: r.height, unpicked))
axes.scatter(rx, ry, rz, c='#a5b6d3', label='Potential positions.')
# Highlight ground
gx = np.arange(0, self.input.width + 10)
gy = np.arange(0, self.input.height + 10)
gx, gy = np.meshgrid(gx, gy)
gz = gx * 0
axes.plot_surface(gx, gy, gz, alpha=0.2, color='brown')
# Plot assignments
for rn, sns in self.relay_to_sensors.items():
for sn in sns:
tx, ty = self.interm_point(sn, rn)
x = [rn.x, tx, sn.x]
y = [rn.y, ty, sn.y]
z = [rn.height, 0, -sn.depth]
axes.plot(x, y, z, linewidth=0.5, c='#000000')
# Highlight max pair
if highlight_max:
ms, mr = self.max_loss_pair
tx, ty = self.interm_point(ms, mr)
mx = [ms.x, tx, mr.x]
my = [ms.y, ty, mr.y]
mz = [-ms.depth, 0, mr.height]
axes.plot(mx,
my,
mz,
c='tab:orange',
label='Max assignment (%.2f)' % self.max_loss)
# Plot all sensors
sx = list(map(lambda s: s.x, self.sensors))
sy = list(map(lambda s: s.y, self.sensors))
sz = list(map(lambda s: -s.depth, self.sensors))
axes.scatter(sx, sy, sz, c='r', label='Sensors')
# Plot all relays
rx = list(map(lambda r: r.x, self.relays))
ry = list(map(lambda r: r.y, self.relays))
rz = list(map(lambda r: r.height, self.relays))
axes.scatter(rx, ry, rz, c='b', label='Relays')
def visualize( Z, solution=[]):
Y = np.arange(0,YSIZE,1)
X = np.arange(0,XSIZE,1)
fig = plt.figure()
ax = fig.gca(projection='3d')
X, Y = np.meshgrid(X, Y)
surf = ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm, linewidth=0, antialiased=False)
if 0 < len( solution) :
# TODO plot line not implemented - which function again?
Axes3D.plot(X, Y, solution)
ax.set_zlim(0.0, 1.01)
ax.zaxis.set_major_locator(LinearLocator(10))
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
fig.colorbar(surf, shrink=0.5, aspect=5)
plt.show()
def draw_trace3d(data, ax=None, person=None, color='red'):
if person is None:
person = [0]
if ax is None:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
Axes3D.view_init(ax, 45, 135)
times = np.array([i for i in range(2000)])
for i in range(len(person)):
draw_data = np.zeros((2000, 2), dtype=np.uint32)
draw_data[:, 0] = data.data[person[i]][0][0:2000, 0]
draw_data[:, 1] = data.data[person[i]][0][0:2000, 1]
Axes3D.plot(ax,
xs=draw_data[:, 0],
ys=draw_data[:, 1],
zs=times,
color=color)
return ax
def drawcylinder( ax ) :
npts = 2048
xa = numpy.array( [-1, 1] )
ya = numpy.array( [0.,0.] )
za = numpy.array( [40.,40.] )
q = Axes3D.plot(ax, xa, za, zs=ya, c='gray' )
q = Axes3D.plot(ax, ya, za, zs=xa, c='gray' )
za = numpy.array( [-40.,-40.] )
q = Axes3D.plot(ax, xa, za, zs=ya, c='gray' )
q = Axes3D.plot(ax, ya, za, zs=xa, c='gray' )
xa = numpy.array( [0.,0.] )
za = numpy.array( [0.,0.] )
ya = numpy.array( [-40.,40.] )
q = Axes3D.plot(ax, xa, ya, zs=za, c='gray', linestyle='--' )
x = numpy.zeros( npts )
y = -30.*numpy.ones( npts )
z = numpy.zeros( npts )
xx = numpy.zeros( npts )
yy = numpy.zeros( npts )
zz = numpy.zeros( npts )
n = 0
for phi in numpy.arange( 0., 2*math.pi, 2*math.pi/npts ) :
x[n] = math.cos(phi)
z[n] = math.sin(phi)
n = n+1
#q = Axes3D.plot(ax, x, y, zs=z, c='gray' )
y = 30.* numpy.ones( npts )
#q = Axes3D.plot(ax, x, y, zs=z, c='gray' )
n = 0
for phi in numpy.arange( -30., 30.0, (60./npts) ) :
yy[n] = phi
amp = math.cos(phi)
zz[n] = amp * math.cos( .007 * (30.-phi) )
xx[n] = -1.* amp * math.sin( .007 * (30.-phi) )
#zz[n] = math.cos(phi)
n = n+1
q = Axes3D.plot(ax, xx, yy, zs=zz, c='black' )
xxx = numpy.zeros( 2 )
yyy = 40.*numpy.ones( 2 )
zzz = numpy.zeros( 2 )
zzz[0] = -1.
zzz[1] = 1.
q = Axes3D.plot(ax, xxx, yyy, zs=zzz, c='black' )
xxxx = numpy.zeros( 2 )
yyyy = -40.*numpy.ones( 2 )
zzzz = numpy.zeros( 2 )
xxxx[0] = -1.* math.sin( .007 * (60.) )
zzzz[0] = math.cos( .007 * (60.) )
xxxx[1] = 1.* math.sin( .007 * (60.) )
zzzz[1] = -1.* math.cos( .007 * (60.) )
q = Axes3D.plot(ax, xxxx, yyyy, zs=zzzz, c='black' )
def addcurve( ax, path, endpoints, color ) : px = path[ :,0 ] py = path[ :,1 ] pz = path[ :,2 ] n = len(px) - 1 q = Axes3D.plot(ax, px, py, zs=pz, c=color, linewidth=2 ) px = endpoints[ :,0 ] py = endpoints[ :,1 ] pz = endpoints[ :,2 ] print px, py, pz q = Axes3D.scatter(ax, px,py, zs=pz, c=color, marker='o', s=60)
def plot_scatter_with_errorbars(
ax: Axes3D,
x: Vector,
y: Vector,
means: Vector,
stds: Vector,
masks: List[MaskArr],
markers: List[str],
colors: List[str],
ylabel: str,
):
for mask, marker, color in zip(masks, markers, colors):
ax.scatter(x[mask], y[mask], means[mask], marker=marker, color=color)
for x_i, y_j, mu, sigma in zip(x[mask], y[mask], means[mask], stds[mask]):
ax.plot([x_i, x_i], [y_j, y_j], [mu - sigma, mu + sigma], color=color)
ax.view_init(azim=50, elev=30)
ax.set_xlabel('$x_{{\\mathrm{{запад-восток}}}}$, м')
ax.set_ylabel('$y_{{\\mathrm{{юг-север}}}}$, м')
ax.set_zlabel(ylabel)
def Plot3D(x, y, q):
#---------------REVISE THIS---------------
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
surf = ax.scatter(x[0],
x[1],
y,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
# Customize the z axis.
ax.zaxis.set_major_locator(LinearLocator(10))
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
# Add a color bar which maps values to colors.
#fig.colorbar(surf, shrink=0.5, aspect=5)
ax.plot(x[0], x[1], x[0] * q[0] + x[1] * q[1])
plt.show()
def draw_function(d, button_text):
plt.clf()
ax = fig.add_subplot(111, projection='3d')
VecStart_x = []
VecStart_y = []
VecStart_z = []
VecEnd_x = []
VecEnd_y = []
VecEnd_z = []
count = 0
for hedge in d.hedgeList:
if (hedge.incidentFace.identifier != 'i' and hedge.next is not None):
VecStart_x.append(hedge.origin.x)
VecEnd_x.append(hedge.next.origin.x)
VecStart_y.append(hedge.origin.y)
VecEnd_y.append(hedge.next.origin.y)
VecStart_z.append(hedge.origin.z)
VecEnd_z.append(hedge.next.origin.z)
count = count + 1
for i in range(count):
ax.plot([VecStart_x[i], VecEnd_x[i]], [VecStart_y[i], VecEnd_y[i]],
'k',
zs=[VecStart_z[i], VecEnd_z[i]])
axclear = plt.axes([0.7, 0.05, 0.1, 0.075])
bclear = Button(axclear, 'Clear')
axtrian = plt.axes([0.81, 0.05, 0.15, 0.075])
btrian = Button(axtrian, 'Triangulate')
ax_button = plt.axes([0.05, 0.05, 0.25, 0.075])
button = Button(ax_button, button_text)
if button_text == 'See one-face example':
button.on_clicked(clear_function_part)
bclear.on_clicked(clear_function_full)
btrian.on_clicked(triangle_function_full)
else:
button.on_clicked(clear_function_full)
bclear.on_clicked(clear_function_part)
btrian.on_clicked(triangle_function_one)
plt.show()
Axes3D.plot()
def vectorplot(Points_From,Points_To):
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for i in range(len(Points_From)):
# ax.plot([Points_From[i][0], Points_To[i][0]], [Points_From[i][1], Points_To[i][1]],zs=[Points_From[i][2], Points_To[i][2]])
a=Arrow3D([Points_From[i][0], Points_To[i][0]], [Points_From[i][1], Points_To[i][1]],[Points_From[i][2], Points_To[i][2]],mutation_scale=2000, lw=3, arrowstyle="-|>", color="r")
ax.add_artist(a)
plt.show()
Axes3D.plot()
def ViewResults(Yestimates, y, mode='2D', x=0):
if mode == '2D':
err = (y - Yestimates)
plt.figure(1, figsize=(10,5))
plt.subplot(211)
plt.plot(y, color='orange', label='Target Function "Y"', alpha=0.7)
plt.plot(Yestimates, color='green', label='Function Approximation "Ysim"', linewidth=1.0)
plt.legend()
plt.subplot(212)
plt.plot(err, color='red', label='Relative Error')
plt.legend()
plt.show()
if mode == '3D':
plt.figure()
ax = plt.axes(projection='3d')
Axes3D.plot(ax,xs=x[:,0], ys=x[:,1], zs=y.flatten(), color='orange', label='Target Function "Y"', alpha=0.7)
Axes3D.plot(ax,xs=x[:,0], ys=x[:,1], zs=Yestimates.flatten(), color='green', label='Function Approximation "Ysim"', linewidth=1.0)
plt.legend()
plt.show()
return
def visualize(Z, solution=[]):
Y = np.arange(0, YSIZE, 1)
X = np.arange(0, XSIZE, 1)
fig = plt.figure()
ax = fig.gca(projection='3d')
X, Y = np.meshgrid(X, Y)
surf = ax.plot_surface(X,
Y,
Z,
rstride=1,
cstride=1,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
if 0 < len(solution):
# TODO plot line not implemented - which function again?
Axes3D.plot(X, Y, solution)
ax.set_zlim(0.0, 1.01)
ax.zaxis.set_major_locator(LinearLocator(10))
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
fig.colorbar(surf, shrink=0.5, aspect=5)
plt.show()
def __init__(self, ax: Axes3D, pose_data: SE3, style='-'):
if not isinstance(ax, Axes3D):
raise TypeError("The axes must be of 3D type.")
if not isinstance(pose_data, (SIM3, SE3, SO3)):
raise TypeError(
"The pose_data must be of one of the types: SIM3, SE3, SO3.")
self._pose_data = pose_data
Q, t = self._pose_to_Qt(pose_data)
colors = ['r', 'g', 'b']
colors = [c + style for c in colors]
self._frame_lines = []
for a in range(3):
temp, = ax.plot([t[0, 0], t[0, 0] + Q[0, a]],
[t[1, 0], t[1, 0] + Q[1, a]],
[t[2, 0], t[2, 0] + Q[2, a]], colors[a])
self._frame_lines.append(temp)
self._frame_center, = ax.plot(t[0, 0], t[1, 0], t[2, 0], 'ko')
if isinstance(pose_data, SO3):
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
def plotCamPos(mat_R, mat_t, h_rot, veri_rot, fn):
# Set marker position
markerP = [[0, 0, 1], [1, 1, 0], [1, -1, 0], [-1, -1, 0], [-1, 1, 0]]
# Set figure, draw marker
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
square_marker = markerP
square_marker.append(markerP[1])
square_marker = np.transpose(square_marker[1:6])
Axes3D.plot(ax, square_marker[0].flatten(), square_marker[1].flatten(),
square_marker[2].flatten(), 'r')
# For each figure, plot the camera posture
for i in range(0, len(mat_R)):
nowR = mat_R[i]
nowt = mat_t[i]
RTt = np.dot(np.transpose(nowR), nowt).transpose()
nowCamP = []
for point in markerP:
pointCam = np.dot(np.transpose(nowR), point) - RTt
nowCamP.append(pointCam)
square_cam = nowCamP
square_cam.append(nowCamP[1])
square_cam = np.transpose(square_cam[1:6])
line1_cam = np.transpose([nowCamP[1], nowCamP[0], nowCamP[2]])
line2_cam = np.transpose([nowCamP[3], nowCamP[0], nowCamP[4]])
# Plot
Axes3D.plot(ax, square_cam[0].flatten(), square_cam[1].flatten(),
square_cam[2].flatten(), 'k')
Axes3D.plot(ax, line1_cam[0].flatten(), line1_cam[1].flatten(),
line1_cam[2].flatten(), 'k')
Axes3D.plot(ax, line2_cam[0].flatten(), line2_cam[1].flatten(),
line2_cam[2].flatten(), 'k')
textP = nowCamP[0].flatten()
Axes3D.text(ax,
x=textP[0, 0],
y=textP[0, 1],
z=textP[0, 2],
s=str(i),
zdir=None)
# Change view angle
ax.view_init(veri_rot, h_rot)
plt.savefig(fn)
def check_tree():
m, x, cx, y, cy, z, cz = np.loadtxt(inname, unpack=True)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
maxsize = 30
plt3d.scatter(ax,
x,
y,
z,
c='r',
s=maxsize * m / np.max(m),
label="Particles")
w, x, cx, y, cy, z, cz = np.loadtxt(outname, unpack=True)
maxsize = 20
# plt3d.scatter(ax, x, y, z, 'x', c='g', s=maxsize/4, label="Centers")
# plt3d.scatter(ax, cz, cy, cz, c='hotpink', s=2*w/np.max(w), label="COMs")
# for width, j, k, i in zip(w, x, y, z):
for width, i, j, k in zip(w, x, y, z):
# plt3d.scatter(ax, [i], [j], [k], c='r')
for top in range(2):
for side in range(2):
plt3d.plot(ax, [i - width / 2, i + width / 2], [
j - width / 2 + side * width, j - width / 2 + side * width
], [k - width / 2 + top * width, k - width / 2 + top * width],
c='k',
linewidth=.2)
plt3d.plot(
ax, [
i - width / 2 + side * width,
i - width / 2 + side * width
], [j - width / 2, j + width / 2],
[k - width / 2 + top * width, k - width / 2 + top * width],
c='k',
linewidth=.2)
plt3d.plot(ax, [
i - width / 2 + top * width, i - width / 2 + top * width
], [
j - width / 2 + side * width, j - width / 2 + side * width
], [k - width / 2, k + width / 2],
c='k',
linewidth=.2)
plt.legend()
plt.xlabel("X")
plt.ylabel("Y")
ax.set_zlabel("Z")
plt.show()
#%pylab inline
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D as ax
x = np.arange(0, 10, 0.1)
y = np.sin(x)
fig, ax = plt.subplots(facecolor='w', edgecolor='k')
ax.plot(x, y, marker="o", color="r", linestyle='None')
ax.grid(True)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.grid(True)
ax.legend(["y = x**2"])
plt.title('Puntos')
plt.show()
fig.savefig("grafica.png")
stuff = end_effector(dh_mat)
A06 = stuff[0]
end_of_links = stuff[1]
X = np.array([ np.array(val[0])[0][0] for val in end_of_links])
Y = np.array([ np.array(val[1])[0][0] for val in end_of_links])
Z = np.array([ np.array(val[2])[0][0] for val in end_of_links])
print "The position of the End Effector is"
print A06[:,3][:3]
print "The orientation of the End Effector is"
print A06[:3,:3]
# make graph
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(X,Y,Z)
plt.show()
Axes3D.plot()
ax.savefig('plot1.png')
'''
Answer
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
The position of the End Effector is
[[ 0.39269908]
[ 1.09980586]
[ 0.55536037]]
The orientation of the End Effector is
[[ -5.00000000e-01 -5.00000000e-01 7.07106781e-01]
[ 5.00000000e-01 5.00000000e-01 7.07106781e-01]
[ -7.07106781e-01 7.07106781e-01 2.22044605e-16]]
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
def makeaxes( ax ) : x = numpy.array( [-1, 1] ) y = numpy.array( [0,0] ) q = Axes3D.plot(ax, x, y, zs=y, c='black' ) q = Axes3D.plot(ax, y, x, zs=y, c='black' ) q = Axes3D.plot(ax, y, y, zs=x, c='black' )
def brownian():
# CALCULATE THE BROWNIAN MOTION OF A PARTICLE IN 3D
# Number of particles
N =1
# Initial value
x=0
y=0
z=0
# Total time
T =10.0
# Steps
steps =10000.0
# Time step
dt =T/steps
# Variance: The Weiner Process states that the variance is t.
variance =dt
# Create a list of all of the times
time =np.arange(0,T,dt)
# Create random variables from a gaussian distribution in a list
# xGaussian and yGaussian are lists containing N lists of some number(steps) of random variables
# Together they are the change in the position of the particle (the delta-x vector)
xGaussian =[]
yGaussian =[]
zGaussian =[]
for particle in range(0,N):
xGaussian.append(norm.rvs(loc =0,size =steps, scale = variance))
yGaussian.append(norm.rvs(loc =0,size =steps, scale = variance))
zGaussian.append(norm.rvs(loc =0,size =steps, scale = variance))
# allxPaths contains the xPaths of the all N particles. xPath contains the path of a particle in the x direction.
allxPaths, allyPaths, allzPaths =[], [], []
xPath, yPath, zPath=[0], [0], [0]
for particle in range(0,N):
for t in range(0,int(steps)):
x += xGaussian[particle][t]
y += yGaussian[particle][t]
z += zGaussian[particle][t]
xPath.append(x)
yPath.append(y)
zPath.append(z)
allxPaths.append(xPath)
allyPaths.append(yPath)
allzPaths.append(zPath)
#reset the path, so every path is unique
x, y, z =0, 0, 0
xPath, yPath, zPath =[0], [0], [0]
# MAKE A SCATTER PLOT FOR THE PATHS OF THE PARTICLES
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for particle in range(0,N):
ax.plot(allxPaths[particle],allyPaths[particle],allzPaths[particle], alpha =.3) # plots the paths of the particle
ax.scatter(0,0,0, color ='green', marker ='H',s =50, label ='start') # labels the starting point
ax.scatter(allxPaths[-1][-1],allyPaths[-1][-1],allzPaths[-1][-1], color ='red', marker ='H',s =50, label ='end')
ax.text2D(0.05, 0.95, "3D Brownian Motion", transform=ax.transAxes) # Makes a 2D title
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
plt.show()
plt.legend(numpoints =1)
Axes3D.plot()
return 0
def plotSimulationResults(self):
try:
if 'self.ax' not in locals(
): # create self.ax becuase it throws an error when in the __init__() function, b/c of threads?
# self.ax = self.fig.add_subplot(111, projection='3d')
self.ax = self.fig.gca(projection='3d')
ts = self.sim_results.ts
ts_a = np.array(
self.sim_results.t_now[2:-1]) - self.sim_results.t_now[2]
# Switch to right figure
plt.figure(1)
self.ax.clear()
# Plot reference trajectory vs time
Axes3D.plot(self.ax,
self.sim_results.sx,
self.sim_results.sy,
ts,
c='r',
marker='o')
plt.ylabel('Position/Velocity y')
plt.xlabel('Position/Velocity x')
self.ax.set_zlabel('time')
# Plot actual trajectory vs time
Axes3D.plot(self.ax,
self.sim_results.sx_a[2:-1],
self.sim_results.sy_a[2:-1],
ts_a,
c='b',
marker='.')
plt.figure(2)
plt.clf()
plt.subplot(3, 1, 1)
plt.plot(ts, self.sim_results.vx, 'r-')
# plt.ylabel('acceleration')
plt.subplot(3, 1, 1)
plt.plot(ts_a, self.sim_results.vx_a[2:-1], 'b-')
plt.ylabel('vx error')
# Get v_mag and v_mag_a
v_mag = np.sqrt(
np.multiply(self.sim_results.vx, self.sim_results.vx) +
np.multiply(self.sim_results.vy, self.sim_results.vy))
v_mag_a = np.sqrt(
np.multiply(self.sim_results.vx_a, self.sim_results.vx_a) +
np.multiply(self.sim_results.vy_a, self.sim_results.vy_a))
plt.subplot(3, 1, 3)
plt.plot(ts, v_mag, 'b-')
plt.ylabel('vmag')
plt.subplot(3, 1, 3)
plt.plot(ts_a, v_mag_a[2:-1], 'g-')
plt.ylabel('vmag')
plt.ion()
plt.show()
plt.pause(0.001)
except BaseException as e:
self.PrintException()
def approx(x0, y0, z0):
while True:
x = phi0(x0, y0, z0)
y = phi1(x0, y0, z0)
if (norm(x - x0, y - y0) < eps):
return [x, y]
x0, y0 = x, y
step = 0.1
Z = arange(0, 1 + step, step)
X, Y = [], []
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
c = 0.3
def X0(z):
return sqrt((1 - sqr(z) / 4) / (1 + 9 / sqr(c)))
def Y0(z):
return sqrt((1 - sqr(z) / 9) / (1 + 4 / sqr(c)))
for zi in Z:
xi, yi = approx(X0(zi), Y0(zi), zi)
X.append(xi)
Y.append(yi)
print xi, yi, zi, F0(xi, yi, zi), F1(xi, yi, zi)
ax.plot(X, Y, Z)
plt.show()
Axes3D.plot()
size=(observations,
1)) #1000 to 1 matrice containing random numbers between -10/10
zs = np.random.uniform(-10, 10, (observations, 1))
inputs = np.column_stack((xs, zs))
print(inputs.shape)
#CREATE TARGETS
noise = np.random.uniform(-1, 1, (observations, 1))
targets = 2 * xs - 3 * zs + 5 + noise
print(targets.shape)
#PLOT TRAINING DATA
targets = targets.reshape(observations, )
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(xs, zs, targets)
ax.set_xlabel('xs')
ax.set_ylabel('zs')
ax.set_zlabel('Targets')
ax.view_init(azim=100)
plt.show()
targets = targets.reshape(observations, 1)
#INITIALIZE VARIABLES
init_range = 0.1
weights = np.random.uniform(-init_range, init_range,
size=(2, 1)) #random weights
biases = np.random.uniform(-init_range, init_range, size=1) #random biases
print(weights)
print(biases)
def plot_data(self):
# print('u', acceleration.value)
# print('tf', self.tf.value)
# print('tf', self.tf.value[0])
print('sx', self.sx.value)
print('sy', self.sy.value)
print('a', self.a.value)
ts = self.d.time * self.tf.value[0]
# plot results
fig = plt.figure(2)
ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, self.sx.value, self.sy.value, ts, c='r', marker='o')
plt.ylim(-5120, 5120)
plt.xlim(-4096, 4096)
plt.ylabel('Position y')
plt.xlabel('Position x')
ax.set_zlabel('time')
fig = plt.figure(3)
ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, self.vx.value, self.vy.value, ts, c='r', marker='o')
plt.ylim(-2500, 2500)
plt.xlim(-2500, 2500)
plt.ylabel('velocity y')
plt.xlabel('Velocity x')
ax.set_zlabel('time')
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(ts, self.a, 'r-')
plt.ylabel('acceleration')
plt.subplot(3, 1, 2)
plt.plot(ts, np.multiply(self.yaw, 1 / math.pi), 'r-')
plt.ylabel('turning input')
plt.subplot(3, 1, 3)
plt.plot(ts, self.v_mag, 'b-')
plt.ylabel('vmag')
# plt.figure(1)
#
# plt.subplot(7,1,1)
# plt.plot(ts,self.sz.value,'r-',linewidth=2)
# plt.ylabel('Position z')
# plt.legend(['sz (Position)'])
#
# plt.subplot(7,1,2)
# plt.plot(ts,self.vz.value,'b-',linewidth=2)
# plt.ylabel('Velocity z')
# plt.legend(['vz (Velocity)'])
#
# # plt.subplot(4,1,3)
# # plt.plot(ts,mass.value,'k-',linewidth=2)
# # plt.ylabel('Mass')
# # plt.legend(['m (Mass)'])
#
# plt.subplot(7,1,3)
# plt.plot(ts,self.u_thrust.value,'g-',linewidth=2)
# plt.ylabel('Thrust')
# plt.legend(['u (Thrust)'])
#
# plt.subplot(7,1,4)
# plt.plot(ts,self.sx.value,'r-',linewidth=2)
# plt.ylabel('Position x')
# plt.legend(['sx (Position)'])
#
# plt.subplot(7,1,5)
# plt.plot(ts,self.vx.value,'b-',linewidth=2)
# plt.ylabel('Velocity x')
# plt.legend(['vx (Velocity)'])
#
# # plt.subplot(4,1,3)
# # plt.plot(ts,mass.value,'k-',linewidth=2)
# # plt.ylabel('Mass')
# # plt.legend(['m (Mass)'])
#
# plt.subplot(7,1,6)
# plt.plot(ts,self.u_pitch.value,'g-',linewidth=2)
# plt.ylabel('Torque')
# plt.legend(['u (Torque)'])
#
# plt.subplot(7,1,7)
# plt.plot(ts,self.pitch.value,'g-',linewidth=2)
# plt.ylabel('Theta')
# plt.legend(['p (Theta)'])
#
# plt.xlabel('Time')
# plt.figure(2)
#
# plt.subplot(2,1,1)
# plt.plot(self.m.time,m.t_sx,'r-',linewidth=2)
# plt.ylabel('traj pos x')
# plt.legend(['sz (Position)'])
#
# plt.subplot(2,1,2)
# plt.plot(self.m.time,m.t_sz,'b-',linewidth=2)
# plt.ylabel('traj pos z')
# plt.legend(['vz (Velocity)'])
# #export csv
#
# f = open('optimization_data.csv', 'w', newline = "")
# writer = csv.writer(f)
# writer.writerow(['time', 'sx', 'sz', 'vx', 'vz', 'u thrust', 'theta', 'omega_pitch', 'u pitch']) # , 'vx', 'vy', 'vz', 'ax', 'ay', 'az', 'quaternion', 'boost', 'roll', 'pitch', 'yaw'])
# for i in range(len(self.m.time)):
# row = [self.m.time[i], self.sx.value[i], self.sz.value[i], self.vx.value[i], self.vz.value[i], self.u_thrust.value[i], self.pitch.value[i],
# self.omega_pitch.value[i], self.u_pitch.value[i]]
# writer.writerow(row)
# print('wrote row', row)
plt.show()
poly_train = np.append(poly_train, poly.fit_transform(train_data[:, [1]]), axis=1);
poly_test = np.append(poly_test, poly.fit_transform(test_data[:, [1]]), axis=1);
# polynomial regression
poly_regr = linear_model.LinearRegression();
poly_regr.fit(poly_train, train_data[:, [2]]);
predicted_test_data = poly_regr.predict(poly_test);
print("Mean Square Error :", mean_squared_error(test_data[:, [2]], predicted_test_data));
print("R2 Score :", poly_regr.score(poly_test, test_data[:, [2]]));
# plotting
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.scatter(train_data[:, 0], train_data[:, 1], train_data[:, 2]);
plot_data = []
for i in range(0, 100):
plot_data.append([i]);
poly_plot_data = poly.fit_transform(plot_data)
poly_plot_data = np.append(poly_plot_data, poly_plot_data, axis=1);
predicted_plot_data = poly_regr.predict(poly_plot_data);
for i in range(0,100):
ax.plot(plot_data, plot_data, predicted_plot_data[:,0], c='r',linewidth=2)
plt.show();
square_r.append(show_points_3D_r[1])
square_l = np.transpose(square_l[1:6])
square_r = np.transpose(square_r[1:6])
line1_l = np.transpose(
[show_points_3D_l[1], show_points_3D_l[0], show_points_3D_l[2]])
line2_l = np.transpose(
[show_points_3D_l[3], show_points_3D_l[0], show_points_3D_l[4]])
line1_r = np.transpose(
[show_points_3D_r[1], show_points_3D_r[0], show_points_3D_r[2]])
line2_r = np.transpose(
[show_points_3D_r[3], show_points_3D_r[0], show_points_3D_r[4]])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
Axes3D.scatter(ax, points4D[0], points4D[1], points4D[2])
Axes3D.plot(ax, square_l[0].flatten(), square_l[1].flatten(),
square_l[2].flatten(), 'C1')
Axes3D.plot(ax, line1_l[0].flatten(), line1_l[1].flatten(),
line1_l[2].flatten(), 'C1')
Axes3D.plot(ax, line2_l[0].flatten(), line2_l[1].flatten(),
line2_l[2].flatten(), 'C1')
Axes3D.plot(ax, square_r[0].flatten(), square_r[1].flatten(),
square_r[2].flatten(), 'C2')
Axes3D.plot(ax, line1_r[0].flatten(), line1_r[1].flatten(),
line1_r[2].flatten(), 'C2')
Axes3D.plot(ax, line2_r[0].flatten(), line2_r[1].flatten(),
line2_r[2].flatten(), 'C2')
plt.show()
plt.savefig("../../output/task_7/Projection_before_rectify.png")
def dynamics():
# Number of particles
N=2
# Total time
T =10.0
# Steps
steps =1000.0
# Time step
dt =T/steps
# Variance: The Weiner Process states that the variance is t.
variance =1
# MAKE THE FORCE VECTORS
# allForces will be a list containing 3Nx1 force vectors
# The 3Nx1 vectors are the 3 dimensional force vectors for each N particles.
allForces =[]
for timestep in range(0, int(steps)):
allForces.append(norm.rvs(loc =0,size =3*N, scale = variance))
# MAKE THE POSITION VECTORS
# Make the Position vector for all N particles
position =[[1,0,0,2,0,0]] # MAKE NXN POSITION VECTOR
for timestep in range(0,int(steps)):
# Make the mobility matrix of p1
mobility = mobilityMatrix(position[timestep], N)
# Append the next position to the path
position.append(position[timestep] + MF(mobility,allForces[timestep]))
# MAKE LISTS FOR THE X PATHS, Y PATHS, AND Z PATHS SO WE CAN PLOT THE PARTICLES
allxPaths,allyPaths,allzPaths =[[],[]],[[],[]],[[],[]] # A list for each particles # MAKE EACH LIST N LENGTH
for particle in range(0,2):
for vector in position:
allxPaths[particle].append(vector[3*particle])
allyPaths[particle].append(vector[3*particle+1])
allzPaths[particle].append(vector[3*particle+2])
# MAKE A SCATTER PLOT FOR THE PATHS OF THE PARTICLES
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for particle in range(0,N):
ax.plot(allxPaths[particle],allyPaths[particle],allzPaths[particle], alpha =.3,) # plots the paths of the particle
ax.scatter(-1,0,0, color ='green', marker ='H',s =50, label ='start') # labels the starting points
ax.scatter(1,0,0, color ='blue', marker ='H',s =50, label ='start')
ax.scatter(allxPaths[0][-1],allyPaths[0][-1],allzPaths[0][-1], color ='blue', marker ='^',s =50, label ='end') #label the end points
ax.scatter(allxPaths[1][-1],allyPaths[1][-1],allzPaths[1][-1], color ='green', marker ='^',s =50, label ='end') #label the end points
ax.text2D(0.05, 0.95, "3D Brownian Motion of two particles", transform=ax.transAxes) # Makes a 2D title
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
plt.show()
plt.legend(numpoints =1)
Axes3D.plot()
return 0
def main():
'''Program to compute posture of mannequin and plot results'''
# parse arguments
parser = argparse.ArgumentParser()
parser.add_argument('--filename', '-f', type=str, default='posture',
help='Filename with posture click points')
parser.add_argument('--savename', '-s', type=str, default='posture.yaml',
help='Filename for saving posture estimation results')
parser.add_argument('--calibname', '-c', type=str, default='calibration.yaml',
help='Filename with kinect calibration values')
args = parser.parse_args()
# parsing the arguments
postureFile = args.filename
resultsFile = args.savename
calibrationFile = args.calibname
# initialize ros node
rospy.init_node('compute_posture')
# load transformation matrix from yaml file
with open(calibrationFile, 'r') as f:
params = yaml.load(f)
# get rotation quaternion and translation vector
trans = params['trans']
rot = params['rot_euler']
# construct transformation matrix
transMatrix = tf.transformations.compose_matrix(translate = trans, angles = rot)
# load posture points
posturePoints = np.loadtxt(postureFile, delimiter = ',')
posturePoints = np.hstack((posturePoints, np.ones((posturePoints.shape[0], 1))))
# apply transformation matrix to points
posturePoints = np.dot(transMatrix, posturePoints.T)
posturePoints = posturePoints[:-1,:].T
# compute neck and body points
neck = (posturePoints[LEFTSHOULDER,:] + posturePoints[RIGHTSHOULDER,:])/2
body = neck.copy()
body[2] = body[2]-BODYLENGTH
# append neck and body points
posturePoints = np.vstack((posturePoints,neck,body))
# compute posture centroid
postureMean = posturePoints.mean(axis=0)
# compute vectors
headNeck = posturePoints[HEAD,:] - posturePoints[NECK,:]
headNeck = headNeck/np.linalg.norm(headNeck)
neckBody = posturePoints[NECK,:] - posturePoints[BODY,:]
neckBody = neckBody/np.linalg.norm(neckBody)
leftArm = posturePoints[LEFTWRIST,:] - posturePoints[LEFTSHOULDER,:]
leftArm = leftArm/np.linalg.norm(leftArm)
rightArm = posturePoints[RIGHTWRIST,:] - posturePoints[RIGHTSHOULDER,:]
rightArm = rightArm/np.linalg.norm(rightArm)
# compute head, left arm and right arm angles
leftAngle = np.arccos(np.dot(leftArm,neckBody))*180/np.pi
headAngle = np.arccos(np.dot(headNeck,neckBody))*180/np.pi
rightAngle = np.arccos(np.dot(rightArm,neckBody))*180/np.pi
# plot mannequin posture
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# plot the data
Axes3D.scatter(ax, posturePoints[:,0], posturePoints[:,1], posturePoints[:,2],
s = 50, c = 'r')
Axes3D.plot(ax, posturePoints[[HEAD,NECK,BODY],0], posturePoints[[HEAD,NECK,BODY],1],
posturePoints[[HEAD,NECK,BODY],2], linewidth = 3, c = 'b')
Axes3D.plot(ax, posturePoints[[LEFTWRIST,LEFTSHOULDER,RIGHTSHOULDER,RIGHTWRIST], 0],
posturePoints[[LEFTWRIST,LEFTSHOULDER,RIGHTSHOULDER,RIGHTWRIST], 1],
posturePoints[[LEFTWRIST,LEFTSHOULDER,RIGHTSHOULDER,RIGHTWRIST], 2],
linewidth = 3, c = 'b')
# add labels and title
ax.set_xticks([])
ax.set_yticks([])
ax.set_zticks([])
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
plt.title('Mannequin Posture (%.1f,%.1f,%.1f)' % (headAngle,leftAngle,rightAngle))
plt.show()
# save results to yaml file
results = dict(
Location = postureMean.tolist(),
Angles = dict(
Head = float(headAngle),
LeftArm = float(leftAngle),
RightArm = float(rightAngle)
),
Posture = dict(
HEAD = posturePoints[HEAD,:].tolist(),
NECK = posturePoints[NECK,:].tolist(),
BODY = posturePoints[BODY,:].tolist(),
LEFTWRIST = posturePoints[LEFTWRIST,:].tolist(),
RIGHTWRIST = posturePoints[RIGHTWRIST,:].tolist(),
LEFTSHOULDER = posturePoints[LEFTSHOULDER,:].tolist(),
RIGHTSHOULDER = posturePoints[RIGHTSHOULDER,:].tolist()
)
)
# write results
with open(resultsFile, 'w') as f:
f.write(yaml.dump(results))
