def objective_straight(X):
x, marks, _ = model.set_initial_pose(
X, 90, (0, 0), len_leg=1, len_tor=1.2)
ptrn = gnerate_ptrn_symmetric(X, n_cycles)
for ref in ptrn:
x, marks, _, _, _ = model.predict_next_pose(
ref, x, marks, len_leg=1, len_tor=1.2, f=f_weights)
mx, my = marks
obj = -my[1]
obj_history.append(obj)
print('step', len(obj_history), '\t', round(obj, 4))
return obj
def objective_curve(X):
x, marks, _ = model.set_initial_pose(
x0, 90, (0, 0), len_leg=1, len_tor=1.2)
ptrn = gnerate_ptrn(X, n_cycles)
for ref in ptrn:
x, marks, _, _, _ = model.predict_next_pose(
ref, x, marks, len_leg=1, len_tor=1.2, f=f_weights)
eps = x[-1]
obj = 90-eps
obj_history.append(obj)
print('step', len(obj_history), '\t', round(obj, 4))
return obj
def predict_gait(references, initial_pose, weight=None, lens=[None]):
if not lens:
lens = [1, 1.2]
len_leg = lens[0]
len_tor = lens[1]
if not weight:
weight = [model.f_l, model.f_o, model.f_a]
gait = GeckoBotGait(initial_pose)
for idx, ref in enumerate(references):
x, (mx, my), f, constraint, cost = model.predict_next_pose(
ref,
gait.poses[idx].x,
gait.poses[idx].markers,
f=weight,
len_leg=len_leg,
len_tor=len_tor)
gait.append_pose(
GeckoBotPose(x, (mx, my), f, constraint=constraint, cost=cost))
_ = gait.poses.pop(0) # remove initial pose
return gait
def objective(x):
print([round(xx, 2) for xx in x])
x_opt, marks_opt, _, _ = model.predict_next_pose([x, feet_ref],
x_init,
marks_act,
f=f,
len_leg=len_leg,
len_tor=len_tor)
eps_opt = x_opt[-1]
pos_opt = np.r_[marks_opt[1][0], marks_opt[1][1]]
dpos_opt = p_ref - pos_opt
dist_opt = np.linalg.norm(dpos_opt)
dir_opt = np.r_[np.cos(np.radians(eps_opt)),
np.sin(np.radians(eps_opt))]
deps_opt = uf.calc_angle(dpos_opt, dir_opt)
cost = (weight_dist * (dist_opt / dist_act))
# + (1 - weight_dist)*abs(deps_opt/180.))
print(round(cost, 3))
return cost
eps,
p1,
len_leg=ell[0],
len_tor=ell[2])
feet = [1, 0, 0, 1]
initial_pose = pf.GeckoBotPose(x, marks, feet)
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
x_ref = (10, 10)
xbar = op.xbar(x_ref, p1, eps)
alp_ref, feet_ref = op.optimal_planner(xbar,
alp,
feet,
n=2,
dist_min=.1,
show_stats=0)
x, marks, f, constraint, cost = model.predict_next_pose([alp_ref, feet_ref],
x,
marks,
len_leg=1,
len_tor=1.2)
gait.append_pose(pf.GeckoBotPose(x, marks, f, constraint=constraint,
cost=cost))
gait.plot_gait()
alp0, eps0, p1, len_leg=ell[0], len_tor=ell[2])
feet = [1, 0, 0, 1]
initial_pose = pf.GeckoBotPose(x, marks, feet)
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
osci = Oscillator(alp0, feet)
for (q1, q2) in Q:
pattern = osci.get_ref(q1, q2)
for ref in pattern:
print(ref)
alp_ref, feet_ref, p_time = ref
x, marks, f, constraint, cost = model.predict_next_pose(
[alp_ref, feet_ref], x, marks, len_leg=ell[0], len_tor=ell[2], f=weights)
gait.append_pose(
pf.GeckoBotPose(x, marks, f, constraint=constraint, cost=cost))
#gait.plot_gait()
gait.plot_markers(1)
gait.plot_orientation(length=7, w=.4, size=3, colp='orange')
# %%
#plt.xlabel('$x$ position (cm)')
#plt.ylabel('$y$ position (cm)')
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
for cyc in range(n):
for sign in [1, -1]:
act_pose = gait.poses[-1]
x, y = act_pose.markers
# generate ref
x1_ = x1 * sign
feet = [not (f) for f in feet0] if sign == 1 else feet0
alpha = optplanner.alpha(x1_, x2, feet)
print(x1_, x2, alpha, feet)
# predict
predicted_pose = model.predict_next_pose([alpha, feet],
act_pose.x, (x, y),
weight,
len_leg=len_leg,
len_tor=len_tor)
predicted_pose = pf.GeckoBotPose(*predicted_pose)
gait.append_pose(predicted_pose)
# switch feet
# Plot
gait.plot_gait(reverse_col=1)
gait.plot_markers(1)
# gait.plot_com()
plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12)
plt.axis('off')
# plt.savefig('Out/opt_pathplanner/gait.png', transparent=False,
# dpi=300)
eps,
xref,
act_pose,
save_as_tikz=True,
gait=gait,
show_dec=0)
print('\n\npose ', i)
print('pose:\t\t', pose_id, ' -- ', alpha)
print('distance:\t', calc_dist(gait.poses[-1], xref))
# NOISE
# alpha = add_noise(alpha)
if ':' not in pose_id or pose_id == 'crawling':
predicted_pose = model.predict_next_pose([alpha, feet],
act_pose.x,
(x, y),
len_leg=ell[0],
len_tor=ell[2],
f=[f_l, f_o, f_a])
predicted_pose = pf.GeckoBotPose(*predicted_pose)
gait.append_pose(predicted_pose)
i += 1
if i > 30:
print('Failed')
sucess[mindist] -= 1
break
# %%
# gait.plot_markers(1)
# # gait.plot_com()
x1 = -80
x2 = -.5
alp3 = alpha1(x1, x2, f1)
pose3 = pf.GeckoBotPose(*model.set_initial_pose(alp3, eps, (0, 0)))
pose3.f = f1
pose3.save_as_tikz('pose3', compileit=False)
x1 = 80
alp4 = alpha1(x1, x2, f2)
pose4 = pf.GeckoBotPose(*model.set_initial_pose(alp4, eps, (0, 0)))
pose4.f = f2
pose4.save_as_tikz('pose4', compileit=False)
x1 = -80
x2 = .5
alp5 = alpha1(x1, x2, f1)
pose5 = pf.GeckoBotPose(*model.set_initial_pose(alp5, eps, (0, 0)))
pose5.f = f1
pose5.save_as_tikz('pose5', compileit=False)
alp5a = alpha1(-x1, x2, f2)
pose5a = pf.GeckoBotPose(
*model.predict_next_pose([alp5a, f2], pose5.x, pose5.markers))
pose5a.save_as_tikz('pose5a', compileit=False)
x1 = 80
alp6 = alpha1(x1, x2, f2)
pose6 = pf.GeckoBotPose(*model.set_initial_pose(alp6, eps, (0, 0)))
pose6.f = f2
pose6.save_as_tikz('pose6', compileit=False)
while calc_dist(gait.poses[-1], xref) > .7:
act_pose = gait.poses[-1]
x, y = act_pose.markers
act_pos = (x[1], y[1])
eps = act_pose.x[-1]
alp_act = act_pose.alp
xbar = opt.xbar(xref, act_pos, eps)
alpha, feet = opt.optimal_planner(xbar,
alp_act,
feet,
n,
show_stats=1)
alpha = add_noise(alpha)
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.figure('opt_hist: ' + prop_str)
plt.title('opt_hist: ' + prop_str)
plt.plot(obj_hist)
x, marks, f = model.set_initial_pose(
alp_opt[:5], 90, (0, 0), len_leg=1, len_tor=1.2)
initial_pose = pf.GeckoBotPose(x, marks, f)
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
if cat == 'straight':
ptrn = gnerate_ptrn_symmetric(alp_opt, 1, half=True)
if cat == 'curve':
ptrn = gnerate_ptrn(alp_opt, 1, half=True)
for ref in ptrn:
x, marks, f, constraint, cost = model.predict_next_pose(
ref, x, marks, len_leg=1, len_tor=1.2, f=f_weights)
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])
# REF
refs = []
step = 15
for alp in np.arange(0, 91, step):
for bet in np.arange(-90, 91, step):
for gam in np.arange(0, 91, step):
refs.append([[0, alp, bet, gam, 0], [0, 1, 1, 0]])
maxima = {'minx': 1, 'maxx': 1, 'miny': 1, 'maxy': 1}
index = {'minx': 0, 'maxx': 0, 'miny': 0, 'maxy': 0}
for idx, ref in enumerate(refs):
x, marks, f, constraint, cost = model.predict_next_pose(ref,
x,
marks,
len_leg=ell[0],
len_tor=ell[2])
gait.append_pose(
pf.GeckoBotPose(x, marks, f, constraint=constraint, cost=cost))
# check maxima:
px, py = marks[0][1], marks[1][1]
if px < maxima['minx']:
maxima['minx'] = px
index['minx'] = idx
if px > maxima['maxx']:
maxima['maxx'] = px
index['maxx'] = idx
if py < maxima['miny']:
maxima['miny'] = py
index['miny'] = idx
[alpha,
feet], q = opt.optimal_planner_nhorizon(xbar,
alp_act,
feet,
lastq1,
nmax=nmax,
show_stats=1,
q1bnds=[50, 90])
lastq1 = q[0]
Q1.append(abs(q[0]))
Q2.append(q[1])
# alpha = add_noise(alpha)
predicted_pose = model.predict_next_pose([alpha, feet],
act_pose.x, (x, y),
f=[f_l, f_o, f_a],
len_leg=l_leg,
len_tor=l_tor)
predicted_pose = pf.GeckoBotPose(*predicted_pose)
gait.append_pose(predicted_pose)
i += 1
dist = calc_dist(gait.poses[-1], xref)
DIST.append(dist)
print('pose', i, 'dist: \t', round(dist, 2), '\n')
if i > 100:
break
# %% Plots
