Multi-threading breaks determinism of DDP
Solving the exact same OC problem several times is non-deterministic if multi-threading is enabled.
The following script can be used to reproduce:
from __future__ import print_function
import time
import example_robot_data
import numpy as np
import crocoddyl
import pinocchio
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem
robot_model = example_robot_data.loadHyQ().model
lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot)
q0 = robot_model.referenceConfigurations['half_sitting'].copy()
v0 = pinocchio.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
gait_phase = {
'walking': {
'stepLength': 0.25,
'stepHeight': 0.25,
'timeStep': 1e-2,
'stepKnots': 25,
'supportKnots': 2
}
}
value = gait_phase['walking']
ddp = crocoddyl.SolverDDP(gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'], value['stepKnots'], value['supportKnots']))
np.set_printoptions(2)
iters = []
for i in range(10):
xs = [robot_model.defaultState] * len(ddp.models())
us = [m.quasicStatic(d, robot_model.defaultState) for m, d in list(zip(ddp.models(), ddp.datas()))[:-1]]
c_start = time.time()
ddp.solve(xs, us, 1000, False, 0.1)
c_end = time.time()
print("Solved in", c_end - c_start, "-", ddp.iter, "iterations")
iters.append(ddp.iter)
print("Mean iter", np.mean(iters), "+/-", np.std(iters))
Output without multi-threading:
Solved in 0.207679033279 - 11 iterations
Solved in 0.209436893463 - 11 iterations
Solved in 0.209597826004 - 11 iterations
Solved in 0.210040807724 - 11 iterations
Solved in 0.21022105217 - 11 iterations
Solved in 0.209685087204 - 11 iterations
Solved in 0.211217880249 - 11 iterations
Solved in 0.210963964462 - 11 iterations
Solved in 0.212151050568 - 11 iterations
Solved in 0.212280988693 - 11 iterations
Mean iter 11.0 +/- 0.0
Output with multi-threading (threads=4, no vectorisation):
Solved in 0.386569023132 - 26 iterations
Solved in 0.337476015091 - 22 iterations
Solved in 0.370394945145 - 25 iterations
Solved in 0.372181892395 - 25 iterations
Solved in 0.41090297699 - 28 iterations
Solved in 0.400117874146 - 28 iterations
Solved in 0.421534061432 - 29 iterations
Solved in 0.448474884033 - 32 iterations
Solved in 0.333574056625 - 22 iterations
Solved in 0.293792009354 - 19 iterations
Mean iter 25.6 +/- 3.6660605559646715