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_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 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
# %% for mindist in MINDIST: sucess[mindist] = tries 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]]
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
XREF = {} # XREF['left'] = (-2, 3) # XREF['right'] = (2, 3) XREF['backwards'] = (2, -3) n = 1 for key in XREF: xref = XREF[key] for replay in range(5): alpha = [90, 0, -90, 90, 0] feet = [1, 0, 0, 1] # alpha = [0, 90, 90, 0, 90] # feet = [0, 1, 1, 0] 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) 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)) i = 0 while calc_dist(gait.poses[-1], xref) > .7:
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')
# plt.text(x2_idx*dx, 1+n_cyc, 'x2={}'.format(round(x2, 1))) f1 = [0, 1, 1, 0] 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')
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)) # %%
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))
@author: AmP """ from Src.TrajectoryPlanner import optimal_planner as op 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,
# -*- 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_')
f_len, f_ori, f_ang, len(obj_hist), round(opt_obj, 2), alp_opt[0], alp_opt[1], alp_opt[2], alp_opt[3], alp_opt[4], alp_opt[5], alp_opt[6], alp_opt[7], alp_opt[8], alp_opt[9]) print('initial guess:\n', x0, '\n') print('property string:\n', prop_str, '\n') print('optimal pattern:\n', alp_opt, '\n') # %% RENDER GAIT & PLOTS 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()
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 = """
] ref2_ = [[[[ 45 - gam / 2. + x, 45 + gam / 2. + x, gam + x, 45 - gam / 2. + x, 45 + gam / 2. + x ], [0, 1, 1, 0]], [[ 45 + gam / 2. + x, 45 - gam / 2. + x, -gam + x, 45 + gam / 2. + x, 45 - gam / 2. + x ], [1, 0, 0, 1]]] for gam, x in [(80, 22), (80, 22)]] ref2 = model.flat_list(ref2_) # print(ref2) # ref2 = ref init_pose = pf.GeckoBotPose( *model.set_initial_pose(ref2[0][0], eps, ref2[0][1])) gait = pf.predict_gait(ref2, init_pose) gait.plot_gait() gait.plot_markers() gait.plot_stress() gait.plot_travel_distance() gait.plot_orientation() gait.plot_alpha() gait.plot_phi() print(gait.get_travel_distance()) # line_ani = gait.animate() # pf.save_animation(line_ani) # gait.save_as_tikz('test_plot_fun')
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 - 30, (0, 0), len_leg=len_leg, len_tor=len_tor)) # sim one fast cycle -> drop first cycle init_ref = [ref2[int(res - 1)], ref2[-1]] init_gait = pf.predict_gait(init_ref, init_pose, weight, (len_leg, len_tor)) init_pose = init_gait.poses[-1] 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))
p1 = (0, 0) # REF straight alp = [5, 85, 87.5, 5, 85] 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]
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))
save.save_plt_as_tikz('Out/opt_pathplanner/dist_n_{}.tex'.format(n), **kwargs) # %% Gecko 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)
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()
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_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() RESULT_STRESS[x2_idx][x1_idx] = cumstress
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() RESULT_STRESS[x1_idx][x2_idx][c1_idx] = cumstress
# if feet != self.last_feet: # switch fix # pattern.append([self.last_alp, [1, 1, 1, 1], t[1]]) # fix # pattern.append([self.last_alp, feet, t[2]]) # defix 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