def leg_ik(leg, claw, times={}):
"""
Takes:
leg: leg data object
claw: desired claw location in cylindrical coordinates in leg frame
Returns:
[h, e, k] list
If inputs are invalid, returns a list of length 4
"""
# Timing value
tv_leg_ik = time()
# Constants
hyp = ((claw[1] - leg.trolen)**2 + claw[2]**2)**.5
try:
# Inverse Kinematics
h = claw[0] - leg.gamma
e = helpers.rtod(
m.asin((claw[1] - leg.trolen) / hyp) +
m.acos((leg.tiblen**2 - leg.femlen**2 - hyp**2) /
(-2 * leg.femlen * hyp)) - (m.pi / 2)) - 90
k = helpers.rtod(m.pi -
m.acos((hyp**2 - leg.tiblen**2 - leg.femlen**2) /
(-2 * leg.tiblen * leg.femlen))) - 180
times = helpers.dict_timer("Ik.leg_ik", times, time() - tv_leg_ik)
return np.array([fix_angles_2(h), fix_angles_2(e), fix_angles_2(k)])
except Exception as e:
times = helpers.dict_timer("Ik.leg_ik", times, time() - tv_leg_ik)
print("Invalid parameters. Leg ik failed.")
return np.array([0, 1, 2,
3]) # Arbitray list of 4 to indicate ik failure
def set_target_positions(pos_list):
global times
if (len(pos_list) != len(IDS)):
print("pos_list doesn't have the same dimension as ids_list")
return -1
for i in range(len(pos_list)):
# write goal position
dxl_addparam_result = dynamixel.groupSyncWriteAddParam(
GROUP_NUM, IDS[i], pos_list[i] + HOME, LEN_MX_GOAL_POSITION)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
"""dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME)
dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
return -1
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return -1"""
tv_gswtp = time()
dynamixel.groupSyncWriteTxPacket(GROUP_NUM)
times = helpers.dict_timer("DT.groupSyncWriteTxPacket", times,
time() - tv_gswtp)
# if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS:
# dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION))
tv_gswcp = time()
dynamixel.groupSyncWriteClearParam(GROUP_NUM)
times = helpers.dict_timer("DT.groupSyncWriteClearParam", times,
time() - tv_gswcp)
def update_robot(body, current_state, dt):
global ctr
global t
global was_still
global times
# Timing value
tv_update_robot = time()
# read state
enable = bool(current_state["enable"])
vx = float(current_state["vx"])
vy = float(current_state["vy"])
omega = float(current_state["omega"])
height = float(current_state["height"])
pitch = float(current_state["pitch"])
roll = float(current_state["roll"])
yaw = float(current_state["yaw"])
pan = float(current_state["pan"])
tilt = float(current_state["tilt"])
home_wid = float(current_state["homewidth"])
home_len = float(current_state["homelength"])
# timestep = current_state[timestep]
stridelength = float(current_state["stridelength"])
raisefrac = float(current_state["raisefrac"])
raiseh = float(current_state["raiseh"])
lift_phase = float(current_state["liftphase"])
phase0 = float(current_state["phasefl"])
phase1 = float(current_state["phasebl"])
phase2 = float(current_state["phasebr"])
phase3 = float(current_state["phasefr"])
return_home = bool(current_state["gohome"])
# call timestep function
sleeptime, angles, t, was_still, times = gait_alg_test.timestep(
body, enable, return_home, vx, vy, omega, height, pitch, roll, yaw, t,
home_wid, home_len, dt, stridelength, raisefrac, raiseh, lift_phase,
[phase0, phase1, phase2, phase3], was_still, times)
angles.append(pan)
angles.append(tilt)
times = helpers.dict_timer("DT.update_robot", times,
time() - tv_update_robot)
# update servos accordingly: error check is that when given impossible values IK returns array of angles of incorrect length
if (len(angles) == 14):
# Timing print
tv_stp = time()
err = set_target_positions(check_angles(deg_to_dyn(angles)))
times = helpers.dict_timer("DT.set_target_positions", times,
time() - tv_update_robot)
if (ctr > num_iters):
ctr = 0
for k in times.keys():
print(k, "time: ", times[k] / num_iters)
times[k] = 0
print("\n")
ctr += 1
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 update_leg(state,
vx,
vy,
omega,
t,
lift_phase,
timestep,
stridelength,
raisefrac,
raiseh,
times={}):
"""Updates the state of a leg at each timestep, given state and robot velocity"""
# Timing value
tv_ul = time()
# The phase of the walking cycle is calculated
phase = (t + state.phase_offset * stridelength) % stridelength
# If the leg is being lifted
if phase < lift_phase:
state.home_offs[0] = -1 * (-vx + state.yawhomes[1] * omega) * (
stridelength * (1 - lift_phase) / 2)
state.home_offs[1] = -1 * (-vy - state.yawhomes[0] * omega) * (
stridelength * (1 - lift_phase) / 2)
# Move it horizontally towards the home position
state.loc[0] += calculate_step(state.loc[0],
state.yawhomes[0] + state.home_offs[0],
phase, lift_phase, timestep,
stridelength, times)
state.loc[1] += calculate_step(state.loc[1],
state.yawhomes[1] + state.home_offs[1],
phase, lift_phase, timestep,
stridelength, times)
# If it's being lifted
if phase < lift_phase * raisefrac:
# Lift it
state.loc[2] += calculate_step(state.loc[2], raiseh, phase,
lift_phase * raisefrac, timestep,
stridelength, times)
# If it's being lowered,
elif phase > lift_phase * (1 - raisefrac):
# Lower it
state.loc[2] += calculate_step(state.loc[2], 0, phase, lift_phase,
timestep, stridelength, times)
else:
# Otherwise, move it horizontally based on robot velocity
state.loc[0] += (-vx + state.loc[1] * omega) * timestep
state.loc[1] += (-vy - state.loc[0] * omega) * timestep
times = helpers.dict_timer("GAT.update_leg", times, time() - tv_ul)
def leg_ik(leg, claw, times={}):
# Timing value
tv_leg_ik = time()
# Constants
hyp = ((claw[1] - leg.trolen)**2 + claw[2]**2)**.5
try:
# Inverse Kinematics
h = claw[0] - leg.gamma
e = helpers.rtod(m.asin((claw[1] - leg.trolen)/hyp) + m.acos((leg.tiblen**2 - leg.femlen**2 - hyp**2)/(-2 * leg.femlen * hyp)) - (m.pi/2)) - 90
k = helpers.rtod(m.pi - m.acos((hyp**2 - leg.tiblen**2 - leg.femlen**2)/(-2 * leg.tiblen * leg.femlen))) - 180
times = helpers.dict_timer("Ik.leg_ik", times, time()-tv_leg_ik)
return np.array([fix_angles_2(h), fix_angles_2(e), fix_angles_2(k)])
except:
times = helpers.dict_timer("Ik.leg_ik", times, time()-tv_leg_ik)
print("Invalid parameters. Leg ik failed.")
return np.array([0, 1, 2, 3])
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 calculate_step(cur,
goal,
phase,
endphase,
timestep,
stridelength,
times={}):
"""Calculate the next step to reach a goal at a certain time"""
# Timing value
tv_cs = time()
# Check if the end phase is very close or past
if phase > endphase - macros.TOLERANCE:
# If so, go to the goal right away
return goal - cur
times = helpers.dict_timer("GAT.calculate_step", times, time() - tv_cs)
# Otherwise, go at the speed that will reach the goal at the target time
return (goal - cur) * timestep / (stridelength * (endphase - phase))
def extract_angles(body,
claws,
pitch=macros.DEFAULT_PITCH,
roll=macros.DEFAULT_ROLL,
height=macros.DEFAULT_HEIGHT,
zs=[0, 0, 0, 0],
times={}):
"""
Takes:
body: body_data object
claws: list of claw positions
pitch: desired pitch in degrees
roll: desired roll in degrees
height: desired height
zs: (Don't really know)
times: (Don't really know)
Returns:
list of angles [h1, e1, k1, h2, e2, k2, h3, e3, k3, h4, e4, k4]
"""
# Timing value
tv_ea = time()
# Returns np.array
# newclaws = body_ik(body, claws, pitch, roll, height, zs, times)
# ret_angles = np.array([])
# for i in range(len(body.legs)):
# ret_angles = np.append(ret_angles, (leg_ik(body.legs[i], newclaws[i], times)))
# Returns list
newclaws = body_ik(body, claws, pitch, roll, height, zs, times)
ret_angles = []
for i in range(len(body.legs)):
toadd = leg_ik(body.legs[i], newclaws[i], times)
for j in toadd:
ret_angles.append(j)
times = helpers.dict_timer("Ik.extract_angles", times, time() - tv_ea)
return ret_angles
def extract_angles(body, claws, pitch=macros.DEFAULT_PITCH, roll=macros.DEFAULT_ROLL, height=macros.DEFAULT_HEIGHT, zs=[0,0,0,0], times={}):
# Timing value
tv_ea = time()
# Returns np.array
# newclaws = body_ik(body, claws, pitch, roll, height, zs, times)
# ret_angles = np.array([])
# for i in range(len(body.legs)):
# ret_angles = np.append(ret_angles, (leg_ik(body.legs[i], newclaws[i], times)))
# Returns list
newclaws = body_ik(body, claws, pitch, roll, height, zs, times)
ret_angles = []
for i in range(len(body.legs)):
toadd = leg_ik(body.legs[i], newclaws[i], times)
for j in toadd:
ret_angles.append(j)
times = helpers.dict_timer("Ik.extract_angles", times, time()-tv_ea)
return ret_angles
vx = 0
vy = 0
omega = 0
height = 8
pitch = 0
roll = 0
yaw = 20 * m.sin(t)
home_wid = 9
home_len = 9
sleeptime, angles, t, was_still, times = gait_alg.timestep(
body, vx, vy, omega, height, pitch, roll, yaw, t, home_wid, home_len,
was_still, times)
times = helpers.dict_timer("Cont.update_robot", times,
time() - tv_update_robot)
if (len(angles) == 12):
tv_servosend = time()
client.send(hlsockets.SERVO, quick_fix_angles(angles))
times = helpers.dict_timer("Cont.servosend", times,
time() - tv_servosend)
sleep(sleeptime)
# if (ctr > num_iters):
# ctr = 0
# for k in times.keys():
# print(k, "time: ", times[k]/num_iters)
# times[k]=0
# print("\n")
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)
