test_qpOASES_solver_funcs.py 文件源码

def test_comp_topp(self, pp_fixture):
        pcs, solver = pp_fixture
        I0 = np.r_[0.1, 0.2]
        IN = np.r_[0.1, 0.2]
        solver.set_start_interval(I0)
        solver.set_goal_interval(IN)
        us, xs = solver.solve_topp(reg=0)

        # Proper parameteriation
        assert xs[0] <= I0[1] + TINY and xs[0] >= I0[0] - TINY
        assert xs[-1] <= IN[1] + TINY and xs[-1] >= IN[0] - TINY
        assert np.all(xs >= 0)
        for i in range(solver.N):
            Delta_i = solver.ss[i+1] - solver.ss[i]
            assert np.allclose(xs[i+1], xs[i] + 2 * us[i] * Delta_i)

        # Constraint satisfy-ability
        for i in range(solver.N):
            for c in pcs:
                if c.nm != 0:
                    assert np.all(
                        c.a[i] * us[i] + c.b[i] * xs[i] + c.c[i] <= TINY)