from Src.Utils import plot_fun as pf
alp = [90, 0, -90, 90, 0]
eps = 0
ell = [1, 1, 1.2, 1, 1]
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,
x, marks, f = model.set_initial_pose(alp,
eps,
p1,
len_leg=ell[0],
len_tor=ell[2])
ref1_pose = pf.GeckoBotPose(*model.set_initial_pose(
ref1[0], eps, (25, -3), len_leg=ell[0] * .6, len_tor=ell[2] * .6))
ref2_pose = pf.GeckoBotPose(*model.set_initial_pose(
ref2[0], eps, (25, -3), len_leg=ell[0] * .6, len_tor=ell[2] * .6))
initial_marks = marks
initial_pose = pf.GeckoBotPose(x, marks, f)
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
[f_l, f_o, f_a] = [89, 10, 5.9]
dev_ang = 100
blow = .9
bup = 1.1
n_limbs = 5
n_foot = 4
class Memory(object):
def __init__(self):
self.val = 0
#
elemtary_patterns['curve_right_super_tight'] = [
[[1, 90, 90, 1, 90], [0, 1, 1, 0], 'pose1'],
[[1, 90, 90, 1, 90], [1, 0, 0, 1]],
[[50, 30, 90, 30, 150], [1, 0, 0, 1]],
[[50, 30, 90, 30, 150], [0, 1, 1, 0]],
[[124, 164, 152, 62, 221], [0, 1, 1, 0]],
[[124, 164, 152, 62, 221], [1, 0, 0, 1]],
[[30, 90, 80, 10, 10], [1, 0, 0, 1]],
[[30, 90, 80, 10, 10], [0, 1, 1, 0]],
[[1, 90, 90, 1, 90], [0, 1, 1, 0], 'pose1'],
]
for key in elemtary_patterns:
gait = pf.GeckoBotGait()
x, marks, f = model.set_initial_pose(
alp_0, eps_0, (0, 0), len_leg=1, len_tor=1.2)
for ref in elemtary_patterns[key]:
try:
posename = ref[2]
ref = ref[:2]
except IndexError:
posename = None
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, name=posename))
plt.yticks(
[0, 90, 180, 270, 360],
['0',
pifrac('', 2), '$\\pi$',
pifrac(3, 2), '$2\\pi$'])
plt.grid()
kwargs = {'extra_axis_parameters': {'anchor=origin'}}
name = 'PHI_{}_{}_{}.tex'.format(key, startpose,
str(x2).replace('.', '_'))
save_utils.save_plt_as_tikz('Out/analytic_model8/' + name,
**kwargs)
# %%
# fig.clear()
pf.save_animation(gait.animate(),
'../../Out/analytic_model8/gait.mp4')
# %% POSES
gait_ = pf.GeckoBotGait()
gait_.append_pose(gait.poses[1]) # init pose
gait_.append_pose(gait.poses[int((res))]) # midpose
gait_.append_pose(gait.poses[int((res + 1))]) # midpose
gait_.append_pose(gait.poses[-1]) # end pose
gait_.plot_gait()
gait_.plot_orientation(length=5)
gait_.save_as_tikz('gait')
# %%
plt.show()
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 25 17:25:25 2019
@author: ls
"""
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_')