lqr.py 1.21 KB
Newer Older
1
2
import time

Guilhem Saurel's avatar
Guilhem Saurel committed
3
4
5
6
7
import numpy as np

import crocoddyl
from crocoddyl.utils import LQRDerived

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))

42
print('Python-derived lqr:')
Guilhem Saurel's avatar
Guilhem Saurel committed
43
avrg_duration, min_duration, max_duration = runBenchmark(LQRDerived)
44
print('  CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))