def make_standard_bot(side=macros.SIDE, trolen=macros.TROLEN, femlen=macros.FEMLEN, tiblen=macros.TIBLEN, zdist=macros.ZDIST):
# Timing value
tv_msb = time()
claws = []
legs = []
poly_rad = side/(2 * m.sin(m.pi/macros.NUMLEGS)) # polygon radius
for i in range(macros.NUMLEGS):
# calculate unit multipliers
x_unit = m.cos(helpers.dtor(macros.GAMMAS[i]))
y_unit = m.sin(helpers.dtor(macros.GAMMAS[i]))
# append appropriate data to claws and legs
claws.append([(x_unit * (poly_rad + macros.DEFAULT_RADIUS)), (y_unit * (poly_rad + macros.DEFAULT_RADIUS))])
legs.append(leg_data(i, x_unit * poly_rad, y_unit * poly_rad, macros.GAMMAS[i], trolen, femlen, tiblen))
if (i == 0):
legs[i].state.signs = np.array([1, 1])
elif (i == 1):
legs[i].state.signs = np.array([-1, 1])
elif (i == 2):
legs[i].state.signs = np.array([-1, -1])
else:
legs[i].state.signs = np.array([1, -1])
# create a body with the legs
body = body_data(legs, side, zdist, trolen, femlen, tiblen)
# Timing print
print("Make_standard_bot time: ", (time() - tv_msb))
return(claws, body)
def body_ik(body, claws, pitch, roll, height, zs, times={}):
# Timing value
tv_body_ik = time()
# copy claws to prevent error propagation and add z coordinate
# hclaws = copy.copy(claws)
# create rotation matrix
pc, ps, rc, rs = m.cos(helpers.dtor(pitch)), m.sin(helpers.dtor(pitch)), m.cos(helpers.dtor(roll)), m.sin(helpers.dtor(roll))
pitchm = np.matrix([[pc, 0, -ps], [0, 1, 0], [ps, 0, pc]])
rollm = np.matrix([[1, 0, 0], [0, rc, -rs], [0, rs, rc]])
rot = pitchm * rollm
# put leg offsets through rotation and subtract difference
newclaws = []
for i in range(len(body.legs)):
claws[i] = np.append(claws[i], -1 * height)
vec = rot * body.legs[i].off[np.newaxis].T
newclaws.append(helpers.tocyl(claws[i] - np.squeeze(np.asarray(vec))))
# incorporate leg lifts
for i in range(len(zs)):
if (zs[i] > 0):
newclaws[i][2] = -1*(height-zs[i])
times = helpers.dict_timer("Ik.body_ik", times, time()-tv_body_ik)
return newclaws
def body_ik(body, claws, pitch, roll, height, zs, times={}):
"""
Takes:
body: body.legs contains the list of leg data objects
claws: list of desired claw locations in floor plane
pitch: desired pitch of robot
roll: desired roll of robot
height: desired height of robot
zs: (Don't really know)
times: (Don't really know)
Returns:
newclaws: list of desired claw positions in cylindrical coordinates in leg frame of rotated robot
"""
# Timing value
tv_body_ik = time()
# copy claws to prevent error propagation and add z coordinate
# hclaws = copy.copy(claws)
# create rotation matrix
pc, ps, rc, rs = m.cos(helpers.dtor(pitch)), m.sin(
helpers.dtor(pitch)), m.cos(helpers.dtor(roll)), m.sin(
helpers.dtor(roll))
pitchm = np.matrix([[pc, 0, -ps], [0, 1, 0], [ps, 0, pc]])
rollm = np.matrix([[1, 0, 0], [0, rc, -rs], [0, rs, rc]])
rot = pitchm * rollm
# put leg offsets through rotation and subtract difference
newclaws = []
for i in range(len(body.legs)):
claws[i] = np.append(claws[i], -1 * height)
vec = rot * body.legs[i].off[np.newaxis].T
newclaws.append(helpers.tocyl(claws[i] - np.squeeze(np.asarray(vec))))
# incorporate leg lifts
for i in range(len(zs)):
if (zs[i] > 0):
newclaws[i][2] = -1 * (height - zs[i])
times = helpers.dict_timer("Ik.body_ik", times, time() - tv_body_ik)
return newclaws
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))
DEFAULT_RADIUS = 8
# Default pitch and roll values (degrees)
DEFAULT_PITCH = 0
DEFAULT_ROLL = 0
# Default yaw value
DEFAULT_YAW = 0
# Default home positions of legs in body frame
HOMES = []
poly_rad = SIDE / (2 * m.sin(m.pi / NUMLEGS)) # polygon radius
clawdist_from_center = poly_rad + DEFAULT_RADIUS # claw distance from center of robot
for i in range(NUMLEGS):
HOMES.append([
clawdist_from_center * m.cos(helpers.dtor(GAMMAS[i])),
clawdist_from_center * m.sin(helpers.dtor(GAMMAS[i])),
-1 * DEFAULT_HEIGHT
])
# Default velocities for walking
DEFAULT_VX = 1
DEFAULT_VY = 0
DEFAULT_OMEGA = 40
DEFAULT_PAN = 0
DEFAULT_TILT = -30
# # # # # # # # # # # # #
# # # # IK MACROS # # # #
# # # # # # # # # # # # #
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)
# Default claw distance from hip (centimeters)
DEFAULT_RADIUS = 8
# Default pitch and roll values (degrees)
DEFAULT_PITCH = 0
DEFAULT_ROLL = 0
# Default yaw value
DEFAULT_YAW = 0
# Default home positions of legs in body frame
HOMES = []
poly_rad = SIDE/(2 * m.sin(m.pi/NUMLEGS)) # polygon radius
clawdist_from_center = poly_rad + DEFAULT_RADIUS # claw distance from center of robot
for i in range(NUMLEGS):
HOMES.append([clawdist_from_center * m.cos(helpers.dtor(GAMMAS[i])),
clawdist_from_center * m.sin(helpers.dtor(GAMMAS[i])), -1 * DEFAULT_HEIGHT])
# Default velocities for walking
DEFAULT_VX = 1
DEFAULT_VY = 0
DEFAULT_OMEGA = 40
# # # # # # # # # # # # #
# # # # IK MACROS # # # #
# # # # # # # # # # # # #
# Bounds on heights of claw