def desired_xy(transverse, hip, knee, legno):
from Main_Functions import GP2_Function_V7 as gait1
msg = Float32MultiArray()
x, y, z = gait1.forward_kinematics_V3(transverse, hip, knee)
msg.data = [float(x), float(z), legno]
pub2 = rospy.Publisher('desired', Float32MultiArray)
pub2.publish(msg)
def fwd_kin(data):
global flag
global legspos2cg
global legspos2joint
global all_data
global Fwd_node
global flatten_list
global onetime
# if(flag):w
# time.sleep(1)
# flag=False
# print(Fwd_node)
all_angels = data.data[:12]
hipangles = np.zeros((4, 1))
kneeangles = np.zeros((4, 1))
transverseangles = np.zeros((4, 1))
for i in range(hipangles.shape[0]):
transverseangles[i] = all_angels[0 + 3 * i]
hipangles[i] = all_angels[1 + 3 * i]
kneeangles[i] = all_angels[2 + 3 * i]
pos2cg, pos2joint = Alg.GetEndEffectorPos(transverseangles, hipangles,
kneeangles)
#print('pos2joint is'+str(pos2joint))
#legspos2cg.append(pos2cg)
legspos2joint = list(chain.from_iterable(pos2joint))
legspos2cg = list(chain.from_iterable(pos2cg))
all_data = legspos2joint + legspos2cg
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 trajectory_modification(x_current, y_current, x_target, cycle_time,
legfix_Prev_angs, legvar_Prev_angs):
global l1
global l2
global steps
global initial_leg_height
global var
global var2
global var3
global var4
global indx_fix
global x_fixed
global y_fixed
stride = (x_target - x_current) * 2
h = y_current # maximum height of the trajectory
x_initial = x_target - stride
initial_distance = x_initial
sample_time = cycle_time / steps # sample time, steps should be even
xnew = np.zeros([steps / 2, 1], dtype=float)
ynew = np.zeros([steps / 2, 1], dtype=float)
t = 0
i = 0
#i_f= steps/2
n = (cycle_time / 2)
current = 0
last_fix = 0
last_var = 0
while (1):
current = time.time() #absolute
if ((current - last_fix) >
sample_time_f) and indx_fix < steps: #Publish Fixed leg point
last_fix = current
trans, hip, knee = gait.inverse_kinematics_3d_v6(
x_fixed[indx_fix], 112.75, y_fixed[indx_fix], 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)
#rospy.loginfo(x_fixed[indx_fix])
#pub.publish(x_fixed[indx_fix])
#rospy.loginfo(y_fixed[indx_fix])
#pub.publish(y_fixed[indx_fix])
indx_fix = indx_fix + 1
if ((current - last_var) >
sample_time) and i < steps / 2: #Publish Variable leg point
last_var = 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
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
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)
#rospy.loginfo(xnew[i])
#pub.publish(xnew[i])
#rospy.loginfo(ynew[i])
#pub.publish(ynew[i])
i = i + 1
t = t + sample_time
if (indx_fix == steps) and i == steps / 2:
#print("done")
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
################### MAIN #####################
## el 2 lines dol 3alashan el sudden motion ale by3melha fel awel
#gait.Cheetah_Wakeup()
#gait.Cheetah_Sleep()
# gait.onestepcreeping('f',120)
# time.sleep(0.5)
# gait.Body_mover_To_point(0,gait.a,-gait.initalheight,0.01) # el line dah byraga3 el joints lel initial position
# time.sleep(0.5)
# gait.trial()
#20 may 2020
# time.sleep(0.5)
# gait.initialize()
for i in range(3):
gait.trot3_adjusted_fwd_kin(1, 'f', 100)
time.sleep(0.5)
gait.trot3_adjusted_fwd_kin(2, 'f', 100)
time.sleep(0.5)
# for i in range(5):
# gait.Move_side("l")
# time.sleep(0.5)
# transverses , hips, knees = gait.getjointanglesfromvrep()
# legspos2cg,legspos2joint=gait.GetEndEffectorPos(transverses,hips,knees)#
# print(legspos2cg)
# print(legspos2joint)
# 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
# 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
# xnew_multi = np.append(xnew_multi, xnew , axis=1)
# ynew_multi = np.append(ynew_multi, ynew , axis=1)
# xnew_multi=np.transpose(xnew_multi)
# ynew_multi=np.transpose(ynew_multi)
# #print(xnew_multi)
# l=len(xnew_multi[0])
# for j in range(l):
# x=xnew_multi[j]
# y=ynew_multi[j]
# plt.plot(x,y)
# plt.show()
result2=gait.forward_kinematics_V3(0,-2.099,1.108)
result0=gait.forward_kinematics_V3(0,-2.058,1.071)
result1=gait.forward_kinematics_V3(0,-2.037,1.140)
print("---------------------")
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 move_leg(leg_no):
# Parameters:
global l1
global l2
global initial_leg_height
global stride
global h
global cycle_time
global steps
global initial_distance
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
global indx_fix
flag_start =0
x_current = 0
y_current = 0
x_current_f =0
#sample_time = np.float(cycle_time) / steps # sample time
sample_time_f =np.float(cycle_time) / steps # sample time
t_left = 0
current = 0
last_fix = 0
if leg_no == 1:
initial_distance =leg_pos1_hip[0]
elif leg_no == 2:
initial_distance =leg_pos2_hip[0]
elif leg_no == 3:
initial_distance =leg_pos3_hip[0]
elif leg_no == 4:
initial_distance =leg_pos4_hip[0]
xnew = np.zeros([steps, 1], dtype=float)
t = 0
i = 0
for t in np.arange(0, cycle_time, sample_time_f):
xnew[i] = (stride * ((t / cycle_time) - ((1 / (2 * np.pi)) * np.sin(2 * np.pi * (t / cycle_time)))) - (stride / 2) + stride / 2) + initial_distance
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 / 2, sample_time_f):
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
n = (cycle_time / 2)
for t in np.arange(n, cycle_time, sample_time_f):
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
ynew = ynew
x_fixed = xnew
y_fixed = ynew
if leg_no == 1:
legfix_Prev_angs = leg1_ang
elif leg_no ==2:
legfix_Prev_angs = leg2_ang
elif leg_no ==3:
legfix_Prev_angs = leg3_ang
elif leg_no ==4:
legfix_Prev_angs = leg4_ang
i=0
while(1):
current = time.time() #absolute
if ((current - last_fix) > sample_time_f ) and i < steps:
last_fix = current
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] )
#Publish Fixed leg point
var = leg_no - 1
set_angle((var*3),trans)
set_angle((var*3)+1 , hip)
set_angle(((var*3)+2), knee)
i = i + 1
t = t + sample_time_f
if (i == steps):
flag_start =0
break
def trajectory_modification(x_current, y_current, x_target, cycle_time,
indx_fix, pair_no):
global l1
global l2
global steps
global initial_leg_height
global h
global sample_time_f
global x_fixed
global y_fixed
global leg1_ang
global leg2_ang
global leg3_ang
global leg4_ang
stride = (x_target - x_current) * 2
h = y_current # maximum height of the trajectory
initial_distance = x_target - stride
sample_time = cycle_time / steps # sample time, steps should be even
xnew = np.zeros([steps / 2, 1], dtype=float)
ynew = np.zeros([steps / 2, 1], dtype=float)
t = (cycle_time / 2)
i = 0
current = 0
last_fix = 0
last_var = 0
if pair_no == 1: #odd number pair
varF = 0
varV = 2
legfix_Prev_angs = leg3_ang
legvar_Prev_angs = leg1_ang
else:
varF = 1
varV = 3
legfix_Prev_angs = leg4_ang
legvar_Prev_angs = leg2_ang
while (1):
current = time.time() #absolute
if ((current - last_fix) >
sample_time_f) and indx_fix < steps: #Publish Fixed leg point
last_fix = current
trans, hip, knee = gait.inverse_kinematics_3d_v6(
x_fixed[indx_fix], 112.75, y_fixed[indx_fix], 0,
legfix_Prev_angs[1], legfix_Prev_angs[2])
set_angle((varF * 3), trans)
set_angle((varF * 3) + 1, hip)
set_angle(((varF * 3) + 2), knee)
indx_fix = indx_fix + 1
if ((current - last_var) >
sample_time) and i < steps / 2: #Publish Variable leg point
last_var = 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
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
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((varV * 3), trans)
set_angle((varV * 3) + 1, hip)
set_angle(((varV * 3) + 2), knee)
i = i + 1
t = t + sample_time
if (indx_fix == steps) and i == steps / 2:
print("Modification Done !!")
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
