def move_leg(leg_no, x, y, z): 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 global cycle_time global steps sample_time = np.float(cycle_time) / steps # sample time current = 0 last_fix = 0 t = 0 i = 0 if leg_no == 1: legg = 0 elif leg_no == 3: legg = 2 elif leg_no == 4: legg = 3 elif leg_no == 2: legg = 1 while (1): current = time.time() #absolute if ((current - last_fix) > sample_time) and i < steps: last_fix = current send_ik_point(x[i], y[i], z[i], legg) i = i + 1 t = t + sample_time
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
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
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