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