success = False
break
if (np.linalg.norm(dv[:6]) >= 1e3):
print "Slipped and fell."
success = False
break
SOL.append(sol_fullqp)
data.q = q + noise_q
data.v = v + noise_v
data.noise_q = noise_q
data.noise_v = noise_v
data.tau_fullqp = tau
data.dv_fullqp = dv
data.activeset_fullqp = sol_fullqp.activeSet
# Perform onboard computation around contact switches
if LOCAL and step * STEP_DURATION - ONBOARD_WINDOW <= i < step * STEP_DURATION + ONBOARD_WINDOW:
# when on-board, only compute full qp every ONBOARD_FREQ ms
if i % ONBOARD_FREQ == 0:
delay = 2 + int(1000 * data.fullqp_time)
# 2 ms extra delay for local controller (<= 1ms)
# and one-time pre-computation spread out over the whole window (possible with multithreading)
for h in range(ONBOARD_FREQ):
# check if this delay < network delay
if h < delay:
delayed_ids[i + h] = np.maximum(
delayed_ids[i + h], i - ONBOARD_FREQ)
if h >= delay:
delayed_ids[i + h] = np.maximum(delayed_ids[i + h], i)