def test_qp_setup_with_toy_example(self):
gen = ClassicGenerator()
# define input matrices
gen.pos_H[...] = numpy.eye(gen.pos_nv)
gen.pos_g[...] = numpy.ones((gen.pos_nv, ))
gen.pos_lb[...] = -numpy.ones((gen.pos_nv, )) * 0.5
gen.pos_ub[...] = numpy.ones((gen.pos_nv, )) * 0.5
gen.pos_A[...] = numpy.eye(gen.pos_nc, gen.pos_nv) + numpy.eye(
gen.pos_nc, gen.pos_nv, k=1)
gen.pos_lbA[...] = -numpy.ones((gen.pos_nc, ))
gen.pos_ubA[...] = numpy.ones((gen.pos_nc, ))
# define input matrices
gen.ori_H[...] = numpy.eye(gen.ori_nv)
gen.ori_g[...] = numpy.ones((gen.ori_nv, ))
gen.ori_lb[...] = -numpy.ones((gen.ori_nv, )) * 0.5
gen.ori_ub[...] = numpy.ones((gen.ori_nv, )) * 0.5
gen.ori_A[...] = numpy.eye(gen.ori_nc, gen.ori_nv) + numpy.eye(
gen.ori_nc, gen.ori_nv, k=1)
gen.ori_lbA[...] = -numpy.ones((gen.ori_nc, ))
gen.ori_ubA[...] = numpy.ones((gen.ori_nc, ))
# define solution
ori_x = -numpy.ones((gen.ori_nv, )) * 0.5
ori_f = -12.0
pos_x = -numpy.ones((gen.pos_nv, )) * 0.5
pos_f = -13.5
# test first qp solution
gen._solve_qp()
gen._postprocess_solution()
# get solution
assert_allclose(gen.ori_dofs, ori_x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), ori_f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_dofs, pos_x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), pos_f, rtol=RTOL, atol=ATOL)
def test_qp_setup_with_toy_example(self):
gen = ClassicGenerator()
# define input matrices
gen.pos_H[...] = numpy.eye(gen.pos_nv)
gen.pos_g[...] = numpy.ones((gen.pos_nv,))
gen.pos_lb[...] = -numpy.ones((gen.pos_nv,))*0.5
gen.pos_ub[...] = numpy.ones((gen.pos_nv,))*0.5
gen.pos_A[...] = numpy.eye(gen.pos_nc, gen.pos_nv) + numpy.eye(gen.pos_nc, gen.pos_nv, k=1)
gen.pos_lbA[...] = -numpy.ones((gen.pos_nc,))
gen.pos_ubA[...] = numpy.ones((gen.pos_nc,))
# define input matrices
gen.ori_H[...] = numpy.eye(gen.ori_nv)
gen.ori_g[...] = numpy.ones((gen.ori_nv,))
gen.ori_lb[...] = -numpy.ones((gen.ori_nv,))*0.5
gen.ori_ub[...] = numpy.ones((gen.ori_nv,))*0.5
gen.ori_A[...] = numpy.eye(gen.ori_nc, gen.ori_nv) + numpy.eye(gen.ori_nc, gen.ori_nv, k=1)
gen.ori_lbA[...] = -numpy.ones((gen.ori_nc,))
gen.ori_ubA[...] = numpy.ones((gen.ori_nc,))
# define solution
ori_x = -numpy.ones((gen.ori_nv,))*0.5
ori_f = -12.0
pos_x = -numpy.ones((gen.pos_nv,))*0.5
pos_f = -13.5
# test first qp solution
gen._solve_qp()
gen._postprocess_solution()
# get solution
assert_allclose(gen.ori_dofs, ori_x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), ori_f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_dofs, pos_x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), pos_f, rtol=RTOL, atol=ATOL)
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = numpy.array([ 1.0, 1.0 ]).reshape((1,2))
g = numpy.array([ 1.5, 1.0 ])
lb = numpy.array([ 0.5, -2.0 ])
ub = numpy.array([ 5.0, 2.0 ])
lbA = numpy.array([ -1.0 ])
ubA = numpy.array([ 2.0 ])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2,))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2,))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
A_new = numpy.array([ 1.0, 5.0 ]).reshape((1,2))
g_new = numpy.array([ 1.0, 1.5 ])
lb_new = numpy.array([ 0.0, -1.0 ])
ub_new = numpy.array([ 5.0, -0.5 ])
lbA_new = numpy.array([ -2.0 ])
ubA_new = numpy.array([ 1.0 ])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
A = numpy.array([1.0, 1.0]).reshape((1, 2))
g = numpy.array([1.5, 1.0])
lb = numpy.array([0.5, -2.0])
ub = numpy.array([5.0, 2.0])
lbA = numpy.array([-1.0])
ubA = numpy.array([2.0])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2, ))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2, ))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([1.0, 0.5, 0.5, 0.5]).reshape((2, 2))
A_new = numpy.array([1.0, 5.0]).reshape((1, 2))
g_new = numpy.array([1.0, 1.5])
lb_new = numpy.array([0.0, -1.0])
ub_new = numpy.array([5.0, -0.5])
lbA_new = numpy.array([-2.0])
ubA_new = numpy.array([1.0])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)