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 predict_gait(references, initial_pose, weight=None, lens=[None]): if not lens: lens = [1, 1.2] len_leg = lens[0] len_tor = lens[1] if not weight: weight = [model.f_l, model.f_o, model.f_a] gait = GeckoBotGait(initial_pose) for idx, ref in enumerate(references): x, (mx, my), f, constraint, cost = model.predict_next_pose( ref, gait.poses[idx].x, gait.poses[idx].markers, f=weight, len_leg=len_leg, len_tor=len_tor) gait.append_pose( GeckoBotPose(x, (mx, my), f, constraint=constraint, cost=cost)) _ = gait.poses.pop(0) # remove initial pose return gait
def objective(x): print([round(xx, 2) for xx in x]) x_opt, marks_opt, _, _ = model.predict_next_pose([x, feet_ref], x_init, marks_act, f=f, len_leg=len_leg, len_tor=len_tor) eps_opt = x_opt[-1] pos_opt = np.r_[marks_opt[1][0], marks_opt[1][1]] dpos_opt = p_ref - pos_opt dist_opt = np.linalg.norm(dpos_opt) dir_opt = np.r_[np.cos(np.radians(eps_opt)), np.sin(np.radians(eps_opt))] deps_opt = uf.calc_angle(dpos_opt, dir_opt) cost = (weight_dist * (dist_opt / dist_act)) # + (1 - weight_dist)*abs(deps_opt/180.)) print(round(cost, 3)) return cost
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)) gait.plot_gait()
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)) #gait.plot_gait() gait.plot_markers(1) gait.plot_orientation(length=7, w=.4, size=3, colp='orange') # %% #plt.xlabel('$x$ position (cm)') #plt.ylabel('$y$ position (cm)')
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), weight, len_leg=len_leg, len_tor=len_tor) predicted_pose = pf.GeckoBotPose(*predicted_pose) gait.append_pose(predicted_pose) # switch feet # Plot gait.plot_gait(reverse_col=1) gait.plot_markers(1) # gait.plot_com() plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12) plt.axis('off') # plt.savefig('Out/opt_pathplanner/gait.png', transparent=False, # dpi=300)
eps, xref, act_pose, save_as_tikz=True, gait=gait, show_dec=0) print('\n\npose ', i) print('pose:\t\t', pose_id, ' -- ', alpha) print('distance:\t', calc_dist(gait.poses[-1], xref)) # NOISE # alpha = add_noise(alpha) if ':' not in pose_id or pose_id == 'crawling': predicted_pose = model.predict_next_pose([alpha, feet], act_pose.x, (x, y), len_leg=ell[0], len_tor=ell[2], f=[f_l, f_o, f_a]) predicted_pose = pf.GeckoBotPose(*predicted_pose) gait.append_pose(predicted_pose) i += 1 if i > 30: print('Failed') sucess[mindist] -= 1 break # %% # gait.plot_markers(1) # # gait.plot_com()
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 alp4 = alpha1(x1, x2, f2) pose4 = pf.GeckoBotPose(*model.set_initial_pose(alp4, eps, (0, 0))) pose4.f = f2 pose4.save_as_tikz('pose4', compileit=False) x1 = -80 x2 = .5 alp5 = alpha1(x1, x2, f1) pose5 = pf.GeckoBotPose(*model.set_initial_pose(alp5, eps, (0, 0))) pose5.f = f1 pose5.save_as_tikz('pose5', compileit=False) alp5a = alpha1(-x1, x2, f2) pose5a = pf.GeckoBotPose( *model.predict_next_pose([alp5a, f2], pose5.x, pose5.markers)) pose5a.save_as_tikz('pose5a', compileit=False) x1 = 80 alp6 = alpha1(x1, x2, f2) pose6 = pf.GeckoBotPose(*model.set_initial_pose(alp6, eps, (0, 0))) pose6.f = f2 pose6.save_as_tikz('pose6', compileit=False)
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', round(calc_dist(gait.poses[-1], xref), 2), '\n') if i > 10: break # gait.plot_gait() gait.plot_markers(1) # gait.plot_com() # %% Plots plt.plot(xref[0], xref[1], marker='o', color='red', markersize=12)
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() RESULTS[method][cat][f_len][f_ang][f_ori]['stress'] = stress RESULTS[method][cat][f_len][f_ang][f_ori]['dist'] = dist RESULTS[method][cat][f_len][f_ang][f_ori]['deps'] = deps RESULTS[method][cat][f_len][f_ang][f_ori]['alp'] = alp_opt save_obj(RESULTS, 'RESULTS_curve') gait.plot_gait() gait.plot_markers([1])
# REF refs = [] step = 15 for alp in np.arange(0, 91, step): for bet in np.arange(-90, 91, step): for gam in np.arange(0, 91, step): refs.append([[0, alp, bet, gam, 0], [0, 1, 1, 0]]) maxima = {'minx': 1, 'maxx': 1, 'miny': 1, 'maxy': 1} index = {'minx': 0, 'maxx': 0, 'miny': 0, 'maxy': 0} for idx, ref in enumerate(refs): x, marks, f, constraint, cost = model.predict_next_pose(ref, x, marks, len_leg=ell[0], len_tor=ell[2]) gait.append_pose( pf.GeckoBotPose(x, marks, f, constraint=constraint, cost=cost)) # check maxima: px, py = marks[0][1], marks[1][1] if px < maxima['minx']: maxima['minx'] = px index['minx'] = idx if px > maxima['maxx']: maxima['maxx'] = px index['maxx'] = idx if py < maxima['miny']: maxima['miny'] = py index['miny'] = idx
[alpha, feet], q = opt.optimal_planner_nhorizon(xbar, alp_act, feet, lastq1, nmax=nmax, show_stats=1, q1bnds=[50, 90]) lastq1 = q[0] Q1.append(abs(q[0])) Q2.append(q[1]) # alpha = add_noise(alpha) predicted_pose = model.predict_next_pose([alpha, feet], act_pose.x, (x, y), f=[f_l, f_o, f_a], len_leg=l_leg, len_tor=l_tor) predicted_pose = pf.GeckoBotPose(*predicted_pose) gait.append_pose(predicted_pose) i += 1 dist = calc_dist(gait.poses[-1], xref) DIST.append(dist) print('pose', i, 'dist: \t', round(dist, 2), '\n') if i > 100: break # %% Plots