p1 = (0, 0)
x, marks, feet = model.set_initial_pose(alp,
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))
mx, my = pose.markers
act_pos = np.r_[mx[1], my[1]]
dpos = xref - act_pos
return np.linalg.norm(dpos)
def add_noise(alpha):
return list(np.r_[alpha] + np.random.normal(0, 5, 5))
i = 0
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',
# for idx2, x2 in enumerate(X2):
# D[idx2][idx1] = optplanner.calc_d(xbar, optplanner.dx(x1, x2),
# optplanner.deps(x1, x2), n)
#
#plot_contour(X1, X2, D)
#kwargs = {'extra_axis_parameters': {'font=\\small'}}
#save.save_plt_as_tikz('Out/opt_pathplanner/dist_test.tex', **kwargs)
# %% Create Case Study:
xref = [20, 35] # global cos
eps = 90
p1 = (0, 0)
n = 4
xbar = optplanner.xbar(xref, p1, eps)
print('xbar:', xbar)
X1 = np.arange(50, 90.01, 2)
X2 = np.arange(-.5, .501, .01)
D = np.zeros((len(X2), len(X1)))
dmin = {'val': 1e16, 'x1': None, 'x2': None}
for idx1, x1 in enumerate(X1):
for idx2, x2 in enumerate(X2):
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)
# %% 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)
xbar = opt.xbar(xref, p1, eps)
pattern = rotspot.rotate_on_spot(xbar, alpha, feet)
for pose in pattern:
alpha, feet, ptime = pose
act_pose = gait.poses[-1]
x, y = act_pose.markers
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)
# gait.plot_gait()
plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12)