def pose(q1, q2):
pose1 = pf.GeckoBotPose(*model.set_initial_pose(
alpha(q1, q2), eps, p1, len_leg=len_leg,
len_tor=len_tor)).get_tikz_repr(R=.7, col=colors)
pose2 = pf.GeckoBotPose(*model.set_initial_pose(
alpha(-q1, q2), eps, p2, len_leg=len_leg,
len_tor=len_tor)).get_tikz_repr(R=.7, col=colors)
label1 = "\n\\path (OM)++(0,15)node{$\\bm{r}(q_1, q_2)$};"
label2 = "\n\\path (OM)++(0,15)node{$\\bm{r}(-q_1, q_2)$};"
return "\n\\begin{scope}[scale=%s]" % roboscale + pose1 + label1 + pose2 + label2 + "\n\\end{scope}"
def objective(x):
alp, ell, eps = x[0:n_limbs], x[n_limbs:2 * n_limbs], x[-1]
phi = model._calc_phi(alp, eps)
obj_ori, obj_len, obj_ang = 0, 0, 0
for idx in range(n_foot):
if f[idx]:
# dphi = calc_difference(phi[idx], phi_init[idx])
dphi = phi[idx] - phi_init[idx]
obj_ori = (obj_ori + dphi**2)
for idx in range(n_limbs):
obj_ang = obj_ang + (alpref[idx] - alp[idx])**2
for idx in range(n_limbs):
obj_len = obj_len + (ell[idx] - ell_nominal[idx])**2
objective = (f_ori * np.sqrt(obj_ori) + f_ang * np.sqrt(obj_ang) +
f_len * np.sqrt(obj_len))
if gait and abs(last_obj.val - objective) > .01:
marks = model._calc_coords(x, markers_init, f)
for fidx, fi in enumerate(f):
if fi == 1:
marks[0][midx(fidx)] = markers_init[0][midx(fidx)]
marks[1][midx(fidx)] = markers_init[1][midx(fidx)]
X.val.append(list(alp) + list(ell) + [eps])
Marks.val.append(marks)
Stress.val.append(objective)
gait.append_pose(pf.GeckoBotPose(x, marks, f, cost=objective))
print(gait.poses[-1].x[0])
last_obj.val = objective
return objective
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 q2 < 0:
if 1:
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))
x1, x2 = x1_opt, x2_opt # optimal gait
feet0 = [1, 0, 0, 1] # feet states
alpha0 = [90, 0, -90, 90, 0]
weight = [89, 10, 5.9] # [f_l, f_o, f_a]
len_leg, len_tor = calibration.get_len('vS11')
# alpha = [90, 0, -90, 90, 0]
x, (mx, my), f = model.set_initial_pose(alpha0,
eps,
p1,
len_leg=len_leg,
len_tor=len_tor)
initial_pose = pf.GeckoBotPose(x, (mx, my), f)
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),
ref1 = [[85, 5, -87.5, 85, 5], [1, 0, 0, 1]]
ref2 = [[85, 5, -87.5, 85, 5], [1, 0, 0, 1]]
#ref2 = [[5, 85, 87.5, 5, 85], [0, 1, 1, 0]]
# REF curve
#alp = [5, 5, -24, 5, 5]
#ref1 = [[164, 124, -152, 221, 62], [1, 0, 0, 1]]
#ref2 = [[5, 5, -24, 5, 5], [0, 1, 1, 0]]
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
for try_idx in range(tries):
alpha = [90, 0, -90, 90, 0]
eps0 = 90
p1 = (0, 0)
ell = [9.1, 9.1, 10.3, 9.1, 9.1]
f_l, f_o, f_a = 1, 1, 1
x, (mx, my), f = model.set_initial_pose(alpha,
eps0,
p1,
len_leg=ell[0],
len_tor=ell[2])
initial_pose = pf.GeckoBotPose(x, (mx, my),
f,
len_leg=ell[0],
len_tor=ell[2])
gait = pf.GeckoBotGait()
gait.append_pose(initial_pose)
ref = st.ReferenceGenerator('L')
xref = (20, 30)
def calc_dist(pose, xref):
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))
f1 = [0, 1, 1, 0]
f2 = [1, 0, 0, 1]
if x2 < 0:
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
if and_half:
ref2 = ref2[1:] + [ref2[0]]
# get start positions
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0],
eps, (x2_idx * dx,
x1_idx * dy),
len_leg=len_leg,
len_tor=len_tor))
gait_ = pf.predict_gait(ref2, init_pose, weight,
(len_leg, len_tor))
alp_ = gait_.poses[-1].alp
ell_ = gait_.poses[-1].ell
init_pose = pf.GeckoBotPose(*model.set_initial_pose(
alp_, eps, (x2_idx * dx, x1_idx * dy), ell=ell_),
len_leg=len_leg,
len_tor=len_tor)
# actually simulation
gait = pf.predict_gait(ref2, init_pose, weight,
(len_leg, len_tor))
from Src.Math import kinematic_model as model
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],
first = True
for simidx, ref in enumerate(ref2):
doit = 2 if first else 1
first = False
for job in range(doit):
alp, f = ref
for idx, alpi in enumerate(alp):
REF['alp'][idx].append(alpi)
REF['eps'][0].append(-X1[simidx] * x2)
# ref2 = fill_pattern([alpha(x1, x2, f1), f1],
# [alpha(-x1, x2, f2), f2],
# dig=res)
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0], eps, (0, 0)))
gait = pf.predict_gait(ref2, init_pose, weight)
(dxx, dyy), deps = gait.get_travel_distance()
print('(x2, x1):', round(x2, 1), round(x1, 1), ':',
round(deps, 2))
Phi = gait.plot_phi()
for phi in Phi:
RESULT['phi'].append(phi)
plt.title('Phi')
cumstress = gait.plot_stress()
plt.title('Inner Stress')
Alp = gait.plot_alpha()
f2 = [0, 1, 1, 0]
f1 = [1, 0, 0, 1]
incr = 20
ref = []
for q1 in np.linspace(-89, 89, incr):
ref.append([alpha(q1, f2), f2])
ref.append([alpha(q1, f2), [1, 1, 1, 1]])
for q1 in np.linspace(89, -89, incr):
ref.append([alpha(q1, f1), f1])
ref.append([alpha(q1, f1), [1, 1, 1, 1]])
ref.append([alpha(q1, f1), f2])
init_pose = pf.GeckoBotPose(*model.set_initial_pose(
ref[0][0], eps, p1, len_leg=len_leg, len_tor=len_tor))
gait = pf.predict_gait(ref, init_pose, weight, (len_leg, len_tor))
gait.plot_gait()
# %%
header_0 = """
\\documentclass[tikz]{standalone}
%% This file was create by 'GeckoBotModel/Scripts/Animation_gait/straight.py'
\\usepackage[sfdefault, light]{FiraSans}
\\usepackage{bm}
\\begin{document}
"""
header = """
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))
# %%
gait.plot_gait()
gait.plot_markers([1])
plt.axis('off')
plt.show()
# %% SAVE AS TIKZ
option = """
pose1/.style = {shape=circle, draw, align=center, top color=white, bottom color=blue!40, minimum width=1.9cm, opacity=.5},
pose2/.style = {shape=circle, draw, align=center, top color=white, bottom color=red!40, minimum width=1.9cm, opacity=.5},
for x1_idx, x1 in enumerate(X1):
for x2_idx, x2 in enumerate(X2):
f1 = [0, 1, 1, 0]
f2 = [1, 0, 0, 1]
if x2 < 0:
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
if and_half:
ref2 += [ref2[0]]
init_pose = pf.GeckoBotPose(
*model.set_initial_pose(ref2[0][0],
eps, (x2_idx * dx, x1_idx * dy),
len_leg=1,
len_tor=1.2))
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, 2), round(x1, 1), ':',
round(deps, 2))
Phi = gait.plot_phi()
plt.title('Phi')
cumstress = gait.plot_stress()
# -*- 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_')
pattern.append([alp, feet, t[0]]) # move
self.last_alp = alp
self.last_feet = feet
return pattern
# %% with Sim Model
x, marks, feet = model.set_initial_pose(
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))
plt.figure('GeckoBotGait')
plt.text(x_idx * dx, 1 + n_cyc, 'x={}'.format(round(x)))
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()
for x1 in X1[::-1]: # reverse
alp = alpha(x1, x2, f2, c1)
ref2.append([alp, f2])
for idx, alpi in enumerate(alp):
REF['alp'][idx].append(alpi)
REF['eps'][0].append(
(3 * c1 * X1[0] * x2 + x1 * x2 * c1) / 1)
REF['f1'].append(0)
REF['f2'].append(1)
else:
for x1 in X1:
ref2.append([alpha(-x1, x2, f2), f2])
init_pose = pf.GeckoBotPose(*model.set_initial_pose(
ref2[0][0], eps0, (0,
0), 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()
print('(x2, x1):', round(x2, 1), round(x1, 1), ':',
round(deps, 2))
Phi = gait.plot_phi()
for phi in Phi:
RESULT['phi'].append(phi)
plt.title('Phi')
cumstress = gait.plot_stress()
plt.title('Inner Stress')
for c1_idx, c1 in enumerate(C1):
for x2_idx, x2 in enumerate(X2):
f1 = [0, 1, 1, 0]
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()
def alpha1(x1, x2, f):
alpha = [(45 - x1 / 2. - abs(x1) * x2 / 2. + (f[0]) * x1 * x2),
(45 + x1 / 2. + abs(x1) * x2 / 2. + (f[1]) * x1 * x2),
x1 + x2 * abs(x1),
(45 - x1 / 2. - abs(x1) * x2 / 2. + (f[2]) * x1 * x2),
(45 + x1 / 2. + abs(x1) * x2 / 2. + (f[3]) * x1 * x2)]
return alpha
eps = 90
alp1 = [85, 5, -80, 85, 5]
alp2 = [5, 85, 80, 5, 85]
f1 = [1, 0, 0, 1]
f2 = [0, 1, 1, 0]
pose1 = pf.GeckoBotPose(*model.set_initial_pose(alp1, eps, (0, 0)))
pose1.f = f1
pose1.save_as_tikz('pose1', compileit=False)
pose2 = pf.GeckoBotPose(*model.set_initial_pose(alp2, eps, (0, 0)))
pose2.f = f2
pose2.save_as_tikz('pose2', compileit=False)
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
