sendangle = float(angle) msg = str(joint) + ' ' + str(sendangle) pub.publish(msg) #####################################################################################################################3 # Main if __name__ == '__main__': rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('setter', String, queue_size=10) gait.ros.ros_init(1) rospy.Subscriber('fwd', Float32MultiArray, leg_pos) rospy.Subscriber('getter', Float32MultiArray, imudata) time.sleep(2) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): legfix_initial_hip = leg_pos4_hip[0] legvar_initial_hip = leg_pos2_hip[0] legfix_initial_cg = leg_pos4_cg legvar_initial_cg = leg_pos2_cg leg4_Prev_angs = leg4_ang leg2_Prev_angs = leg2_ang #time.sleep(1) Gate_Publisher(4, legfix_initial_hip, legvar_initial_hip, legfix_initial_cg, legvar_initial_cg, leg4_Prev_angs, leg2_Prev_angs) Gate_Publisher(2, legfix_initial_hip, legvar_initial_hip,
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 set_angle(joint, angle): global pub msg=str(joint) + ' ' + str(angle) sendangle = float(angle) msg = str(joint)+ ' ' + str(sendangle) pub.publish(msg) ##################################################################################################################### # Main if __name__ == '__main__': rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('setter', String, queue_size=10) gait.ros.ros_init(1) rospy.Subscriber('fwd', Float32MultiArray, leg_pos) rospy.Subscriber('getter', Float32MultiArray , imudata) time.sleep(2) rate = rospy.Rate(100) # 10hz first_step_flag=1 while not rospy.is_shutdown(): if first_step_flag == 1: legfix_initial_hip = leg_pos4_hip[0] legvar_initial_hip = leg_pos2_hip[0] legfix_initial_cg = leg_pos4_cg legvar_initial_cg = leg_pos2_cg time.sleep(delay_seq) Gate_Publisher(4 ,legfix_initial_hip,legvar_initial_hip,legfix_initial_cg,legvar_initial_cg,150) Gate_Publisher(2 ,legfix_initial_hip,legvar_initial_hip,legfix_initial_cg,legvar_initial_cg,150)
#####################################################################################################################3 # Main if __name__ == '__main__': global pub rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('setter', String, queue_size=10) gait.ros.ros_init(1) rospy.Subscriber('fwd', Float32MultiArray, leg_pos) rospy.Subscriber('getter', Float32MultiArray, imudata) time.sleep(2) rate = rospy.Rate(10) # 10hz global leg_pos3_hip global leg_pos4_hip global leg_pos1_hip global leg_pos2_hip global leg_pos3_cg global leg_pos4_cg global leg_pos1_cg global leg_pos2_cg global leg3_ang global leg4_ang global leg1_ang
def init_angles(): set_angle(0, 0) time.sleep(0.1) set_angle(1, hipinit) time.sleep(0.1) set_angle(2, kneeinit) time.sleep(0.1) set_angle(3, 0) time.sleep(0.1) set_angle(4, hipinit) time.sleep(0.1) set_angle(5, kneeinit) time.sleep(0.1) set_angle(6, 0) time.sleep(0.1) set_angle(7, hipinit) time.sleep(0.1) set_angle(8, kneeinit) time.sleep(0.1) set_angle(9, 0) time.sleep(0.1) set_angle(10, hipinit) time.sleep(0.1) set_angle(11, kneeinit) time.sleep(0.1) print("Angles initialized")
def ros_init_gaz(): #rospy.init_node('talker', anonymous=True) global ab1_pub global bc1_pub global cd1_pub global ab2_pub global bc2_pub global cd2_pub global ab3_pub global bc3_pub global cd3_pub global ab4_pub global bc4_pub global cd4_pub ab1_pub = rospy.Publisher( '/SimplifiedAssembly/ab1_position_controller/command', Float64, queue_size=1) bc1_pub = rospy.Publisher( '/SimplifiedAssembly/bc1_position_controller/command', Float64, queue_size=1) cd1_pub = rospy.Publisher( '/SimplifiedAssembly/cd1_position_controller/command', Float64, queue_size=1) ab2_pub = rospy.Publisher( '/SimplifiedAssembly/ab2_position_controller/command', Float64, queue_size=1) bc2_pub = rospy.Publisher( '/SimplifiedAssembly/bc2_position_controller/command', Float64, queue_size=1) cd2_pub = rospy.Publisher( '/SimplifiedAssembly/cd2_position_controller/command', Float64, queue_size=1) ab3_pub = rospy.Publisher( '/SimplifiedAssembly/ab3_position_controller/command', Float64, queue_size=1) bc3_pub = rospy.Publisher( '/SimplifiedAssembly/bc3_position_controller/command', Float64, queue_size=1) cd3_pub = rospy.Publisher( '/SimplifiedAssembly/cd3_position_controller/command', Float64, queue_size=1) ab4_pub = rospy.Publisher( '/SimplifiedAssembly/ab4_position_controller/command', Float64, queue_size=1) bc4_pub = rospy.Publisher( '/SimplifiedAssembly/bc4_position_controller/command', Float64, queue_size=1) cd4_pub = rospy.Publisher( '/SimplifiedAssembly/cd4_position_controller/command', Float64, queue_size=1) rospy.Subscriber('/SimplifiedAssembly/joint_states', JointState, angles) time.sleep(0.5) print("DONEE")
i = i + 1 #t = t + sample_time_f if (i == steps): flag_start =0 break ##################################################################################################################### # Main if __name__ == '__main__': rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('setter', String, queue_size=10) gait.ros.ros_init(1) rospy.Subscriber('fwd', Float32MultiArray, leg_pos) rospy.Subscriber('getter', Float32MultiArray , imudata) time.sleep(2) rate = rospy.Rate(100) # 10hz first_step_flag=1 while not rospy.is_shutdown(): if first_step_flag == 1: move_leg(3) time.sleep(1.2) move_leg(1) time.sleep(1.2) move_leg(4) time.sleep(1.2) move_leg(2) time.sleep(1.2) move_base() first_step_flag =0
total.data = [] arr = [x, y, z, leg] total.data = arr pub2.publish(total) ##################################################################################################################### # Main if __name__ == '__main__': rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('setter', String, queue_size=10) gait.ros.ros_init(1) rospy.Subscriber('fwd', Float32MultiArray, leg_pos) rospy.Subscriber('getter', Float32MultiArray, imudata) pub2 = rospy.Publisher('ik_setter', Float32MultiArray, queue_size=10) time.sleep(2) rate = rospy.Rate(100) # 10hz first_step_flag = 1 while not rospy.is_shutdown(): if first_step_flag == 1: move_leg(4) # time.sleep(0.5) # move_leg(1) # time.sleep(0.5) # move_leg(4) # time.sleep(0.5) # move_leg(2) # time.sleep(0.5) # move_base() first_step_flag = 0
ab3_pub = rospy.Publisher( '/SimplifiedAssembly/ab3_position_controller/command', Float64, queue_size=1) bc3_pub = rospy.Publisher( '/SimplifiedAssembly/bc3_position_controller/command', Float64, queue_size=1) cd3_pub = rospy.Publisher( '/SimplifiedAssembly/cd3_position_controller/command', Float64, queue_size=1) ab4_pub = rospy.Publisher( '/SimplifiedAssembly/ab4_position_controller/command', Float64, queue_size=1) bc4_pub = rospy.Publisher( '/SimplifiedAssembly/bc4_position_controller/command', Float64, queue_size=1) cd4_pub = rospy.Publisher( '/SimplifiedAssembly/cd4_position_controller/command', Float64, queue_size=1) rate = rospy.Rate(100) # 10hz time.sleep(0.5) init_angles() while not rospy.is_shutdown(): rate.sleep()
def delay_print(s): for c in s: sys.stdout.write(c) sys.stdout.flush() time.sleep(0.1)
9. Wackycks are the currency of the game, 1 WK = 1,000 $ 10.If you want to throw, there is a cooldown of 4 turns !NOW ENJOY! ''') print("\n") print("\n") print("\n") print("\n") print("\n") print("\n") time.sleep(0) # Settings username = input("What shall be your name?") while True: gender = input( "What gender do you want your character to be? (remenber that you can only chose boy or girl...)" ) if gender == "boy" or gender == "Boy" or gender == "BOY" or gender == "girl" or gender == "Girl" or gender == "GIRL": print("\n") break elif gender == "e": player.inventory(gender) continue elif gender == "Hey god give me some of your powers!":