def objective(x):
global idx
global lastDeps
x2, x3 = x
x = [x1, x2, x3, x3, x2, x6]
x_ = [-x1, x2, x3, x3, x2, x6]
f1 = [0, 1, 1, 0]
f2 = [1, 0, 0, 1]
if x[5] < 0:
ref2 = [
[alpha(x_, f2), f2],
# [alpha(x_, f2), f1],
[alpha(x, f1), f1],
# [alpha(x, f1), f2]
]
else:
ref2 = [
[alpha(x, f1), f1],
# [alpha(x, f1), f2],
[alpha(x_, f2), f2],
# [alpha(x_, f2), f1]
]
ref2 = ref2 * n_cyc + [ref2[0]]
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0], eps, (0, 0)))
gait = pf.predict_gait(ref2, init_pose, weight=weight)
(dxx, dyy), deps = gait.get_travel_distance()
stress = [pose.cost for pose in gait.poses]
cum_stress = sum(stress[3:])
cum_stress = sum(stress)
# gait.plot_gait(str(idx).zfill(3))
# gait.plot_stress(str(idx).zfill(3))
# plt.show()
print('x:', [round(xx, 2) for xx in x], ':', round(deps),
round(cum_stress))
idx += 1
lastDeps = deps
return cum_stress
f2 = [1, 0, 0, 1]
if 1:
ref2 = [[alpha(-x1, x2, f2, c1), f2],
[alpha(x1, x2, f1, c1), f1]]
# else:
# ref2 = [[alpha(x1, x2, f1, c1), f1],
# [alpha(-x1, x2, f2, c1), f2]
# ]
ref2 += [ref2[0]]
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0],
eps, (x2_idx * dx, c1_idx * dy),
len_leg=len_leg,
len_tor=len_tor))
gait = pf.predict_gait(ref2, init_pose, weight, (len_leg, len_tor))
(dxx, dyy), deps = gait.get_travel_distance()
RESULT_DX[x1_idx][x2_idx][c1_idx] = dxx
RESULT_DY[x1_idx][x2_idx][c1_idx] = dyy
RESULT_DEPS[x1_idx][x2_idx][c1_idx] = deps
print('(x1, x2, c1):', round(x1, 2), round(x2, 2), round(c1, 2),
':', round(deps, 2))
Phi = gait.plot_phi()
plt.title('Phi')
cumstress = gait.plot_stress()
RESULT_STRESS[x1_idx][x2_idx][c1_idx] = cumstress
plt.title('Inner Stress')
ref2 = [[[
45 - gam / 2., 45 + gam / 2., gam + x, 45 - gam / 2.,
45 + gam / 2.
], [0, 1, 1, 0]],
[[
45 + gam / 2., 45 - gam / 2., -gam + x, 45 + gam / 2.,
45 - gam / 2.
], [1, 0, 0, 1]]] * n_cyc
# ref2 = model.flat_list(ref2)
print('(gam, x):', round(gam, 1), round(x, 1))
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0], eps, (x_idx * dx,
-gam_idx * dy)))
gait = pf.predict_gait(ref2, init_pose)
(dxx, dyy), deps = gait.get_travel_distance()
RESULT_DX[x_idx][gam_idx] = dxx
RESULT_DY[x_idx][gam_idx] = dyy
RESULT_DEPS[x_idx][gam_idx] = deps
plt.figure('GeckoBotGait')
plt.text(x_idx * dx, -gam_idx * dy - 2.5,
'{}'.format(round(deps, 1)))
gait.plot_gait()
gait.plot_orientation()
gait.plot_stress()
plt.figure('GeckoBotGait')
ref2 = [[alpha(-q1, q2, f2), f2], [alpha(q1, q2, f1), f1]]
# else:
# ref2 = [[alpha(q1, q2, f1), f1],
# [alpha(-q1, q2, f2), f2]
# ]
ref2 = ref2 * n_cyc
if and_half:
ref2 = ref2 + [ref2[0]]
# get start positions
init_pose = roboter_repr.GeckoBotPose(
*model.set_initial_pose(ref2[0][0],
eps, (q2_idx * dx, q1_idx * dy),
len_leg=len_leg,
len_tor=len_tor))
gait_ = roboter_repr.predict_gait(ref2, init_pose, weight,
(len_leg, len_tor))
alp_ = gait_.poses[-1].alp
ell_ = gait_.poses[-1].ell
init_pose = roboter_repr.GeckoBotPose(*model.set_initial_pose(
alp_, eps, (q2_idx * dx, q1_idx * dy), ell=ell_),
len_leg=len_leg,
len_tor=len_tor)
# actually simulation
gait = roboter_repr.predict_gait(ref2, init_pose, weight,
(len_leg, len_tor))
(dxx, dyy), deps = gait.get_travel_distance()
RESULT_DX[q2_idx][q1_idx] = dxx
RESULT_DY[q2_idx][q1_idx] = dyy
f2 = [1, 0, 0, 1]
# if x2 < 0:
if 1:
ref2 = [[alpha(-x1, x2, f2), f2],
[alpha(x1, x2, f1), f1]
]
# else:
# ref2 = [[alpha(x1, x2, f1), f1],
# [alpha(-x1, x2, f2), f2]
# ]
ref2 = ref2*n_cyc + [ref2[0]]
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0], eps,
(x2_idx*dx, -x1_idx*dy)))
gait = pf.predict_gait(ref2, init_pose, weight)
(dxx, dyy), deps = gait.get_travel_distance()
RESULT_DX[x2_idx][x1_idx] = dxx
RESULT_DY[x2_idx][x1_idx] = dyy
RESULT_DEPS[x2_idx][x1_idx] = deps
print('(x2, x1):', round(x2, 1), round(x1, 1), ':', round(deps, 2))
Phi = gait.plot_phi()
plt.title('Phi')
cumstress = gait.plot_stress()
RESULT_STRESS[x2_idx][x1_idx] = cumstress
plt.title('Inner Stress')
Alp = gait.plot_alpha()
from Src.Utils import plot_fun as pf
from Src.Math import kinematic_model as model
# %% POSE 0000
alpha = [0, 0, -0, 0, 0]
eps = 88
F1 = (0, 0)
p0000 = pf.GeckoBotPose(*model.set_initial_pose(alpha, eps, F1))
g0000 = pf.GeckoBotGait(p0000)
g0000.plot_gait()
g0000.save_as_tikz('0000')
# %% POSE 1000
ref = [[0, 90, 90, 0, 90], [0, 1, 1, 0]]
g1000 = pf.predict_gait([ref], p0000)
# g1000.plot_gait()
g1000.save_as_tikz('1000')
g1000.plot_gait()
print(g1000.get_travel_distance())
# %% POSE 0000_
ref = [[0, 0, 0, 0, 0], [1, 0, 0, 1]]
g0000_ = pf.predict_gait([ref], g1000.poses[-1])
g0000_.save_as_tikz('0000_')
# %% POSE 0100
ref = [[90, 0, -90, 90, 0], [1, 0, 0, 1]]
g0100 = pf.predict_gait([ref], p0000)
g0100.plot_gait()
g0100.save_as_tikz('0100')