예제 #1
0
            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)