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))