def move_base():
global cycle_time_f
global steps
global sample_time_f
#st = 40
cycle_time_f = 0.2
current = 0
last_fix = 0
i=0
sample_time_f =np.float(cycle_time_f) / steps # sample time
trans1,hip1,knee1 = gait.generalbasemover_modifed(1, 'f',60 ,steps)
trans2,hip2,knee2 = gait.generalbasemover_modifed(2, 'f',60 ,steps)
trans3,hip3,knee3 = gait.generalbasemover_modifed(3, 'f',60 ,steps)
trans4,hip4,knee4 = gait.generalbasemover_modifed(4, 'f',60 ,steps)
var =1
varr =2
varrr =3
varrrr =4
while(1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f ) and i < steps:
last_fix = current
set_angle(((var-1)*3),trans1[i])
set_angle(((var-1)*3)+1 , hip1[i])
set_angle((((var-1)*3)+2), knee1[i])
set_angle(((varr-1)*3),trans2[i])
set_angle(((varr-1)*3)+1 , hip2[i])
set_angle((((varr-1)*3)+2), knee2[i])
set_angle(((varrr-1)*3),trans3[i])
set_angle(((varrr-1)*3)+1 , hip3[i])
set_angle((((varrr-1)*3)+2), knee3[i])
set_angle(((varrrr-1)*3),trans4[i])
set_angle(((varrrr-1)*3)+1 , hip4[i])
set_angle((((varrrr-1)*3)+2), knee4[i])
i = i + 1
#t = t + sample_time_f
if (i == steps):
flag_start =0
break
def Gate_Publisher(leg_no, legfix_initial_hip, legvar_initial_hip,
legfix_initial_cg, legvar_initial_cg, legfix_Prev_angs,
legvar_Prev_angs):
# Parameters:
global l1
global l2
global initial_leg_height
global initial_leg_height_f
global stride
global stride_f
global h
global h_f
global cycle_time
global cycle_time_f
global steps
global initial_distance
global initial_distance_f
global z
global sample_time_f
global leg_pos3_hip
global leg_pos4_hip
global leg_pos1_hip
global leg_pos2_hip
global x_fixed
global y_fixed
global var
global var2
global var3
global var4
flag_start = 0
x_current = 0
y_current = 0
x_current_f = 0
global indx_fix
t_left = 0
current = 0
last_fix = 0
sample_time = np.float(cycle_time) / steps # sample time
sample_time_f = np.float(cycle_time_f) / steps # sample time
if leg_no == 4 or leg_no == 3:
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
if leg_no == 4:
initial_distance_f = leg_pos4_hip[0]
else:
initial_distance_f = leg_pos3_hip[0]
for t in np.arange(0, cycle_time_f, sample_time_f):
xnew[i] = (stride_f *
((t / cycle_time_f) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time_f)))) -
(stride_f / 2) + stride_f / 2) + initial_distance_f
i = i + 1
xnew = xnew
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time_f / 2, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time_f) * t) +
((2 * h_f * t) / cycle_time_f) -
(h_f / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
n = (cycle_time_f / 2)
for t in np.arange(n, cycle_time_f, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time_f) * t)) -
((2 * h_f * t) / cycle_time_f) +
((3 * h_f) / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
ynew = ynew
x_fixed = xnew
y_fixed = ynew
elif leg_no == 2 or leg_no == 1:
xnew = np.zeros([steps, 1], dtype=float)
ynew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
if leg_no == 2:
initial_distance = leg_pos2_hip[0]
else:
initial_distance = leg_pos1_hip[0]
if leg_no == 2:
var = 1
var2 = 3
var3 = 0
var4 = 2
else:
var = 0
var2 = 2
var3 = 1
var4 = 3
while (1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f) and i < steps:
last_fix = current
xnew[i] = (stride *
((t / cycle_time) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time)))) -
(stride / 2) + stride / 2) + initial_distance
if t < (cycle_time / 2):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time) * t) +
((2 * h * t) / cycle_time) -
(h / 2)) + (h / 2) - initial_leg_height
else:
ynew[i] = (-(h / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time) * t)) -
((2 * h * t) / cycle_time) +
((3 * h) / 2)) + (h / 2) - initial_leg_height
#Publish Fixed leg point
trans, hip, knee = gait.inverse_kinematics_3d_v6(
x_fixed[i], 112.75, y_fixed[i], 0, legfix_Prev_angs[1],
legfix_Prev_angs[2])
set_angle((var * 3), trans)
set_angle((var * 3) + 1, hip)
set_angle(((var * 3) + 2), knee)
#Publish Variable leg point
time.sleep(0.02)
trans, hip, knee = gait.inverse_kinematics_3d_v6(
xnew[i], 112.75, ynew[i], 0, legvar_Prev_angs[1],
legvar_Prev_angs[2])
set_angle((var2 * 3), trans)
set_angle((var2 * 3) + 1, hip)
set_angle((var2 * 3) + 2, knee)
if flag_start == 0:
trans3, hip3, knee3 = gait.generalbasemover_modifed(
var, 'f', 150, 20)
trans1, hip1, knee1 = gait.generalbasemover_modifed(
var2, 'f', 150, 20)
flag_start = 1
set_angle((var3 * 3), trans3[i])
set_angle((var3 * 3) + 1, hip3[i])
set_angle(((var3 * 3) + 2), knee3[i])
set_angle((var4 * 3), trans1[i])
set_angle((var4 * 3) + 1, hip1[i])
set_angle(((var4 * 3) + 2), knee1[i])
if (z == 1):
x_current = xnew[i]
y_current = ynew[i]
#x_current_f = x_fixed[i]
t_left = cycle_time_f - t
indx_fix = i + 1
break
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start = 0
break
if z == 1:
#x_moved = legfix_initial_hip[0] - x_current_f
#t_elabsed = (x_moved/stride_f) * cycle_time_f
cycletime_required = t_left * 2
legfix_final_cg = np.zeros([2, 1], dtype=float)
legvar_final_cg = np.zeros([2, 1], dtype=float)
legfix_final_cg[0] = legfix_initial_cg[
0] + stride_f # this is coord of the pt at end of traj for fixed leg
legfix_final_cg[1] = legfix_initial_cg[1] # Must be relative to cg
legvar_final_cg[1] = legvar_initial_cg[1]
x_target = Mod_contact(leg_no, legfix_final_cg, legvar_final_cg)
trajectory_modification(x_current, y_current, x_target,
cycletime_required, legfix_Prev_angs,
legvar_Prev_angs)
indx_fix = 0
def Gate_Publisher(pair_no, st):
# Parameters:
global l1
global l2
global initial_leg_height
global initial_leg_height_f
global stride
global stride_f
global h
global h_f
global cycle_time
global cycle_time_f
global steps
global initial_distance
global initial_distance_f
global z
global sample_time_f
global leg_pos3_hip
global leg_pos4_hip
global leg_pos1_hip
global leg_pos2_hip
global leg1_ang
global leg2_ang
global leg3_ang
global leg4_ang
global x_fixed
global y_fixed
global var
global var2
global var3
global var4
global first_step_flag
flag_start = 0
x_current = 0
y_current = 0
x_current_f = 0
global indx_fix
t_left = 0
current = 0
last_fix = 0
sample_time = np.float(cycle_time) / steps # sample time
sample_time_f = np.float(cycle_time_f) / steps # sample time
stride = st
stride_f = st
if pair_no == 1: #odd number pair
initial_distance_f = leg_pos1_hip[0]
initial_distance = leg_pos3_hip[0]
legfix_Prev_angs = leg1_ang
legvar_Prev_angs = leg3_ang
y_offset_f = leg_pos1_hip[1] #leg1
y_offset = leg_pos3_hip[1] #leg3
y_plane_f = np.full((steps + 1, 1), y_offset_f)
y_plane = np.full((steps + 1, 1), y_offset)
L_f = 'l'
L_v = 'r'
if first_step_flag == 1:
if initial_distance_f >= 0:
stride_f_mod = stride_f - initial_distance_f
else:
stride_f_mod = stride_f + np.abs(initial_distance_f)
if initial_distance >= 0:
stride_mod = stride_f - initial_distance
else:
stride_mod = stride_f + np.abs(initial_distance)
first_step_flag = first_step_flag + 1
else:
error_f = stride_f / 2 - np.abs(initial_distance_f)
error = stride / 2 - np.abs(initial_distance)
if error_f >= 0:
stride_f_mod = stride_f - error_f
else:
stride_f_mod = stride_f + np.abs(error_f)
if error >= 0:
stride_mod = stride - error
else:
stride_mod = stride + np.abs(error)
#leg no.1
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
for t in np.arange(0, cycle_time_f, sample_time_f):
xnew[i] = (stride_f_mod *
((t / cycle_time_f) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time_f)))) -
(stride_f_mod / 2) +
stride_f_mod / 2) + initial_distance_f
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time_f / 2, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time_f) * t) +
((2 * h_f * t) / cycle_time_f) -
(h_f / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time_f / 2)
for t in np.arange(n, cycle_time_f, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time_f) * t)) -
((2 * h_f * t) / cycle_time_f) +
((3 * h_f) / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
x_fixed = xnew
y_fixed = ynew
#leg no.2
i = 0
t = 0
xnew = np.zeros([steps, 1], dtype=float)
for t in np.arange(0, cycle_time, sample_time):
xnew[i] = (stride_mod *
((t / cycle_time) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time)))) -
(stride_mod / 2) + stride_mod / 2) + initial_distance
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time / 2, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time) * t) + ((2 * h * t) / cycle_time) -
(h / 2)) + (h / 2) - initial_leg_height
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time / 2)
for t in np.arange(n, cycle_time, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time) * t)) - ((2 * h * t) / cycle_time) +
((3 * h) / 2)) + (h / 2) - initial_leg_height
i = i + 1
flaag = 1
i = 0
while (1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f) and i < steps:
last_fix = current
trans1, hip1, knee1 = gait.inverse_kinematics_3d_v6(
x_fixed[i], y_plane_f[i], y_fixed[i], 0,
legfix_Prev_angs[1], legfix_Prev_angs[2], L_f)
trans3, hip3, knee3 = gait.inverse_kinematics_3d_v6(
xnew[i], y_plane[i], ynew[i], 0, legvar_Prev_angs[1],
legvar_Prev_angs[2], L_v)
if flaag == 1:
trans4, hip4, knee4 = gait.generalbasemover_modifed(
4, 'f', st, steps)
trans2, hip2, knee2 = gait.generalbasemover_modifed(
2, 'f', st, steps)
flaag = 2
#Publish Fixed leg point
set_angle(0, trans1)
set_angle(1, hip1)
set_angle(2, knee1)
set_angle(6, trans3)
set_angle(7, hip3)
set_angle(8, knee3)
# Move Body
set_angle(3, trans2[i])
set_angle(4, hip2[i])
set_angle(5, knee2[i])
set_angle(9, trans4[i])
set_angle(10, hip4[i])
set_angle(11, knee4[i])
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start = 0
break
else: #even legs pair
initial_distance_f = leg_pos2_hip[0]
initial_distance = leg_pos4_hip[0]
legfix_Prev_angs = leg2_ang
legvar_Prev_angs = leg4_ang
y_offset_f = leg_pos2_hip[1] #leg2
y_offset = leg_pos4_hip[1] #leg4
y_plane_f = np.full((steps + 1, 1), y_offset_f)
y_plane = np.full((steps + 1, 1), y_offset)
L_f = 'r'
L_v = 'l'
if first_step_flag == 1:
if initial_distance_f >= 0:
stride_f_mod = stride_f - initial_distance_f
else:
stride_f_mod = stride_f + np.abs(initial_distance_f)
if initial_distance >= 0:
stride_mod = stride_f - initial_distance
else:
stride_mod = stride_f + np.abs(initial_distance)
first_step_flag = first_step_flag + 1
else:
error_f = stride_f / 2 - np.abs(initial_distance_f)
error = stride / 2 - np.abs(initial_distance)
if error_f >= 0:
stride_f_mod = stride_f - error_f
else:
stride_f_mod = stride_f + np.abs(error_f)
if error >= 0:
stride_mod = stride - error
else:
stride_mod = stride + np.abs(error)
#leg no.2
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
for t in np.arange(0, cycle_time_f, sample_time_f):
xnew[i] = (stride_f_mod *
((t / cycle_time_f) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time_f)))) -
(stride_f_mod / 2) +
stride_f_mod / 2) + initial_distance_f
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time_f / 2, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time_f) * t) +
((2 * h_f * t) / cycle_time_f) -
(h_f / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time_f / 2)
for t in np.arange(n, cycle_time_f, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time_f) * t)) -
((2 * h_f * t) / cycle_time_f) +
((3 * h_f) / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
x_fixed = xnew
y_fixed = ynew
#leg no.4
i = 0
t = 0
xnew = np.zeros([steps, 1], dtype=float)
for t in np.arange(0, cycle_time, sample_time):
xnew[i] = (stride_mod *
((t / cycle_time) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time)))) -
(stride_mod / 2) + stride_mod / 2) + initial_distance
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time / 2, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time) * t) + ((2 * h * t) / cycle_time) -
(h / 2)) + (h / 2) - initial_leg_height
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time / 2)
for t in np.arange(n, cycle_time, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time) * t)) - ((2 * h * t) / cycle_time) +
((3 * h) / 2)) + (h / 2) - initial_leg_height
i = i + 1
flaag = 1
i = 0
while (1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f) and i < steps:
last_fix = current
trans2, hip2, knee2 = gait.inverse_kinematics_3d_v6(
x_fixed[i], y_plane_f[i], y_fixed[i], 0,
legfix_Prev_angs[1], legfix_Prev_angs[2], L_f)
trans4, hip4, knee4 = gait.inverse_kinematics_3d_v6(
xnew[i], y_plane[i], ynew[i], 0, legvar_Prev_angs[1],
legvar_Prev_angs[2], L_v)
if flaag == 1:
trans3, hip3, knee3 = gait.generalbasemover_modifed(
3, 'f', st, steps)
trans1, hip1, knee1 = gait.generalbasemover_modifed(
1, 'f', st, steps)
flaag = 2
#Publish Fixed leg point
set_angle(3, trans2)
set_angle(4, hip2)
set_angle(5, knee2)
set_angle(9, trans4)
set_angle(10, hip4)
set_angle(11, knee4)
# Move Body
set_angle(0, trans1[i])
set_angle(1, hip1[i])
set_angle(2, knee1[i])
set_angle(6, trans3[i])
set_angle(7, hip3[i])
set_angle(8, knee3[i])
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start = 0
break
def Gate_Publisher(pair_no, st):
# Parameters:
global l1
global l2
global initial_leg_height
global initial_leg_height_f
global stride
global stride_f
global h
global h_f
global cycle_time
global cycle_time_f
global steps
global initial_distance
global initial_distance_f
global z
global sample_time_f
global leg_pos3_hip
global leg_pos4_hip
global leg_pos1_hip
global leg_pos2_hip
global leg_pos1__cg
global leg_pos2__cg
global leg_pos3__cg
global leg_pos4__cg
global leg1_ang
global leg2_ang
global leg3_ang
global leg4_ang
global x_fixed
global y_fixed
global var
global var2
global var3
global var4
global first_step_flag
flag_start = 0
x_current = 0
y_current = 0
x_current_f = 0
indx_fix = 0
t_left = 0
current = 0
last_fix = 0
sample_time = np.float(cycle_time) / steps # sample time
sample_time_f = np.float(cycle_time_f) / steps # sample time
legfix_final_cg = np.zeros([2, 1], dtype=float)
legvar_final_cg = np.zeros([2, 1], dtype=float)
stride = st
stride_f = st
if pair_no == 1: #odd number pair
initial_distance_f = leg_pos3_hip[0]
initial_distance = leg_pos1_hip[0]
legfix_Prev_angs = leg3_ang
legvar_Prev_angs = leg1_ang
legfix_initial_cg = leg_pos3__cg
legvar_initial_cg = leg_pos1__cg
if first_step_flag == 1:
if initial_distance_f >= 0:
stride_f_mod = stride_f - initial_distance_f
else:
stride_f_mod = stride_f + np.abs(initial_distance_f)
if initial_distance >= 0:
stride_mod = stride_f - initial_distance
else:
stride_mod = stride_f + np.abs(initial_distance)
first_step_flag = first_step_flag + 1
else:
error_f = stride_f / 2 - np.abs(initial_distance_f)
error = stride / 2 - np.abs(initial_distance)
if error_f >= 0:
stride_f_mod = stride_f - error_f
else:
stride_f_mod = stride_f + np.abs(error_f)
if error >= 0:
stride_mod = stride - error
else:
stride_mod = stride + np.abs(error)
#leg no.3
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
for t in np.arange(0, cycle_time_f, sample_time_f):
xnew[i] = (stride_f_mod *
((t / cycle_time_f) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time_f)))) -
(stride_f_mod / 2) +
stride_f_mod / 2) + initial_distance_f
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time_f / 2, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time_f) * t) +
((2 * h_f * t) / cycle_time_f) -
(h_f / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time_f / 2)
for t in np.arange(n, cycle_time_f, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time_f) * t)) -
((2 * h_f * t) / cycle_time_f) +
((3 * h_f) / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
x_fixed = xnew
y_fixed = ynew
#leg no.1
i = 0
t = 0
xnew = np.zeros([steps, 1], dtype=float)
for t in np.arange(0, cycle_time, sample_time):
xnew[i] = (stride_mod *
((t / cycle_time) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time)))) -
(stride_mod / 2) + stride_mod / 2) + initial_distance
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time / 2, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time) * t) + ((2 * h * t) / cycle_time) -
(h / 2)) + (h / 2) - initial_leg_height
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time / 2)
for t in np.arange(n, cycle_time, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time) * t)) - ((2 * h * t) / cycle_time) +
((3 * h) / 2)) + (h / 2) - initial_leg_height
i = i + 1
flaag = 1
i = 0
while (1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f) and i < steps:
last_fix = current
trans3, hip3, knee3 = gait.inverse_kinematics_3d_v6(
x_fixed[i], 112.75, y_fixed[i], 0, legfix_Prev_angs[1],
legfix_Prev_angs[2])
trans1, hip1, knee1 = gait.inverse_kinematics_3d_v6(
xnew[i], 112.75, ynew[i], 0, legvar_Prev_angs[1],
legvar_Prev_angs[2])
if flaag == 1:
trans4, hip4, knee4 = gait.generalbasemover_modifed(
4, 'f', st, steps)
trans2, hip2, knee2 = gait.generalbasemover_modifed(
2, 'f', st, steps)
flaag = 2
#Publish Fixed leg point
set_angle(0, trans1)
set_angle(1, hip1)
set_angle(2, knee1)
set_angle(6, trans3)
set_angle(7, hip3)
set_angle(8, knee3)
# Move Body
set_angle(3, trans2[i])
set_angle(4, hip2[i])
set_angle(5, knee2[i])
set_angle(9, trans4[i])
set_angle(10, hip4[i])
set_angle(11, knee4[i])
if (z == 1):
x_current = xnew[i]
y_current = ynew[i]
t_left = cycle_time_f - t
indx_fix = i + 1
break
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start = 0
break
if z == 1:
cycletime_required = t_left * 2
legfix_final_cg[0] = legfix_initial_cg[
0] + stride_f_mod # this is coord of the pt at end of traj for fixed leg
legvar_final_cg[0] = legvar_initial_cg[0] + stride_mod
legfix_final_cg[1] = legfix_initial_cg[1] # Must be relative to cg
legvar_final_cg[1] = legvar_initial_cg[1]
x_target_fix, x_target_var = Mod_contact2(1, legfix_final_cg,
legvar_final_cg)
#print("x target is "+ str(x_target))
trajectory_modification2(x_current, y_current, x_target_fix,
x_target_var, cycletime_required,
indx_fix, pair_no)
z = 0
else: #even legs pair
initial_distance_f = leg_pos4_hip[0]
initial_distance = leg_pos2_hip[0]
legfix_Prev_angs = leg4_ang
legvar_Prev_angs = leg2_ang
legfix_initial_cg = leg_pos4__cg
legvar_initial_cg = leg_pos2__cg
if first_step_flag == 1:
if initial_distance_f >= 0:
stride_f_mod = stride_f - initial_distance_f
else:
stride_f_mod = stride_f + np.abs(initial_distance_f)
if initial_distance >= 0:
stride_mod = stride_f - initial_distance
else:
stride_mod = stride_f + np.abs(initial_distance)
first_step_flag = first_step_flag + 1
else:
error_f = stride_f / 2 - np.abs(initial_distance_f)
error = stride / 2 - np.abs(initial_distance)
if error_f >= 0:
stride_f_mod = stride_f - error_f
else:
stride_f_mod = stride_f + np.abs(error_f)
if error >= 0:
stride_mod = stride - error
else:
stride_mod = stride + np.abs(error)
#leg no.4
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
for t in np.arange(0, cycle_time_f, sample_time_f):
xnew[i] = (stride_f_mod *
((t / cycle_time_f) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time_f)))) -
(stride_f_mod / 2) +
stride_f_mod / 2) + initial_distance_f
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time_f / 2, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time_f) * t) +
((2 * h_f * t) / cycle_time_f) -
(h_f / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time_f / 2)
for t in np.arange(n, cycle_time_f, sample_time_f):
ynew[i] = (-(h_f / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time_f) * t)) -
((2 * h_f * t) / cycle_time_f) +
((3 * h_f) / 2)) + (h_f / 2) - initial_leg_height_f
i = i + 1
x_fixed = xnew
y_fixed = ynew
#leg no.2
i = 0
t = 0
xnew = np.zeros([steps, 1], dtype=float)
for t in np.arange(0, cycle_time, sample_time):
xnew[i] = (stride_mod *
((t / cycle_time) -
((1 / (2 * np.pi)) * np.sin(2 * np.pi *
(t / cycle_time)))) -
(stride_mod / 2) + stride_mod / 2) + initial_distance
i = i + 1
i = 0
ynew = np.zeros([steps, 1], dtype=float)
# First part of Ynew in piecewise
for t in np.arange(0, cycle_time / 2, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(
((4 * np.pi) / cycle_time) * t) + ((2 * h * t) / cycle_time) -
(h / 2)) + (h / 2) - initial_leg_height
i = i + 1
# second part of Ynew in piecewise
n = (cycle_time / 2)
for t in np.arange(n, cycle_time, sample_time):
ynew[i] = (-(h / (2 * np.pi)) * np.sin(4 * np.pi - ((
(4 * np.pi) / cycle_time) * t)) - ((2 * h * t) / cycle_time) +
((3 * h) / 2)) + (h / 2) - initial_leg_height
i = i + 1
flaag = 1
i = 0
while (1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f) and i < steps:
last_fix = current
trans4, hip4, knee4 = gait.inverse_kinematics_3d_v6(
x_fixed[i], 112.75, y_fixed[i], 0, legfix_Prev_angs[1],
legfix_Prev_angs[2])
trans2, hip2, knee2 = gait.inverse_kinematics_3d_v6(
xnew[i], 112.75, ynew[i], 0, legvar_Prev_angs[1],
legvar_Prev_angs[2])
if flaag == 1:
trans3, hip3, knee3 = gait.generalbasemover_modifed(
3, 'f', st, steps)
trans1, hip1, knee1 = gait.generalbasemover_modifed(
1, 'f', st, steps)
flaag = 2
#Publish Fixed leg point
set_angle(3, trans2)
set_angle(4, hip2)
set_angle(5, knee2)
set_angle(9, trans4)
set_angle(10, hip4)
set_angle(11, knee4)
# Move Body
set_angle(0, trans1[i])
set_angle(1, hip1[i])
set_angle(2, knee1[i])
set_angle(6, trans3[i])
set_angle(7, hip3[i])
set_angle(8, knee3[i])
if (z == 1):
x_current = xnew[i]
y_current = ynew[i]
t_left = cycle_time_f - t
indx_fix = i + 1
break
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start = 0
break
if z == 1:
cycletime_required = t_left * 2
# legfix_final_cg = np.zeros([2, 1], dtype=float)
# legvar_final_cg = np.zeros([2, 1], dtype=float)
legfix_final_cg[0] = legfix_initial_cg[
0] + stride_f_mod # this is coord of the pt at end of traj for fixed leg
legvar_final_cg[0] = legvar_initial_cg[0] + stride_mod
legfix_final_cg[1] = legfix_initial_cg[1] # Must be relative to cg
legvar_final_cg[1] = legvar_initial_cg[1]
x_target_fix, x_target_var = Mod_contact2(2, legfix_final_cg,
legvar_final_cg)
#print("x target is "+ str(x_target))
trajectory_modification2(x_current, y_current, x_target_fix,
x_target_var, cycletime_required,
indx_fix, pair_no)
z = 0