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_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_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_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_classic_generator_weights(self): # weights defined for the Heirdt algorithm a = 1.0 # weight for CoM velocity tracking b = 0.0 # weight for CoM average velocity tracking c = 1e-06 # weight for ZMP reference tracking d = 1e-05 # weight for jerk minimization gen = ClassicGenerator() assert_equal(gen.a, a) assert_equal(gen.b, b) assert_equal(gen.c, c) assert_equal(gen.d, d)
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_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_generator_with_zero_reference_velocity(self): gen = ClassicGenerator() # set reference velocities to zero gen.dC_kp1_x_ref[...] = 0.0 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 for i in range(100): print 'iteration: i = ', i gen.solve() gen.update() gen.simulate() gen.c_k_x[0] = gen. C_kp1_x[0] gen.c_k_x[1] = gen. dC_kp1_x[0] gen.c_k_x[2] = gen.ddC_kp1_x[0] gen.c_k_y[0] = gen. C_kp1_y[0] gen.c_k_y[1] = gen. dC_kp1_y[0] gen.c_k_y[2] = gen.ddC_kp1_y[0] gen.c_k_q[0] = gen. C_kp1_q[0] gen.c_k_q[1] = gen. dC_kp1_q[0] gen.c_k_q[2] = gen.ddC_kp1_q[0] RTOL = 1e-05 ATOL = 1e-05
from walking_generator.classic import ClassicGenerator from walking_generator.visualization import Plotter gen = ClassicGenerator() gen = None show_canvas = True save_to_file = False plotter = Plotter( generator=gen, show_canvas=show_canvas, save_to_file=save_to_file, filename='./poster_pics', fmt='pdf' ) plotter.load_from_file('./data.json') plotter.update() #plotter.create_reference_plot() plotter.create_data_plot() if __name__=='__main__': raw_input('press key: ')
import os, sys import time import numpy numpy.set_printoptions(threshold=numpy.nan, linewidth =numpy.nan) from walking_generator.visualization import Plotter from walking_generator.classic import ClassicGenerator from walking_generator.interpolation import Interpolation # instantiate pattern generator gen = ClassicGenerator(fsm_state='L/R') # instantiate plotter show_canvas = True save_to_file = False plot = Plotter(gen, show_canvas, save_to_file) # Pattern Generator Preparation # set reference velocities to zero velocity_reference = [0.2,0.0,0.2] gen.set_velocity_reference(velocity_reference) gen.set_security_margin(0.09, 0.05) # set 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 gen.set_initial_values(comx, comy, comz, footx, footy, footq, foot='left')
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_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_real_pattern_genererator_data(self): """ verify if data is reproducible """ interp_data = numpy.loadtxt( os.path.join(BASEDIR, "data", "walkForward2m_sInterpolation.dat")) # instantiate pattern generator gen = ClassicGenerator() # index of 3 step after idle motion # 5.0s : idle time # 5.8s : first step # 6.6s : second step # 7.4s : third step ('left') index = 1479 for i in range(1): qp_idx = index + i ini_idx = index + i qp_dofs = interp_data[qp_idx, -(2 * gen.N + 2 * gen.nf):] # extract states c_k_x = interp_data[ini_idx, 1] c_k_y = interp_data[ini_idx, 2] c_k_q = interp_data[ini_idx, 4] dc_k_x = interp_data[ini_idx, 5] dc_k_y = interp_data[ini_idx, 6] dc_k_z = interp_data[ini_idx, 7] dc_k_q = interp_data[ini_idx, 8] h_com = interp_data[ini_idx, 3] ddc_k_x = interp_data[ini_idx, 9] ddc_k_y = interp_data[ini_idx, 10] ddc_k_z = interp_data[ini_idx, 11] ddc_k_q = interp_data[ini_idx, 12] comx = (c_k_x, dc_k_x, ddc_k_x) comy = (c_k_y, dc_k_y, ddc_k_y) comq = (c_k_q, dc_k_q, ddc_k_q) comz = h_com foot_x = 0.0 foot_y = 0.0 foot_q = 0.0 stuff = gen.update() foot = stuff[-2] com_q = (0, 0, 0) # define initial values #gen.set_initial_values( #) # check if current ZMP is calculated correctly z_k_x = interp_data[ini_idx, 13] z_k_y = interp_data[ini_idx, 14] assert_allclose(gen.z_k_x, z_k_x) assert_allclose(gen.z_k_y, z_k_y) # extract single dofs from data block dddC_k_x = qp_dofs[:gen.N] F_k_x = qp_dofs[gen.N:gen.N + gen.nf] dddC_k_y = qp_dofs[-gen.N - gen.nf:-gen.nf] F_k_y = qp_dofs[-gen.nf:] print 'dddC_k_x\n', dddC_k_x print 'dddC_k_y\n', dddC_k_y print 'F_k_x \n', F_k_x print 'F_k_y \n', F_k_y return 0 # set reference velocities gen.dC_kp1_x_ref[...] = 0.2 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 # initialize data gen.set_initial_values(comx, comy, comz, supportfootx, supportfooty, supportfootq)
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_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)
# for j in range(RightFootBuffer.shape[0]): # LeftFootTrajX[j] = LeftFootBuffer[j].z # RightFootTrajX[j] = RightFootBuffer[j].z # LeftFootTrajX.tofile("LeftFootBuffer"+format(i),sep="\n") # RightFootTrajX.tofile("RightFootBuffer"+format(i),sep="\n") # LFx = numpy.append(LFx,LeftFootTrajX) # RFx = numpy.append(RFx,RightFootTrajX) # LFx.tofile("lfttraj.txt",sep="\n") # RFx.tofile("rfttraj.txt",sep="\n") # print "done" #initialize pattern generator gen = ClassicGenerator() comx = [0.00124774,0.0,0.0] comy = [0.00157175,0.0,0.0] comz = 0.814 supportfootx = 0.00949035 supportfooty = 0.095 supportfootq = 0.0 secmarginx = 0.09 secmarginy = 0.05 gen._initState(comx,comy,comz,\ supportfootx,supportfooty,supportfootq,secmarginx,secmarginy) gen.simulate() # initialize interpolation samplingPeriod = 0.005 inter = Interpolation(0.005,gen)
import os, sys import time import numpy numpy.set_printoptions(threshold=numpy.nan, linewidth=numpy.nan) from walking_generator.visualization import Plotter from walking_generator.classic import ClassicGenerator from walking_generator.interpolation import Interpolation # instantiate pattern generator gen = ClassicGenerator(fsm_state='L/R') # instantiate plotter show_canvas = True save_to_file = False plot = Plotter(gen, show_canvas, save_to_file) # Pattern Generator Preparation # set reference velocities to zero velocity_reference = [0.2, 0.0, 0.2] gen.set_velocity_reference(velocity_reference) gen.set_security_margin(0.09, 0.05) # set 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 gen.set_initial_values(comx, comy, comz, footx, footy, footq, foot='left')
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)
def test_against_real_pattern_genererator_walkForward2m_s(self): # get test data data = numpy.loadtxt(os.path.join(BASEDIR, "data", "walkForward2m_s.dat") ) # instantiate pattern generator gen = ClassicGenerator() 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 dC_kp1_x_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_y_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_q_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_x_ref[50:,:] = 0.2 #m/s dC_kp1_y_ref[50:,:] = 0.2 #m/s gen.dC_kp1_x_ref[:] = dC_kp1_x_ref[0,:] gen.dC_kp1_y_ref[:] = dC_kp1_y_ref[0,:] gen.solve() for i in range(100): print 'iteration: i = ', i gen.dC_kp1_x_ref[:] = dC_kp1_x_ref[i,:] gen.dC_kp1_y_ref[:] = dC_kp1_y_ref[i,:] gen.c_k_x[0] = gen. C_kp1_x[0] gen.c_k_x[1] = gen. dC_kp1_x[0] gen.c_k_x[2] = gen.ddC_kp1_x[0] gen.c_k_y[0] = gen. C_kp1_y[0] gen.c_k_y[1] = gen. dC_kp1_y[0] gen.c_k_y[2] = gen.ddC_kp1_y[0] gen.c_k_q[0] = gen. C_kp1_q[0] gen.c_k_q[1] = gen. dC_kp1_q[0] gen.c_k_q[2] = gen.ddC_kp1_q[0] gen.update() gen.solve() # get reference from data dddC_k_x = data[i, 0:16] dddC_k_y = data[i, 18:34] F_k_x = data[i, 16:18] F_k_y = data[i, 34:36] assert_allclose(gen.dddC_k_x, dddC_k_x, rtol=RTOL, atol=ATOL) assert_allclose(gen.dddC_k_y, dddC_k_y, rtol=RTOL, atol=ATOL) assert_allclose(gen.dddC_k_y, 0.0, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_x, F_k_x, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_y, F_k_y, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_q, 0.0, rtol=RTOL, atol=ATOL)
def test_against_real_pattern_genererator_walkForward2m_s(self): # get test data data = numpy.loadtxt( os.path.join(BASEDIR, "data", "walkForward2m_s.dat")) # instantiate pattern generator gen = ClassicGenerator() 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 dC_kp1_x_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_y_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_q_ref = numpy.zeros((data.shape[0], gen.N), dtype=float) dC_kp1_x_ref[50:, :] = 0.2 #m/s dC_kp1_y_ref[50:, :] = 0.2 #m/s gen.dC_kp1_x_ref[:] = dC_kp1_x_ref[0, :] gen.dC_kp1_y_ref[:] = dC_kp1_y_ref[0, :] gen.solve() for i in range(100): print 'iteration: i = ', i gen.dC_kp1_x_ref[:] = dC_kp1_x_ref[i, :] gen.dC_kp1_y_ref[:] = dC_kp1_y_ref[i, :] gen.c_k_x[0] = gen.C_kp1_x[0] gen.c_k_x[1] = gen.dC_kp1_x[0] gen.c_k_x[2] = gen.ddC_kp1_x[0] gen.c_k_y[0] = gen.C_kp1_y[0] gen.c_k_y[1] = gen.dC_kp1_y[0] gen.c_k_y[2] = gen.ddC_kp1_y[0] gen.c_k_q[0] = gen.C_kp1_q[0] gen.c_k_q[1] = gen.dC_kp1_q[0] gen.c_k_q[2] = gen.ddC_kp1_q[0] gen.update() gen.solve() # get reference from data dddC_k_x = data[i, 0:16] dddC_k_y = data[i, 18:34] F_k_x = data[i, 16:18] F_k_y = data[i, 34:36] assert_allclose(gen.dddC_k_x, dddC_k_x, rtol=RTOL, atol=ATOL) assert_allclose(gen.dddC_k_y, dddC_k_y, rtol=RTOL, atol=ATOL) assert_allclose(gen.dddC_k_y, 0.0, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_x, F_k_x, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_y, F_k_y, rtol=RTOL, atol=ATOL) assert_allclose(gen.F_k_q, 0.0, rtol=RTOL, atol=ATOL)
def test_against_real_pattern_genererator_walkForward2m_s_while_walking( self): """ Take data where robot is walking with constant CoM velocity and start test from there. This surpasses FSM implementation and starting and stopping maneuvers. """ interp_data = numpy.loadtxt( os.path.join(BASEDIR, "data", "walkForward2m_sInterpolation.dat")) if True: for i in range(interp_data.shape[0])[1:]: if interp_data[i, 0] == 7.4: print 'i = ', i # instantiate pattern generator in walking mode, # i.e. in single support mode gen = ClassicGenerator(fsm_state='R/L') # define constant x CoM velocity to track gen.dC_kp1_x_ref[...] = 0.2 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 # take walking initial state from interpolation data # NOTE we take the second step as initial state for the test, because # this is a nearly periodic solution # idx is index on states used from interpolation data idx = 1479 comx = (interp_data[idx, 1], interp_data[idx, 5], interp_data[idx, 9]) comy = (interp_data[idx, 2], interp_data[idx, 6], interp_data[idx, 10]) comz = interp_data[idx, 3] supportfootx = interp_data[idx, 15] supportfooty = interp_data[idx, 16] supportfootq = interp_data[idx, 24] gen.set_initial_values(comx, comy, comz, supportfootx, supportfooty, supportfootq) return 0 idx = 69 for i in range(10): print 'iteration = ', i # solve QP for solution gen.solve() # get reference values dddC_k_x_ref = qp_data[idx + i, :gen.N] F_k_x_ref = qp_data[idx + i, gen.N:gen.N + gen.nf] dddC_k_y_ref = qp_data[idx + i, gen.N + gen.nf:2 * gen.N + gen.nf] F_k_y_ref = qp_data[idx + i, -gen.nf:] # check orientation values, should be constant zero, because we # assume that robot does not rotate #assert_allclose(gen.dddC_k_q, 0.0) #assert_allclose(gen.F_k_q, 0.0) # check position DoFs against data from C++ implementation assert_allclose(gen.dddC_k_x, dddC_k_x_ref) assert_allclose(gen.dddC_k_y, dddC_k_y_ref) assert_allclose(gen.F_k_x, F_k_x_ref) assert_allclose(gen.F_k_y, F_k_y_ref) # simulate to get new values gen.simulate() # TODO shifting do I use interpolation or just take the value from # simulation gen.c_k_x[...] = (gen.C_kp1_x[0], gen.dC_kp1_x[0], gen.ddC_kp1_x[0]) gen.c_k_y[...] = (gen.C_kp1_y[0], gen.dC_kp1_y[0], gen.ddC_kp1_y[0]) gen.c_k_q[...] = (gen.C_kp1_q[0], gen.dC_kp1_q[0], gen.ddC_kp1_q[0]) gen.update()
def test_real_pattern_genererator_data(self): """ verify if data is reproducible """ interp_data = numpy.loadtxt( os.path.join(BASEDIR, "data", "walkForward2m_sInterpolation.dat") ) # instantiate pattern generator gen = ClassicGenerator() # index of 3 step after idle motion # 5.0s : idle time # 5.8s : first step # 6.6s : second step # 7.4s : third step ('left') index = 1479 for i in range(1): qp_idx = index + i ini_idx = index + i qp_dofs = interp_data[qp_idx, -(2*gen.N+2*gen.nf):] # extract states c_k_x = interp_data[ini_idx, 1] c_k_y = interp_data[ini_idx, 2] c_k_q = interp_data[ini_idx, 4] dc_k_x = interp_data[ini_idx, 5] dc_k_y = interp_data[ini_idx, 6] dc_k_z = interp_data[ini_idx, 7] dc_k_q = interp_data[ini_idx, 8] h_com = interp_data[ini_idx, 3] ddc_k_x = interp_data[ini_idx, 9] ddc_k_y = interp_data[ini_idx, 10] ddc_k_z = interp_data[ini_idx, 11] ddc_k_q = interp_data[ini_idx, 12] comx = (c_k_x, dc_k_x, ddc_k_x) comy = (c_k_y, dc_k_y, ddc_k_y) comq = (c_k_q, dc_k_q, ddc_k_q) comz = h_com foot_x = 0.0 foot_y = 0.0 foot_q = 0.0 stuff = gen.update() foot = stuff[-2] com_q=(0,0,0) # define initial values #gen.set_initial_values( #) # check if current ZMP is calculated correctly z_k_x = interp_data[ini_idx, 13] z_k_y = interp_data[ini_idx, 14] assert_allclose(gen.z_k_x, z_k_x) assert_allclose(gen.z_k_y, z_k_y) # extract single dofs from data block dddC_k_x = qp_dofs[ :gen.N ] F_k_x = qp_dofs[ gen.N:gen.N+gen.nf] dddC_k_y = qp_dofs[-gen.N-gen.nf: -gen.nf] F_k_y = qp_dofs[ -gen.nf: ] print 'dddC_k_x\n', dddC_k_x print 'dddC_k_y\n', dddC_k_y print 'F_k_x \n', F_k_x print 'F_k_y \n', F_k_y return 0 # set reference velocities gen.dC_kp1_x_ref[...] = 0.2 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 # initialize data gen.set_initial_values( comx,comy,comz, supportfootx,supportfooty,supportfootq )
# for j in range(RightFootBuffer.shape[0]): # LeftFootTrajX[j] = LeftFootBuffer[j].z # RightFootTrajX[j] = RightFootBuffer[j].z # LeftFootTrajX.tofile("LeftFootBuffer"+format(i),sep="\n") # RightFootTrajX.tofile("RightFootBuffer"+format(i),sep="\n") # LFx = numpy.append(LFx,LeftFootTrajX) # RFx = numpy.append(RFx,RightFootTrajX) # LFx.tofile("lfttraj.txt",sep="\n") # RFx.tofile("rfttraj.txt",sep="\n") # print "done" #initialize pattern generator gen = ClassicGenerator() comx = [0.00124774, 0.0, 0.0] comy = [0.00157175, 0.0, 0.0] comz = 0.814 supportfootx = 0.00949035 supportfooty = 0.095 supportfootq = 0.0 secmarginx = 0.09 secmarginy = 0.05 gen._initState(comx,comy,comz,\ supportfootx,supportfooty,supportfootq,secmarginx,secmarginy) gen.simulate() # initialize interpolation samplingPeriod = 0.005 inter = Interpolation(0.005, gen)
def test_against_real_pattern_genererator_walkForward2m_s_while_walking(self): """ Take data where robot is walking with constant CoM velocity and start test from there. This surpasses FSM implementation and starting and stopping maneuvers. """ interp_data = numpy.loadtxt( os.path.join(BASEDIR, "data", "walkForward2m_sInterpolation.dat") ) if True: for i in range(interp_data.shape[0])[1:]: if interp_data[i,0] == 7.4: print 'i = ', i # instantiate pattern generator in walking mode, # i.e. in single support mode gen = ClassicGenerator(fsm_state='R/L') # define constant x CoM velocity to track gen.dC_kp1_x_ref[...] = 0.2 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 # take walking initial state from interpolation data # NOTE we take the second step as initial state for the test, because # this is a nearly periodic solution # idx is index on states used from interpolation data idx = 1479 comx = (interp_data[idx,1], interp_data[idx,5], interp_data[idx, 9]) comy = (interp_data[idx,2], interp_data[idx,6], interp_data[idx,10]) comz = interp_data[idx,3] supportfootx = interp_data[idx, 15] supportfooty = interp_data[idx, 16] supportfootq = interp_data[idx, 24] gen.set_initial_values( comx,comy,comz, supportfootx,supportfooty,supportfootq ) return 0 idx = 69 for i in range(10): print 'iteration = ', i # solve QP for solution gen.solve() # get reference values dddC_k_x_ref = qp_data[idx+i, :gen.N ] F_k_x_ref = qp_data[idx+i,gen.N :gen.N+gen.nf] dddC_k_y_ref = qp_data[idx+i,gen.N+gen.nf:2*gen.N+gen.nf] F_k_y_ref = qp_data[idx+i, -gen.nf: ] # check orientation values, should be constant zero, because we # assume that robot does not rotate #assert_allclose(gen.dddC_k_q, 0.0) #assert_allclose(gen.F_k_q, 0.0) # check position DoFs against data from C++ implementation assert_allclose(gen.dddC_k_x, dddC_k_x_ref) assert_allclose(gen.dddC_k_y, dddC_k_y_ref) assert_allclose(gen.F_k_x, F_k_x_ref) assert_allclose(gen.F_k_y, F_k_y_ref) # simulate to get new values gen.simulate() # TODO shifting do I use interpolation or just take the value from # simulation gen.c_k_x[...] = (gen.C_kp1_x[0], gen.dC_kp1_x[0], gen.ddC_kp1_x[0]) gen.c_k_y[...] = (gen.C_kp1_y[0], gen.dC_kp1_y[0], gen.ddC_kp1_y[0]) gen.c_k_q[...] = (gen.C_kp1_q[0], gen.dC_kp1_q[0], gen.ddC_kp1_q[0]) gen.update()
def test_generator_with_zero_reference_velocity(self): gen = ClassicGenerator() # set reference velocities to zero gen.dC_kp1_x_ref[...] = 0.0 gen.dC_kp1_y_ref[...] = 0.0 gen.dC_kp1_q_ref[...] = 0.0 for i in range(100): print 'iteration: i = ', i gen.solve() gen.update() gen.simulate() gen.c_k_x[0] = gen.C_kp1_x[0] gen.c_k_x[1] = gen.dC_kp1_x[0] gen.c_k_x[2] = gen.ddC_kp1_x[0] gen.c_k_y[0] = gen.C_kp1_y[0] gen.c_k_y[1] = gen.dC_kp1_y[0] gen.c_k_y[2] = gen.ddC_kp1_y[0] gen.c_k_q[0] = gen.C_kp1_q[0] gen.c_k_q[1] = gen.dC_kp1_q[0] gen.c_k_q[2] = gen.ddC_kp1_q[0] RTOL = 1e-05 ATOL = 1e-05
import os, sys import time import numpy numpy.set_printoptions(threshold=numpy.nan, linewidth =numpy.nan) from walking_generator.visualization import Plotter from walking_generator.classic import ClassicGenerator from walking_generator.combinedqp import NMPCGenerator from walking_generator.interpolation import Interpolation # instantiate pattern generator nmpc = NMPCGenerator(fsm_state='L/R') classic = ClassicGenerator(fsm_state='L/R') # Pattern Generator Preparation nmpc. set_security_margin(0.09, 0.05) classic.set_security_margin(0.09, 0.05) # instantiate plotter show_canvas = False save_to_file = False fmt='jpeg' dpi=200 limits = None#((-0.8, 3.5), (-0.5, 0.8)) nmpc_p = Plotter(nmpc, show_canvas=show_canvas, save_to_file=save_to_file, filename='./nmpc/nmpc.png', fmt=fmt, dpi=dpi, limits=limits ) classic_p = Plotter(classic, show_canvas=show_canvas, save_to_file=save_to_file, filename='./classic/classic.png', fmt=fmt, dpi=dpi, limits=limits )
import os, sys import time import numpy numpy.set_printoptions(threshold=numpy.nan, linewidth=numpy.nan) from walking_generator.visualization import Plotter from walking_generator.classic import ClassicGenerator from walking_generator.combinedqp import NMPCGenerator from walking_generator.interpolation import Interpolation # instantiate pattern generator nmpc = NMPCGenerator(fsm_state='L/R') classic = ClassicGenerator(fsm_state='L/R') # Pattern Generator Preparation nmpc.set_security_margin(0.09, 0.05) classic.set_security_margin(0.09, 0.05) # instantiate plotter show_canvas = False save_to_file = False fmt = 'jpeg' dpi = 200 limits = None #((-0.8, 3.5), (-0.5, 0.8)) nmpc_p = Plotter(nmpc, show_canvas=show_canvas, save_to_file=save_to_file, filename='./nmpc/nmpc.png', fmt=fmt, dpi=dpi, limits=limits)