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 get_ref(self, q1, q2, t=[1.2, .1, .1]):
"""
1. generate reference according to input
2. Check fixation
a. fix
b. defix
3. move
"""
pattern = []
alp = op.alpha(abs(q1)*np.sign(self.last_alp[2]*-1), q2)
feet = [0, 1, 1, 0] if alp[2]*q1 > 0 else [1, 0, 0, 1] # ><
# 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
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),
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()
from Src.Utils import plot_fun as pf from Src.Utils import save # %% # #f_l = .1 #f_o = 1 #f_a = 10 f_l = 89. # factor on length objective f_o = 10 # .0003 # factor on orientation objective f_a = 5.9 # factor on angle objective weights = [f_l, f_o, f_a] ## init pose alp0 = op.alpha(-80, -.5) #alp = [90, 0, -90, 90, 0] eps0 = 90 ell = [9.1, 9.1, 10.3, 9.1, 9.1] p1 = (0, 0) # Q ref Q = [(80, -.5), (80, -.5)] # times t_move = 1.2 t_fix = .1 t_defix = .1 # %%
from Src.Utils import plot_fun as pf from Src.Utils import save # %% # #f_l = .1 #f_o = 1 #f_a = 10 f_l = 89. # factor on length objective f_o = 10 # .0003 # factor on orientation objective f_a = 5.9 # factor on angle objective weights = [f_l, f_o, f_a] ## init pose alp0 = op.alpha(-80, .5) #alp0 = [90, 0, -90, 90, 0] eps0 = 0 ell = [9.1, 9.1, 10.3, 9.1, 9.1] p1 = (0, 0) # Q ref Q = [(80, .5), (80, .5)] # times t_move = 1.2 t_fix = .1 t_defix = .1 # %%