gait_tex = gait_tex + '\n%%%%%%%\n' + gait.get_tikz_repr(linewidth='.7mm', dashed=0) plt.xticks(X_idx.T[0], [round(x, 2) for x in Q2]) plt.yticks(Y_idx[0], [round(x, 1) for x in Q1]) plt.ylabel('step length $q_1$ $(^\\circ)$') plt.xlabel('steering $q_2$ (1)') plt.axis('scaled') plt.ylim((Y_idx[0][0] - 30, Y_idx[0][-1] + 30)) plt.xlim((-15, 155)) plt.grid() ax = fig.gca() ax.spines['top'].set_visible(False) ax.spines['left'].set_visible(False) ax.spines['right'].set_visible(False) ax.spines['bottom'].set_visible(False) my_save.save_plt_as_tikz('Out/velocity_space/gait_' + str(weight) + '_c1_' + str(c1) + '.tex', additional_tex_code=gait_tex, scale=.7, scope='scale=.1, opacity=.8') fig.set_size_inches(10.5, 8) fig.savefig('Out/velocity_space/gait_' + str(weight) + '_c1_' + str(c1) + '.png', transparent=True, dpi=300, bbox_inches='tight')
def get_next_reference(self, act_position, act_eps, xref, act_pose=None, save_as_tikz=False, gait=False, save_png=False, show_dec=True): xref = np.r_[xref] act_pos = np.r_[act_position] dpos = xref - act_pos act_dir = np.r_[np.cos(np.radians(act_eps)), np.sin(np.radians(act_eps))] act_deps = calc_angle(dpos, act_dir) act_dist = np.linalg.norm(dpos) if act_dist < .5: pose_id = 'rest' elif not self.crawl: if len(self.graph.get_children(self.pose)) > 1: verts = [v for v, weight in self.graph.get_children(self.pose)] plot = False if all(v[-1] == 'f' for v in verts) else True plot = plot if show_dec else False if plot: figname = 'dec_' + str(self.idx) plt.figure(figname) draw_point_dir(act_pos, act_dir * 10, msize=10) draw_point_dir(act_pos, [0, 0], msize=10, label='ROBOT ($ %s $)' % tex_str(self.pose)) draw_point_dir(xref, [0, 0], msize=20, label='GOAL') act_pose.plot('gray') if save_as_tikz: plt.figure('tikz_' + figname) draw_point_arrow(act_pos, act_dir * 4, size=7, colp='orange') draw_point_dir(act_pos, [1, 0], msize=1, label='robot ($ %s $)' % tex_str(self.pose), colp='orange', size=20) # label size draw_point_dir(xref, [0, 0], msize=7) # goal def suitability(translation_, rotation, v=None, plot=True): translation_ = np.r_[translation_] # translation is configured for eps=90 translation = rotate(translation_, np.radians(act_eps - 90)) dir_ = np.r_[np.cos(np.radians(act_eps + rotation)), np.sin(np.radians(act_eps + rotation))] pos_ = act_pos + translation dist_ = np.linalg.norm(xref - pos_) deps_ = calc_angle(xref - pos_, dir_) # Label the thing if plot: plt.figure(figname) draw_point_dir(act_pos + translation * 10, dir_ * 10, label=v) draw_line(act_pos + translation * 10, xref) if save_as_tikz: plt.figure('tikz_' + figname) draw_point_arrow(act_pos + translation * 10, dir_ * 4, size=5, colp='darkorange', w=.4) draw_point_dir(act_pos + translation * 10, dir_ * 10, label='$' + tex_str(v) + '$', msize=5, colp='darkorange', size=20) # label size draw_line(act_pos + translation * 10, xref) return (dist_, deps_) deps = CandidateHandler() ddist = CandidateHandler() for child in self.graph.get_children(self.pose): v, (translation, rotation) = child dist_, deps_ = suitability(translation, rotation, v, plot) deps[v] = round(deps_, 2) ddist[v] = round(dist_, 2) if abs(act_deps) > 70: # ganz falsche Richtung _, pose_id = deps.minimum() else: max_deps, _ = deps.maximum() max_ddist, _ = ddist.maximum() w = .5 dec = CandidateHandler() for key in deps: for dist, eps in zip(ddist[key], deps[key]): dec[key] = w * dist / max_ddist + ( 1 - w) * abs(eps) / max_deps min_dec, pose_id = dec.minimum() if plot: plt.figure(figname) draw_point_dir(act_pos - act_dir * 20, [0, 0], msize=1, label='choose ($ %s $)' % pose_id) if save_as_tikz: plt.figure('tikz_' + figname) # draw_point_dir(act_pos-act_dir*20, [0, 0], msize=1, # label='choose ($%s$)' % tex_str(pose_id), # colp='gray', # size=15) # label size if gait: gait.plot_markers(1, figname='tikz_' + figname) plt.axis('off') geckostr = act_pose.get_tikz_repr('gray!50', dashed=0, R=.7, linewidth='.3mm') save.save_plt_as_tikz('Out/pathplanner/' + figname + '.tex', geckostr, scope='scale=.1') plt.close('tikz_' + figname) elif save_png: plt.savefig('Out/pathplanner/' + figname + '.png', transparent=True, dpi=300) plt.show() else: # only 1 child pose_id, _ = self.graph.get_children(self.pose)[0] if save_as_tikz: figname = 'dec_' + str(self.idx) plt.figure(figname) draw_point_dir(act_pos - act_dir * 20, [0, 0], msize=1, label='choose ($ %s $)' % pose_id) draw_point_dir(act_pos, act_dir * 10, msize=10) draw_point_dir(act_pos, [0, 0], msize=10, label='ROBOT ($ %s $)' % tex_str(self.pose)) draw_point_dir(xref, [0, 0], msize=20, label='GOAL') act_pose.plot('gray') if save_as_tikz: plt.figure('tikz_' + figname) draw_point_arrow(act_pos, act_dir * 4, size=7, colp='orange') draw_point_dir(act_pos, [1, 0], msize=1, label='robot ($ %s $)' % tex_str(self.pose), colp='orange', size=20) # label size draw_point_dir(xref, [0, 0], msize=7) # goal # draw_point_dir(act_pos-act_dir*20, [0, 0], msize=1, # label='choose ($%s$)' % tex_str(pose_id), # colp='gray', # size=15) # label size if gait: gait.plot_markers(1, figname='tikz_' + figname) plt.axis('off') geckostr = act_pose.get_tikz_repr('gray!50', dashed=0, R=.7, linewidth='.3mm') save.save_plt_as_tikz('Out/pathplanner/' + figname + '.tex', geckostr, scope='scale=.1') plt.close('tikz_' + figname) elif save_png: plt.savefig('Out/pathplanner/' + figname + '.png', transparent=True, dpi=300) plt.show() if pose_id == 'C1_0': self.crawl = True alpha, feet, process_time = self.__get_ref(self.pose) self.crawl_ptrn = ros.rotate_on_spot(act_deps, alpha, feet, t_fix, t_dfx, t_move) self.crawl_idx = 0 if self.crawl: ref = self.crawl_ptrn[self.crawl_idx] alpha, feet, process_time = ref self.crawl_idx += 1 pose_id = 'crawling' if self.crawl_idx + 1 > len(self.crawl_ptrn): self.crawl = False self.crawl_idx = None pose_id = 'C1_2:dfx' self.ref[pose_id] = self.crawl_ptrn[-1] self.crawl_ptrn = None else: alpha, feet, process_time = self.__get_ref(pose_id) self.pose = pose_id self.idx += 1 return alpha, feet, process_time, pose_id
for idx, xxxx in enumerate(xticks): if idx % 2 == 1: xticks[idx] = '' plt.xticks(X_idx.T[0], xticks) plt.yticks(Y_idx[0], [int(x) for x in X1]) plt.axis('scaled') plt.ylim((Y_idx[0][0] - 20, Y_idx[0][-1] + 20)) plt.xlim((X_idx[0][0] - 15, X_idx[-1][0] + 15)) fname = '../../Out/analytic_model6/FitDXDY_{}_order_{}_round_{}.tex'.format( key, order, roundon) my_save.save_plt_as_tikz(fname, extra_axis_parameters={ 'anchor=origin', 'disabledatascaling', 'x=.05cm', 'y=.05cm' }) fig = plt.gcf() fig.set_size_inches(10.5, 8.5) fig.savefig( '../../Out/analytic_model6/FitDXDY_{}_order_{}_round_{}.png'. format(key, order, roundon), dpi=300, trasperent=True, bbox_inches='tight') # %% plt.show()
d = optplanner.calc_d(xbar, optplanner.dx(x1, x2), optplanner.deps(x1, x2), n) if d < dmin['val']: dmin = {'val': d, 'x1': x1, 'x2': x2} D[idx2][idx1] = d x1_opt, x2_opt = dmin['x1'], dmin['x2'] plot_contour(X1, X2, D, lines=8) plt.plot(x2_opt, x1_opt, marker='o', color='purple', markersize=12, markeredgecolor='k') kwargs = {'extra_axis_parameters': {'font=\\small'}} save.save_plt_as_tikz('Out/opt_pathplanner/dist_n_{}.tex'.format(n), **kwargs) # %% Gecko x1, x2 = x1_opt, x2_opt # optimal gait feet0 = [1, 0, 0, 1] # feet states alpha0 = [90, 0, -90, 90, 0] weight = [89, 10, 5.9] # [f_l, f_o, f_a] len_leg, len_tor = calibration.get_len('vS11') # alpha = [90, 0, -90, 90, 0] x, (mx, my), f = model.set_initial_pose(alpha0, eps, p1,
predicted_pose = model.predict_next_pose([alpha, feet], act_pose.x, (x, y)) predicted_pose = pf.GeckoBotPose(*predicted_pose) gait.append_pose(predicted_pose) i += 1 print('pose', i, 'dist: \t', round(calc_dist(gait.poses[-1], xref), 2), '\n') if i > 10: break # gait.plot_gait() gait.plot_markers(1) # gait.plot_com() # %% Plots plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12) plt.axis('off') gait_str = gait.get_tikz_repr() save.save_plt_as_tikz( 'Out/opt_pathplanner/{}/gait_sim_{}.tex'.format(key, replay), gait_str) fig = plt.gcf() fig.clear() # %% Animation # gait.animate()
plt.xlabel('minimum distance $\epsilon$ (cm)') plt.ylabel('sucessful runs (%)') # labels: ax = plt.gca() for idx, mindist in enumerate(MINDIST): percentage = y[idx] ax.annotate( '{}'.format(round(percentage)), xy=(mindist, percentage), xytext=(0, -3 if percentage > 30 else 3), # 3 points vertical offset textcoords="offset points", fontsize=16, ha='center', va='top' if percentage > 30 else 'bottom') ax.spines['top'].set_visible(False) ax.spines['left'].set_visible(False) ax.spines['right'].set_visible(False) ax.spines['bottom'].set_visible(False) plt.yticks([]) MINDIST_ = MINDIST[:] MINDIST_[-1] = 9.9999 plt.xticks(MINDIST_, [str(d) for d in MINDIST]) #plt.grid(axis='y') kwargs = {'extra_axis_parameters': {'width=11cm', 'height=4cm'}} save.save_plt_as_tikz('Out/dist_check.tex', **kwargs)
kwargs = { 'extra_axis_parameters': { 'x=.1cm', 'y=.1cm', 'anchor=origin', 'xmin=-20', 'xmax=20', 'axis line style={draw opacity=0}', # 'hide axis', 'ymin=-20, ymax=20', 'tick pos=left', } } gait_str = gait.get_tikz_repr(dashed=0, linewidth='1mm', R=.8) save.save_plt_as_tikz('Out/with_simmodel.tex', gait_str, scope='scale=.1', **kwargs) plt.show() # %% without sim Model x, marks, feet = model.set_initial_pose(alp0, eps0, p1=[0, 0], len_leg=ell[0], len_tor=ell[2], f=[0, 1, 0, 0]) feet = [1, 0, 0, 1] initial_pose = pf.GeckoBotPose(x, marks, feet)
gait_str += '\\draw[color0, line width=1mm, -latex] (0,0)--(0,10);' kwargs = { 'extra_axis_parameters': { 'x=.1cm', 'y=.1cm', 'anchor=origin', 'xmin=-25', 'xmax=110', 'axis line style={draw opacity=0}', 'ymin=-25, ymax=120', 'tick pos=left', } } save.save_plt_as_tikz('Out/opt_pathplanner/gait.tex', additional_tex_code=gait_str, scope='scale=.1, opacity=1', **kwargs) # %% plt.figure('Q1Q2') plt.plot(Q1, 'b') ax1 = plt.gca() ax2 = ax1.twinx() ax2.plot(Q2, 'r') ax2.set_ylim(-.7, .7) ax2.set_yticks([-.5, 0, .5]) ax1.set_ylim(40, 100) ax1.grid()
ax = plt.gca() ax.spines['top'].set_visible(False) ax.spines['left'].set_visible(False) ax.spines['right'].set_visible(False) ax.spines['bottom'].set_visible(False) plt.grid() kwargs = { 'extra_axis_parameters': { 'x=.1cm', 'y=.1cm', 'anchor=origin', 'xmin=-55', 'xmax=37', 'axis line style={draw opacity=0}', 'ymin=-20, ymax=105', 'tick pos=left', } } gait_str = gait.get_tikz_repr(dashed=0) save.save_plt_as_tikz('Out/pathplanner/gait.tex', gait_str, scope='scale=.1', **kwargs) # %% # gait.animate()
gait.append_pose( pf.GeckoBotPose(x, marks, f, constraint=constraint, cost=cost)) stress = gait.plot_stress() dist, deps = gait.get_travel_distance() RESULTS[method][cat][f_len][f_ang][f_ori]['stress'] = stress RESULTS[method][cat][f_len][f_ang][f_ori]['dist'] = dist RESULTS[method][cat][f_len][f_ang][f_ori]['deps'] = deps RESULTS[method][cat][f_len][f_ang][f_ori]['alp'] = alp_opt save_obj(RESULTS, 'RESULTS_curve') gait.plot_gait() gait.plot_markers([1]) plt.axis('off') plt.savefig('Out/opt_ref/'+cat+'/{}.png'.format(prop_str), transparent=False, dpi=150) # plt.show() plt.close('all') # %% SAVE AS TIKZ gait.plot_markers(1) plt.axis('off') gait_str = gait.get_tikz_repr() save.save_plt_as_tikz('Out/opt_ref/'+cat+'/{}.tex'.format(prop_str), gait_str) plt.close('GeckoBotGait')
'xtick distance={1}', 'ytick distance={1}'} xshift = 3.5 gait.plot_orientation(poses=[0],w=.05, size=5) gait.plot_orientation(poses=[-1], shift=[xshift*(len(gait.poses)-1),0], w=.05, size=5) plt.axis('off') for idx in range(len(gait.poses)): plt.annotate(str(idx), [xshift*idx+.5, -2.5], size=35) gait_str = gait.get_tikz_repr(shift=xshift, R=.15, dashed=0, reverse_col=-100, linewidth='.4mm') save.save_plt_as_tikz('Out/elemtary_patterns/{}.tex'.format(key), gait_str, additional_options=option, scale=.7, extra_axis_parameters=extra_axis_parameters) plt.close('all') gait.plot_orientation(length=1, w=.05, size=5, colp='k') gait.plot_travel_distance(w=.05, size=1, colp='blue') plt.grid() # plt.xlabel('$x$ (1)') # plt.ylabel('$y$ (1)') save.save_plt_as_tikz('Out/elemtary_patterns/{}_dist.tex'.format(key), additional_options=option, scale=.7, extra_axis_parameters=extra_axis_parameters)
act_pos = (x[1], y[1]) predicted_pose = model.predict_next_pose([alpha, feet], act_pose.x, (x, y)) predicted_pose = pf.GeckoBotPose(*predicted_pose) gait.append_pose(predicted_pose) ## %% Plots gait.plot_gait() # gait.plot_markers(1) # gait.plot_com() # plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12) plt.axis('off') gait_str = gait.get_tikz_repr(shift=2.9) save.save_plt_as_tikz('Scripts/principle_sketch.tex', gait_str) # %% Test algorithm for idx, init in enumerate([[[90, 0, -90, 90, 0], [1, 0, 0, 1]], [[0, 90, 90, 0., 90], [0, 1, 1, 0]]]): for jdx, xref in enumerate([(-2, -3), (2, -3)]): alpha, feet = init eps = 90 p1 = (0, 0) x, (mx, my), f = model.set_initial_pose(alpha, eps, p1) initial_pose = pf.GeckoBotPose(x, (mx, my), f) gait = pf.GeckoBotGait() gait.append_pose(initial_pose)
if px > maxima['maxx']: maxima['maxx'] = px index['maxx'] = idx if py < maxima['miny']: maxima['miny'] = py index['miny'] = idx if py > maxima['maxy']: maxima['maxy'] = py index['maxy'] = idx #%% # gait.plot_gait() gait.plot_markers([1]) plt.axis('off') gait_str = gait.poses[index['minx']].get_tikz_repr() gait_str = gait_str + gait.poses[index['maxx']].get_tikz_repr() gait_str = gait_str + gait.poses[index['miny']].get_tikz_repr() gait_str = gait_str + gait.poses[index['maxy']].get_tikz_repr() gait.poses[index['minx']].show_stats() gait.poses[index['maxx']].show_stats() gait.poses[index['maxy']].show_stats() save.save_plt_as_tikz('Out/workspace/test.tex', gait_str) # %% mx, my = model._calc_coords2(x, marks, f) for x, y, i in zip(mx, my, [i for i in range(6)]): plt.plot(x, y, 'ro', markersize=10) plt.text(x, y + .1, str(i))
ax = plt.gca() ax.spines['top'].set_visible(False) ax.spines['left'].set_visible(False) ax.spines['right'].set_visible(False) ax.spines['bottom'].set_visible(False) plt.grid() kwargs = {'extra_axis_parameters': {'x=.15cm', 'y=.15cm', 'anchor=origin', 'xmin=-30', 'xmax=20', 'axis line style={draw opacity=0}', # 'hide axis', 'ymin=-20, ymax=30', 'tick pos=left',}} gait_str = gait.get_tikz_repr(dashed=0, linewidth='1mm', R=.8) save.save_plt_as_tikz('Out/oscillator_demo.tex', gait_str, scope='scale=.15', **kwargs) plt.show() # %% Q plot q1 = [qi[0] for qi in Q] q2 = [qi[1] for qi in Q] plt.figure('Q1Q2') ax1 = plt.gca() ax2 = ax1.twinx() ax1.plot(q1, 'blue')
plt.yticks([-90, -45, 0, 45, 90, 135], ['-' + pih, '', '0', '', pih]) plt.xlabel('time in cycle') plt.ylabel('bending angle') plt.grid() # plt.legend(loc='center left', bbox_to_anchor=(1, .5)) plt.axis('tight') plt.xlim([0, 1]) plt.ylim([-135, 160.43]) name = 'ALPHA_{}_{}_{}.tex'.format(key, startpose, str(x2).replace('.', '_')) kwargs = {'extra_axis_parameters': {'anchor=origin'}} save_utils.save_plt_as_tikz('Out/analytic_model8/' + name, **kwargs) # # %% PLOT PHI / EPS # fig.clear() fig = plt.figure('PHI' + key + str(x2) + startpose) plt.plot(TIME, np.array(RESULT['eps'][0]) - eps0, 'green', label='eps') plt.plot(TIME, REF['eps'][0], ':', color='green', label='eps') # % plt.plot(t1,
plt.axis('scaled') plt.ylim((Y_idx[x1_idx][0][0] - 30, Y_idx[x1_idx][0][-1] + 20)) plt.xlim((X_idx[x1_idx][0][0] - 15, X_idx[x1_idx][-1][0] + 15)) # plt.axis('auto') plt.grid() ax = fig.gca() ax.spines['top'].set_visible(False) ax.spines['left'].set_visible(False) ax.spines['right'].set_visible(False) ax.spines['bottom'].set_visible(False) fname = 'Out/c1_analysis/c1_analysis_q1_{}_weights_{}-{}-{}.tex'.format( x1, f_l, f_o, f_a) my_save.save_plt_as_tikz(fname, additional_tex_code=gait_tex, scale=.7, scope='scale=.1, opacity=.8') fig.savefig( fname[:-3] + 'png', transparent=True, dpi=300, bbox_inches='tight', ) # %% fig.clear() # clean figure # %% save simulation results def save_obj(obj, name):
'extra_axis_parameters': { 'x=.1cm', 'y=.1cm', 'anchor=origin', # 'xmin=-55', 'xmax=37', 'axis line style={draw opacity=0}', 'hide axis', # 'ymin=-20, ymax=105', 'tick pos=left', } } gait_str = gait.get_tikz_repr(dashed=0, linewidth='1mm', R=.8) save.save_plt_as_tikz('Out/gait_' + str(mindist) + '_' + str(try_idx) + '.tex', gait_str, scope='scale=.1', **kwargs) plt.show() fig = plt.gcf() fig.clear() # %% def autolabel(rectdic): """Attach a text label above each bar in *rects*, displaying its height.""" for mode in rectdic: for key in rectdic[mode]: for rect in rectdic[mode][key]: height = rect.get_height()