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