def init_robot():
# Timing value
tv_init = time()
err = init_motors([3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14], [512] * 12)
# print("initiated motors")
claws, body = ik.make_standard_bot()
angles = ik.extract_angles(body, claws, 0, 0, 12)
angles = deg_to_dyn(angles)
# Timing print
print("Init_robot time: ", (time() - tv_init), "\n")
return (body)
def timestep(body,
vx,
vy,
omega,
t,
lift_phase=macros.LIFT_PHASE,
pitch=macros.DEFAULT_PITCH,
roll=macros.DEFAULT_ROLL,
height=macros.DEFAULT_HEIGHT,
yaw=macros.DEFAULT_YAW):
"""Updates the states of every leg for a given robot body, given state and robot velocity"""
xys = []
zs = []
# yawc, yaws = m.cos(helpers.dtor(yaw)), m.sin(helpers.dtor(yaw))
# for i in range(len(body.legs)):
# home_x, home_y = body.legs[i].state.home_x, body.legs[i].state.home_y
# body.legs[i].state.yawhomes = [home_x * yawc - home_y * yaws, home_x * yaws + home_y * yawc]
# if the robot should be stepping
if ((m.sqrt(vx**2 + vy**2) > macros.MIN_V) or (omega > macros.MIN_OMEGA)):
# Add to variables
for i in range(len(body.legs)):
update_leg(body.legs[i].state, vx, vy, helpers.dtor(omega), t,
lift_phase)
xys.append([body.legs[i].state.loc[0], body.legs[i].state.loc[1]])
zs.append(body.legs[i].state.loc[2])
# else, if the claws should be static
else:
for i in range(len(body.legs)):
xys.append(body.legs[i].state.yawhomes)
zs.append(0)
# pretty sure this line is unnecessary because it's handled on the other side of the function call
#t += macros.TIMESTEP
# Return formatted array of angles
return (macros.TIMESTEP,
ik.extract_angles(body, xys, pitch, roll, height, zs))
def timestep(body, enable, return_home, vx, vy, omega, height, pitch, roll,
yaw, t, home_wid, home_len, timestep, stridelength, raisefrac,
raiseh, lift_phase, phases, was_still, times):
"""Updates the states of every leg for a given robot body, given state and robot velocity"""
# Timing value
tv_timestep = time()
timestep = macros.TIMESTEP
# Update leg states
for i in range(len(body.legs)):
body.legs[i].state.phase_offset = phases[i]
body.legs[i].state.homes[0] = body.legs[i].state.signs[0] * home_len
body.legs[i].state.homes[1] = body.legs[i].state.signs[1] * home_wid
# Variables to track change in state
xys = []
zs = []
# Manage yawing
yc, ys = m.cos(helpers.dtor(yaw)), m.sin(helpers.dtor(yaw))
yawm = np.matrix([[yc, -ys], [ys, yc]])
for i in range(len(body.legs)):
body.legs[i].state.yawhomes = np.squeeze(
np.asarray(yawm * body.legs[i].state.homes[np.newaxis].T))
# yawc, yaws = m.cos(helpers.dtor(yaw)), m.sin(helpers.dtor(yaw))
# for i in range(len(body.legs)):
# home_x, home_y = body.legs[i].state.homes[0], body.legs[i].state.homes[1]
# body.legs[i].state.yawhomes = [home_x * yawc - home_y * yaws, home_x * yaws + home_y * yawc]
# If walking is enabled and the robot is at the minimum required velocity
if (enable and ((m.sqrt(vx**2 + vy**2) >= macros.MIN_V) or
(abs(omega) >= macros.MIN_OMEGA))):
# If robot was still immediately before this timestep
if (was_still):
t = 0
was_still = False
# Update each leg
for i in range(len(body.legs)):
update_leg(body.legs[i].state, vx, vy, helpers.dtor(omega), t,
lift_phase, timestep, stridelength, raisefrac, raiseh,
times)
xys.append([body.legs[i].state.loc[0], body.legs[i].state.loc[1]])
zs.append(body.legs[i].state.loc[2])
# Else the claws should be static
else:
was_still = True
for i in range(len(body.legs)):
xys.append(body.legs[i].state.yawhomes)
zs.append(0)
# Increment timestep
t += timestep
times = helpers.dict_timer("GAT.timestep", times, time() - tv_timestep)
ret_angles = ik.extract_angles(body, xys, pitch, roll, height, zs, times)
# Return formatted array of angles
return (timestep, ret_angles, t, was_still, times)
angles[i] *= 1 / .29
angles[i] = int(angles[i])
return (angles)
def walk(vx, vy, omega, time=10):
t = 0
while (t < time):
sleeptime, angles = gait_alg.timestep(body, vx, vy, omega, t)
t += sleeptime
err = set_target_positions(deg_to_dyn(angles))
print(angles)
sleep(sleeptime)
err = init_motors([3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14], [512] * 12)
print("initiated motors")
claws, body = ik.make_standard_bot()
angles = ik.extract_angles(body, claws, 0, 0, 12)
angles = deg_to_dyn(angles)
walk(20, 0, -20, 100)
t = 0
while (1):
bedtime = time()
target = ik.extract_angles(body, claws, 30 * sin(3 * t), 30 * cos(3 * t),
12 + 3 * cos(20 * t))
print("%.2f " * len(target) % tuple(target))
set_target_positions(deg_to_dyn(target))
sleep(.01)
t += time() - bedtime