def objective_3fixed(X):
ptrn = gnerate_ptrn_3fixed_1(X, n_cycles)
if f:
xfinal, rfinal, _, _ = model.predict_pose(ptrn, initial_pose, f=f)
else:
xfinal, rfinal, _, _ = model.predict_pose(ptrn, initial_pose)
xfp, yfp = rfinal
obj = -np.sqrt((sum(yfp))**2 + (sum(xfp))**2)
obj_history.append(obj)
print('step', len(obj_history), '\t', obj)
return obj
def objective_curve(X):
initial_pose = [X[:5], 90, (0, 0)]
ptrn = gnerate_ptrn(X, n_cycles)
xfinal, rfinal, _, _ = model.predict_pose(ptrn, initial_pose, f=f)
eps = xfinal[-1]
obj = -eps
obj_history.append(obj)
print('step', len(obj_history), '\t', obj)
return obj
def objective_straight(X):
initial_pose = [X, 90, (0, 0)]
ptrn = gnerate_ptrn_symmetric(X, n_cycles)
if f:
xfinal, rfinal, data_sim, _ = model.predict_pose(ptrn,
initial_pose,
stats=1, f=f)
else:
xfinal, rfinal, data_sim, _ = model.predict_pose(ptrn,
initial_pose)
_, data_fp, data_nfp, _ = data_sim
_, yfp0 = data_fp[0]
_, ynfp0 = data_nfp[0]
xfp, yfp = rfinal
obj = -(sum(yfp)-sum(yfp0+ynfp0))
obj_history.append(obj)
print 'step', len(obj_history), '\t', obj
# , '\n', yfp, '\n', yfp0+ynfp0, '\n\n'
return obj
def repr_gait(opt_ptrn, f, name=''):
filled_ptrn = model.loop_ptrn(opt_ptrn, n_cycles=2)
filled_ptrn.append(filled_ptrn[0])
init_pose = [opt_ptrn[0][0], 90, (0, 0)]
x, r, data, cst = model.predict_pose(filled_ptrn,
init_pose,
stats=1,
debug=0,
f=f)
model.plot_gait(*data, name='Plot ' + name)
model.tikz_interface(*data, name='Pics/py/' + name + '.tex')
opt_ptrn_ = [[round(ref, 2) for ref in opt_ptrn[0][0]], opt_ptrn[0][1]]
opt_ptrn_ += [[round(ref, 2) for ref in opt_ptrn[1][0]], opt_ptrn[1][1]]
init_pose = [opt_ptrn[0][0], 90, (0, 0)]
# 3FIXED
# opt_ptrn, obj_hist, opt_obj = optimize_gait_3fixed(n_cycles, init_pose,
# method, x0=x0)
# opt_ptrn = opt_ptrn[:4]
filled_ptrn = model.fill_ptrn(opt_ptrn, n_cycles=2, res=2)
prop_str = '{} | {} | {} | {} | {} | {} | {}'.format(
method, len(obj_hist), round(opt_obj, 2), n_cycles, f_len, f_ori,
f_ang)
x, r, data, cst = model.predict_pose(filled_ptrn, init_pose, stats=1, f=f)
model.plot_gait(*data, name='Plot: ' + prop_str)
plt.figure('cost: ' + prop_str)
plt.title('cost: ' + prop_str)
plt.plot(cst)
plt.figure('opt_hist: ' + prop_str)
plt.title('opt_hist: ' + prop_str)
plt.plot(obj_hist)
print('initial guess:\n', x0, '\n')
print('property string:\n', prop_str, '\n')
print('optimal pattern:\n', opt_ptrn_, '\n')
fig = plt.figure('Animation: ' + prop_str)
