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_')