Implement threading for island constraint solving.

Humanoids22 No threads:
 Benchmark Execution Time: 9.475905812s

Humanoids22 with 10 Threads:
 Benchmark Execution Time: 4.871352s

PiperOrigin-RevId: 571214307
Change-Id: I1f4f2c761b4ae6bc8fac1f28c6c695f6c499f339
This commit is contained in:
Matthew Bennice
2023-10-05 21:04:39 -07:00
committed by Copybara-Service
parent 06670155c4
commit 071af3b015
20 changed files with 3667 additions and 30 deletions
+7 -9
View File
@@ -46,23 +46,21 @@ static void run_step_benchmark(const mjModel* model, benchmark::State& state) {
int nsteps = kNumWarmupSteps+kNumBenchmarkSteps;
std::vector<mjtNum> ctrl = GetCtrlNoise(model, nsteps);
// warm-up rollout to get a typcal state
// warm-up rollout to get a typical state
for (int i=0; i < kNumWarmupSteps; i++) {
mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
mj_step(model, data);
}
// save state
std::vector<mjtNum> qpos = AsVector(data->qpos, model->nq);
std::vector<mjtNum> qvel = AsVector(data->qvel, model->nv);
std::vector<mjtNum> act = AsVector(data->act, model->na);
std::vector<mjtNum> warmstart = AsVector(data->qacc_warmstart, model->nv);
int spec = mjSTATE_INTEGRATION;
int size = mj_stateSize(model, spec);
std::vector<mjtNum> initial_state(size);
mj_getState(model, data, initial_state.data(), spec);
// reset state, benchmark subsequent kNumBenchmarkSteps steps
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
mju_copy(data->qpos, qpos.data(), model->nq);
mju_copy(data->qvel, qvel.data(), model->nv);
mju_copy(data->act, act.data(), model->na);
mju_copy(data->qacc_warmstart, warmstart.data(), model->nv);
mj_setState(model, data, initial_state.data(), spec);
for (int i=kNumWarmupSteps; i < nsteps; i++) {
mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);