def test_qp_objective_gradient_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
secmargin = 0.04
gen.set_security_margin(secmargin, secmargin)
comx = [0.06591456, 0.07638739, -0.1467377]
comy = [2.49008564e-02, 6.61665254e-02, 6.72712187e-01]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
gen.set_initial_values(comx, comy, comz, footx, footy, footq)
# define reference velocity
gen.dC_kp1_x_ref[...] = 0.2
gen.dC_kp1_y_ref[...] = 0.0
gen._preprocess_solution()
N = gen.N
nf = gen.nf
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
pos_g = numpy.loadtxt(os.path.join(BASEDIR, "data", "P.dat"),
skiprows=1)
g_mask = numpy.zeros(gen.pos_g.shape, dtype=bool)
# compare values for dddC_kp1_x
g_mask[...] = 0
g_mask[:N] = 1
assert_allclose(gen.pos_g[g_mask], pos_g[:N], rtol=RTOL, atol=ATOL)
# compare values for dddC_kp1_y
g_mask[...] = 0
g_mask[N + nf:-nf] = 1
assert_allclose(gen.pos_g[g_mask],
pos_g[N:2 * N],
rtol=RTOL,
atol=ATOL)
# compare values for F_k_x
g_mask[...] = 0
g_mask[N:N + nf - 1] = 1
assert_allclose(gen.pos_g[g_mask],
pos_g[2 * N:2 * N + 1],
rtol=RTOL,
atol=ATOL)
# compare values for F_k_y
g_mask[...] = 0
g_mask[-nf:-1] = 1
assert_allclose(gen.pos_g[g_mask],
pos_g[2 * N + 1:],
rtol=RTOL,
atol=ATOL)
def test_qp_objective_hessian_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
gen._preprocess_solution()
N = gen.N
nf = gen.nf
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
# assemble pos_H and pos_g for our convention
pos_H = numpy.loadtxt(os.path.join(BASEDIR, "data", "Q.dat"), skiprows=1)
H_mask = numpy.zeros(gen.pos_H.shape, dtype=bool)
# compare values for dddC_kp1_x
H_mask[...] = 0
H_mask[:N, :N] = 1
assert_allclose(
gen.pos_H[H_mask].reshape((N,N)),
pos_H [:N, :N],
rtol=RTOL, atol=ATOL
)
# compare values for dddC_kp1_y
H_mask[...] = 0
H_mask[N+nf:-nf, N+nf:-nf] = 1
assert_allclose(
gen.pos_H[H_mask].reshape((N,N)),
pos_H [N:2*N, N:2*N],
rtol=RTOL, atol=ATOL
)
# compare values for F_k_x
H_mask[...] = 0
H_mask[N:N+nf-1, N:N+nf-1] = 1
assert_allclose(
gen.pos_H[H_mask].reshape((1,1)),
pos_H [2*N:2*N+1, 2*N:2*N+1],
rtol=RTOL, atol=ATOL
)
# compare values for F_k_y
H_mask[...] = 0
H_mask[2*N+nf:-1, 2*N+nf:-1] = 1
assert_allclose(
gen.pos_H[H_mask].reshape((1,1)),
pos_H [2*N+1:, 2*N+1:],
rtol=RTOL, atol=ATOL
)
def test_matrices_of_position_qp_objective(self):
gen = ClassicGenerator()
gen._preprocess_solution()
# check symmetry of Hessian
pos_H = gen.pos_H
assert_allclose(pos_H - pos_H.transpose(), 0.0, rtol=RTOL, atol=ATOL)
# check positive definiteness
U, s, V = linalg.svd(pos_H)
assert_equal((s > 0).all(), True)
# test for equality of block ins pos_H
pos_H_A = pos_H[ :gen.N+gen.nf, :gen.N+gen.nf]
pos_H_B = pos_H[-gen.N-gen.nf:,-gen.N-gen.nf:]
assert_allclose(pos_H_A, pos_H_B, rtol=RTOL, atol=ATOL)
def test_matrices_of_position_qp_objective(self):
gen = ClassicGenerator()
gen._preprocess_solution()
# check symmetry of Hessian
pos_H = gen.pos_H
assert_allclose(pos_H - pos_H.transpose(), 0.0, rtol=RTOL, atol=ATOL)
# check positive definiteness
U, s, V = linalg.svd(pos_H)
assert_equal((s > 0).all(), True)
# test for equality of block ins pos_H
pos_H_A = pos_H[:gen.N + gen.nf, :gen.N + gen.nf]
pos_H_B = pos_H[-gen.N - gen.nf:, -gen.N - gen.nf:]
assert_allclose(pos_H_A, pos_H_B, rtol=RTOL, atol=ATOL)
def test_qp_objective_hessian_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
gen._preprocess_solution()
N = gen.N
nf = gen.nf
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
# assemble pos_H and pos_g for our convention
pos_H = numpy.loadtxt(os.path.join(BASEDIR, "data", "Q.dat"),
skiprows=1)
H_mask = numpy.zeros(gen.pos_H.shape, dtype=bool)
# compare values for dddC_kp1_x
H_mask[...] = 0
H_mask[:N, :N] = 1
assert_allclose(gen.pos_H[H_mask].reshape((N, N)),
pos_H[:N, :N],
rtol=RTOL,
atol=ATOL)
# compare values for dddC_kp1_y
H_mask[...] = 0
H_mask[N + nf:-nf, N + nf:-nf] = 1
assert_allclose(gen.pos_H[H_mask].reshape((N, N)),
pos_H[N:2 * N, N:2 * N],
rtol=RTOL,
atol=ATOL)
# compare values for F_k_x
H_mask[...] = 0
H_mask[N:N + nf - 1, N:N + nf - 1] = 1
assert_allclose(gen.pos_H[H_mask].reshape((1, 1)),
pos_H[2 * N:2 * N + 1, 2 * N:2 * N + 1],
rtol=RTOL,
atol=ATOL)
# compare values for F_k_y
H_mask[...] = 0
H_mask[2 * N + nf:-1, 2 * N + nf:-1] = 1
assert_allclose(gen.pos_H[H_mask].reshape((1, 1)),
pos_H[2 * N + 1:, 2 * N + 1:],
rtol=RTOL,
atol=ATOL)
def test_qp_constraint_setup_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
secmargin = 0.04
gen.set_security_margin(secmargin, secmargin)
# define initial state
comx = [0.06591456, 0.07638739, -0.1467377]
comy = [2.49008564e-02, 6.61665254e-02, 6.72712187e-01]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
gen.set_initial_values(comx, comy, comz, footx, footy, footq)
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
# get box constraints from data
pos_lb = numpy.loadtxt(os.path.join(BASEDIR, "data", "LB.dat"),
skiprows=1)
pos_ub = numpy.loadtxt(os.path.join(BASEDIR, "data", "UB.dat"),
skiprows=1)
# get linear constraints from data
pos_lbA = numpy.loadtxt(os.path.join(BASEDIR, "data", "lbA.dat"),
skiprows=1)[1:70]
# setup QP matrices
gen._preprocess_solution()
# test box constraints
assert_allclose(gen.pos_lb[:34], pos_lb, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_ub[:34], pos_ub, rtol=RTOL, atol=ATOL)
# test linear constraints
assert_allclose(gen.pos_ubA[:-gen.nc_fchange_eq -
gen.nFootPosHullEdges],
pos_lbA,
rtol=RTOL,
atol=ATOL)
gen.simulate()
def test_qp_constraint_setup_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
secmargin = 0.04
gen.set_security_margin(secmargin, secmargin)
# define initial state
comx = [0.06591456,0.07638739,-0.1467377]
comy = [2.49008564e-02,6.61665254e-02,6.72712187e-01]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
gen.set_initial_values(comx, comy, comz, footx, footy, footq)
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
# get box constraints from data
pos_lb = numpy.loadtxt(os.path.join(BASEDIR, "data", "LB.dat"), skiprows=1)
pos_ub = numpy.loadtxt(os.path.join(BASEDIR, "data", "UB.dat"), skiprows=1)
# get linear constraints from data
pos_lbA = numpy.loadtxt(os.path.join(BASEDIR, "data", "lbA.dat"), skiprows=1)[1:70]
# setup QP matrices
gen._preprocess_solution()
# test box constraints
assert_allclose(gen.pos_lb[:34], pos_lb, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_ub[:34], pos_ub, rtol=RTOL, atol=ATOL)
# test linear constraints
assert_allclose(gen.pos_ubA[:-gen.nc_fchange_eq-gen.nFootPosHullEdges], pos_lbA, rtol=RTOL, atol=ATOL)
gen.simulate()
def test_qp_objective_gradient_against_real_pattern_generator(self):
# instantiate pattern generator
gen = ClassicGenerator(fsm_state='L/R')
secmargin = 0.04
gen.set_security_margin(secmargin, secmargin)
comx = [0.06591456,0.07638739,-0.1467377]
comy = [2.49008564e-02,6.61665254e-02,6.72712187e-01]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
gen.set_initial_values(comx, comy, comz, footx, footy, footq)
# define reference velocity
gen.dC_kp1_x_ref[...] = 0.2
gen.dC_kp1_y_ref[...] = 0.0
gen._preprocess_solution()
N = gen.N
nf = gen.nf
# data follows other convention, i.e.
# U_k = (dddC_x, dddC_y, F_x, F_y)
pos_g = numpy.loadtxt(os.path.join(BASEDIR, "data", "P.dat"), skiprows=1)
g_mask = numpy.zeros(gen.pos_g.shape, dtype=bool)
# compare values for dddC_kp1_x
g_mask[...] = 0
g_mask[:N] = 1
assert_allclose(
gen.pos_g[g_mask],
pos_g[:N],
rtol=RTOL, atol=ATOL
)
# compare values for dddC_kp1_y
g_mask[...] = 0
g_mask[N+nf:-nf] = 1
assert_allclose(
gen.pos_g[g_mask],
pos_g[N:2*N],
rtol=RTOL, atol=ATOL
)
# compare values for F_k_x
g_mask[...] = 0
g_mask[N:N+nf-1] = 1
assert_allclose(
gen.pos_g[g_mask],
pos_g[2*N:2*N+1],
rtol=RTOL, atol=ATOL
)
# compare values for F_k_y
g_mask[...] = 0
g_mask[-nf:-1] = 1
assert_allclose(
gen.pos_g[g_mask],
pos_g[2*N+1:],
rtol=RTOL, atol=ATOL
)
def test_compare_constraint_matrices_to_classic_generator(self):
# define initial values
comx = [0.00949035, 0.0, 0.0]
comy = [0.095, 0.0, 0.0]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
# Pattern Generator Preparation
classic = ClassicGenerator(fsm_state='R/L')
# set reference velocities to zero
classic.set_velocity_reference([0.2, 0.0, -0.2])
classic.set_security_margin(0.04, 0.04)
# set initial values
classic.set_initial_values(comx,
comy,
comz,
footx,
footy,
footq,
foot='left')
# build up QP matrices
classic._preprocess_solution()
# reference them for comparison
classic_pos_A = classic.pos_A
classic_pos_lbA = classic.pos_lbA
classic_pos_ubA = classic.pos_ubA
classic_ori_A = classic.ori_A
classic_ori_lbA = classic.ori_lbA
classic_ori_ubA = classic.ori_ubA
# define initial values
comx = [0.00949035, 0.0, 0.0]
comy = [0.095, 0.0, 0.0]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
# Pattern Generator Preparation
nmpc = NMPCGenerator(fsm_state='R/L')
nmpc.set_velocity_reference([0.2, 0.0, -0.2])
nmpc.set_security_margin(0.04, 0.04)
# set initial values
nmpc.set_initial_values(comx,
comy,
comz,
footx,
footy,
footq,
foot='left')
# build up QP matrices
nmpc._preprocess_solution()
# reference them for comparison
nmpc_pos_A = nmpc.A_pos_x
nmpc_pos_lbA = nmpc.lbA_pos
nmpc_pos_ubA = nmpc.ubA_pos
nmpc_ori_A = nmpc.A_ori
nmpc_ori_lbA = nmpc.lbA_ori
nmpc_ori_ubA = nmpc.ubA_ori
nmpc_A_pos = nmpc.qp_A[:nmpc.nc_pos, :2 * (nmpc.N + nmpc.nf)]
nmpc_lbA_pos = nmpc.qp_lbA[:nmpc.nc_pos]
nmpc_ubA_pos = nmpc.qp_ubA[:nmpc.nc_pos]
nmpc_A_ori = nmpc.qp_A[-nmpc.nc_ori:, -2 * nmpc.N:]
nmpc_lbA_ori = nmpc.qp_lbA[-nmpc.nc_ori:]
nmpc_ubA_ori = nmpc.qp_ubA[-nmpc.nc_ori:]
# compare matrices
# position common sub expressions
assert_allclose(classic_pos_A, nmpc_pos_A, atol=ATOL, rtol=RTOL)
assert_allclose(classic_pos_lbA, nmpc_pos_lbA, atol=ATOL, rtol=RTOL)
assert_allclose(classic_pos_ubA, nmpc_pos_ubA, atol=ATOL, rtol=RTOL)
assert_allclose(classic_pos_A, nmpc_A_pos, atol=ATOL, rtol=RTOL)
assert_allclose(classic_pos_lbA, nmpc_lbA_pos, atol=ATOL, rtol=RTOL)
assert_allclose(classic_pos_ubA, nmpc_ubA_pos, atol=ATOL, rtol=RTOL)
# orientation common sub expressions
assert_allclose(classic_ori_A, nmpc_ori_A, atol=ATOL, rtol=RTOL)
assert_allclose(classic_ori_lbA, nmpc_ori_lbA, atol=ATOL, rtol=RTOL)
assert_allclose(classic_ori_ubA, nmpc_ori_ubA, atol=ATOL, rtol=RTOL)
assert_allclose(classic_ori_A, nmpc_A_ori, atol=ATOL, rtol=RTOL)
assert_allclose(classic_ori_lbA, nmpc_lbA_ori, atol=ATOL, rtol=RTOL)
assert_allclose(classic_ori_ubA, nmpc_ubA_ori, atol=ATOL, rtol=RTOL)
def test_compare_submatrices_to_classic_generator(self):
# define initial values
comx = [0.00949035, 0.0, 0.0]
comy = [0.095, 0.0, 0.0]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
# Pattern Generator Preparation
classic = ClassicGenerator()
# set reference velocities to zero
classic.set_velocity_reference([0.2, 0.0, -0.2])
classic.set_security_margin(0.04, 0.04)
# set initial values
classic.set_initial_values(comx,
comy,
comz,
footx,
footy,
footq,
foot='left')
# build up QP matrices
classic._preprocess_solution()
# reference them for comparison
pos_H = classic.pos_H
classic_Q_k_x = pos_H[:classic.N + classic.nf, :classic.N + classic.nf]
classic_Q_k_y = pos_H[-(classic.N + classic.nf):,
-(classic.N + classic.nf):]
pos_g = classic.pos_g
classic_p_k_x = pos_g[:classic.N + classic.nf]
classic_p_k_y = pos_g[-(classic.N + classic.nf):]
ori_H = classic.ori_H
classic_Q_k_qR = ori_H[:classic.N, :classic.N]
classic_Q_k_qL = ori_H[-classic.N:, -classic.N:]
ori_g = classic.ori_g
classic_p_k_qR = ori_g[:classic.N]
classic_p_k_qL = ori_g[-classic.N:]
# define initial values
comx = [0.00949035, 0.0, 0.0]
comy = [0.095, 0.0, 0.0]
comz = 0.814
footx = 0.00949035
footy = 0.095
footq = 0.0
# Pattern Generator Preparation
nmpc = NMPCGenerator()
nmpc.set_velocity_reference([0.2, 0.0, -0.2])
nmpc.set_security_margin(0.04, 0.04)
# set initial values
nmpc.set_initial_values(comx,
comy,
comz,
footx,
footy,
footq,
foot='left')
# build up QP matrices
nmpc._preprocess_solution()
# reference them for comparison
nmpc_Q_k_x = nmpc.Q_k_x
nmpc_p_k_x = nmpc.p_k_x
nmpc_p_k_y = nmpc.p_k_y
nmpc_Q_k_qR = nmpc.Q_k_qR
nmpc_Q_k_qL = nmpc.Q_k_qL
nmpc_p_k_qR = nmpc.p_k_qR
nmpc_p_k_qL = nmpc.p_k_qL
# compare matrices
# position common sub expressions
assert_allclose(classic_Q_k_x, nmpc_Q_k_x, atol=ATOL, rtol=RTOL)
assert_allclose(classic_Q_k_y, nmpc_Q_k_x, atol=ATOL, rtol=RTOL)
assert_allclose(classic_p_k_x, nmpc_p_k_x, atol=ATOL, rtol=RTOL)
assert_allclose(classic_p_k_y, nmpc_p_k_y, atol=ATOL, rtol=RTOL)
# orientation common sub expressions
assert_allclose(classic_Q_k_qR, nmpc_Q_k_qR, atol=ATOL, rtol=RTOL)
assert_allclose(classic_Q_k_qL, nmpc_Q_k_qL, atol=ATOL, rtol=RTOL)
assert_allclose(classic_p_k_qR, nmpc_p_k_qR, atol=ATOL, rtol=RTOL)
assert_allclose(classic_p_k_qL, nmpc_p_k_qL, atol=ATOL, rtol=RTOL)