lqr.py 1.21 KB
Newer Older
 Carlos Mastalli committed Jul 16, 2019 1 2 ``````import time `````` Guilhem Saurel committed Aug 30, 2019 3 4 5 6 7 ``````import numpy as np import crocoddyl from crocoddyl.utils import LQRDerived `````` Carlos Mastalli committed Jul 16, 2019 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 ``````NX = 37 NU = 12 N = 100 # number of nodes T = int(5e3) # number of trials MAXITER = 1 def runBenchmark(model): x0 = np.matrix(np.zeros(NX)).T runningModels = [model(NX, NU)] * N terminalModel = model(NX, NU) xs = [x0] * (N + 1) us = [np.matrix(np.zeros(NU)).T] * N problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel) ddp = crocoddyl.SolverDDP(problem) duration = [] for i in range(T): c_start = time.time() ddp.solve(xs, us, MAXITER) c_end = time.time() duration.append(1e3 * (c_end - c_start)) avrg_duration = sum(duration) / len(duration) min_duration = min(duration) max_duration = max(duration) return avrg_duration, min_duration, max_duration print('cpp-wrapped lqr:') avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.ActionModelLQR) print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration)) `````` Carlos Mastalli committed Jul 16, 2019 42 ``````print('Python-derived lqr:') `````` Guilhem Saurel committed Aug 30, 2019 43 ``````avrg_duration, min_duration, max_duration = runBenchmark(LQRDerived) `````` Carlos Mastalli committed Jul 16, 2019 44 ``print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))``