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