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!":