Add Nesterov momentum with O'Donoghue-Candès restarts to PGS solver (~2x speedup)
Benchmark on `2humanoid100.xml` (nefc=1785, nv=654): ``` Convergence at fixed iteration count (mean relative error vs Newton): 20 iters: 2.98e-03 vs 1.54e-02 (5.2x better) 40 iters: 8.14e-05 vs 2.60e-03 (32x better) 80 iters: 1.17e-07 vs 1.77e-04 (1500x better) Pipeline throughput (tolerance=1e-8, islands disabled): Nesterov: 243 steps/s, 46 iters/step Baseline: 151 steps/s, 95 iters/step Solver speedup: 1.8x, overall step speedup: 1.6x Pipeline throughput (tolerance=1e-8, islands enabled): Nesterov: 306 steps/s, 442 iters/step Baseline: 175 steps/s, 966 iters/step Solver speedup: 2.1x, overall step speedup: 1.7x ``` PiperOrigin-RevId: 936610759 Change-Id: I2978e8bd545971d9151005623967e5cf0ad125cc
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
5cef2472d5
commit
c499f7f2b0
@@ -2,6 +2,14 @@
|
||||
Changelog
|
||||
=========
|
||||
|
||||
Upcoming version (not yet released)
|
||||
-----------------------------------
|
||||
|
||||
General
|
||||
^^^^^^^
|
||||
- Added Nesterov momentum extrapolation with adaptive gradient restart (O'Donoghue-Candès) to the PGS solver,
|
||||
significantly improving convergence. Overall PGS now requires ~2x fewer iterations.
|
||||
|
||||
Version 3.10.0 (June 22, 2026)
|
||||
------------------------------
|
||||
|
||||
|
||||
+128
-8
@@ -26,6 +26,7 @@
|
||||
#include "engine/engine_core_smooth.h"
|
||||
#include "engine/engine_core_util.h"
|
||||
#include "engine/engine_memory.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
@@ -375,6 +376,27 @@ static int dualStateChange(const mjData* d, int* state, int* oldstate,
|
||||
}
|
||||
|
||||
|
||||
// project onto friction ellipsoid, write to force[i+1..i+dim-1]
|
||||
// project tangential force onto friction ellipsoid: sum(f_t[j]^2/mu[j]^2) <= f_n^2
|
||||
// if feasible is true, only scale down if outside the ellipsoid
|
||||
// if feasible is false, always scale to the boundary
|
||||
static void projectEllipsoid(mjtNum* friction, mjtNum normal, const mjtNum* mu,
|
||||
int dim, int feasible) {
|
||||
mjtNum s = 0;
|
||||
for (int j=0; j < dim-1; j++) {
|
||||
s += friction[j]*friction[j] / (mu[j]*mu[j]);
|
||||
}
|
||||
|
||||
mjtNum normal2 = normal*normal;
|
||||
if (!feasible || s > normal2) {
|
||||
mjtNum scl = mju_sqrt(normal2 / mju_max(mjMINVAL, s));
|
||||
for (int j=0; j < dim-1; j++) {
|
||||
friction[j] *= scl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// solve QCQP and project onto friction ellipsoid, write to force[i+1..i+dim-1]
|
||||
static void solveQCQP(mjtNum* force, int i, int dim,
|
||||
mjtNum* Ac, mjtNum* bc, const mjtNum* mu) {
|
||||
@@ -392,14 +414,7 @@ static void solveQCQP(mjtNum* force, int i, int dim,
|
||||
|
||||
// on constraint: put v on ellipsoid, in case QCQP is approximate
|
||||
if (flg_active) {
|
||||
mjtNum s = 0;
|
||||
for (int j=0; j < dim-1; j++) {
|
||||
s += v[j]*v[j] / (mu[j]*mu[j]);
|
||||
}
|
||||
s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s));
|
||||
for (int j=0; j < dim-1; j++) {
|
||||
v[j] *= s;
|
||||
}
|
||||
projectEllipsoid(v, force[i], mu, dim, /*feasible=*/0);
|
||||
}
|
||||
|
||||
// assign
|
||||
@@ -407,6 +422,31 @@ static void solveQCQP(mjtNum* force, int i, int dim,
|
||||
}
|
||||
|
||||
|
||||
// project contact force block onto friction cone (pyramidal or elliptic)
|
||||
static void projectCone(mjtNum* force, const mjtNum* mu, int dim, int type) {
|
||||
// elliptic cone: project onto friction ellipsoid
|
||||
if (type == mjCNSTR_CONTACT_ELLIPTIC) {
|
||||
// clamp normal force
|
||||
if (force[0] < 0) {
|
||||
mju_zero(force, dim);
|
||||
} else {
|
||||
projectEllipsoid(force+1, force[0], mu, dim, /*feasible=*/1);
|
||||
}
|
||||
}
|
||||
|
||||
// pyramidal or scalar: clamp to non-negative
|
||||
else {
|
||||
if (force[0] < 0) {
|
||||
force[0] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// global variable to toggle Nesterov momentum (for benchmarks/tests)
|
||||
mjTHREADLOCAL int mj_nesterov_momentum = 1;
|
||||
|
||||
|
||||
//---------------------------- PGS solver ----------------------------------------------------------
|
||||
|
||||
// core PGS solver: iterates over constraints specified by efclist
|
||||
@@ -423,6 +463,16 @@ static void solPGS(const mjModel* m, mjData* d, int island,
|
||||
int* oldstate = mjSTACKALLOC(d, 2*nefc, int);
|
||||
int* blockstart = oldstate + nefc;
|
||||
|
||||
// Nesterov momentum
|
||||
mjtBool nesterov = (mj_nesterov_momentum != 0);
|
||||
mjtNum* force_prev = NULL;
|
||||
mjtNum* force_momentum = NULL;
|
||||
if (nesterov) {
|
||||
force_prev = mjSTACKALLOC(d, nefc, mjtNum);
|
||||
force_momentum = mjSTACKALLOC(d, nefc, mjtNum);
|
||||
mju_gather(force_prev, force, efclist, nefc);
|
||||
}
|
||||
|
||||
int island_stat = mjMAX(0, island); // island index for diagnostic stats
|
||||
mjtNum scale = 1 / (m->stat.meaninertia * mjMAX(1, m->nv));
|
||||
|
||||
@@ -452,7 +502,56 @@ static void solPGS(const mjModel* m, mjData* d, int island,
|
||||
|
||||
// main iteration
|
||||
int iter = 0;
|
||||
int nesterov_k = 0; // Nesterov counter (resets on adaptive restart)
|
||||
while (iter < maxiter) {
|
||||
// Nesterov momentum extrapolation
|
||||
if (nesterov) {
|
||||
mjtNum beta = 0;
|
||||
if (iter > 0) {
|
||||
beta = (mjtNum)(nesterov_k - 1) / (mjtNum)(nesterov_k + 2);
|
||||
}
|
||||
|
||||
// update with momentum, save pre-extrapolation value
|
||||
if (beta > 0) {
|
||||
for (int c=0; c < nefc; c++) {
|
||||
int i = efclist ? efclist[c] : c;
|
||||
mjtNum f_save = force[i];
|
||||
force[i] += beta*(force[i] - force_prev[c]);
|
||||
force_prev[c] = f_save;
|
||||
}
|
||||
|
||||
// friction loss: project onto bounds
|
||||
for (int c=ne; c < ne+nf; c++) {
|
||||
int i = efclist ? efclist[c] : c;
|
||||
force[i] = mju_clip(force[i], -floss[i], floss[i]);
|
||||
}
|
||||
|
||||
// contact force: project onto friction cone
|
||||
for (int c=ne+nf; c < nefc; ) {
|
||||
int i = efclist ? efclist[c] : c;
|
||||
int dim = 1;
|
||||
int type = d->efc_type[i];
|
||||
const mjtNum* mu = NULL;
|
||||
|
||||
if (type == mjCNSTR_CONTACT_ELLIPTIC) {
|
||||
dim = d->contact[d->efc_id[i]].dim;
|
||||
mu = d->contact[d->efc_id[i]].friction;
|
||||
}
|
||||
|
||||
projectCone(force+i, mu, dim, type);
|
||||
c += dim;
|
||||
}
|
||||
}
|
||||
|
||||
// iter == 0 or beta <= 0 (nesterov_k <= 1): just save current force
|
||||
else {
|
||||
mju_gather(force_prev, force, efclist, nefc);
|
||||
}
|
||||
|
||||
// save extrapolated point for gradient restart check
|
||||
mju_gather(force_momentum, force, efclist, nefc);
|
||||
}
|
||||
|
||||
// clear improvement
|
||||
mjtNum improvement = 0;
|
||||
|
||||
@@ -591,6 +690,27 @@ static void solPGS(const mjModel* m, mjData* d, int island,
|
||||
improvement *= scale;
|
||||
saveStats(m, d, island_stat, iter, improvement, 0, 0, nactive, nchange, 0, 0);
|
||||
|
||||
// Nesterov gradient restart (O'Donoghue-Candès): reset when correction opposes extrapolation
|
||||
if (nesterov) {
|
||||
mjtBool restart = false;
|
||||
if (iter > 0) {
|
||||
mjtNum dot_corr_extr = 0;
|
||||
for (int c=0; c < nefc; c++) {
|
||||
int i = efclist ? efclist[c] : c;
|
||||
mjtNum correction = force[i] - force_momentum[c];
|
||||
mjtNum extrapolation = force_momentum[c] - force_prev[c];
|
||||
dot_corr_extr += correction * extrapolation;
|
||||
}
|
||||
restart = (dot_corr_extr < 0);
|
||||
}
|
||||
|
||||
if (restart) {
|
||||
nesterov_k = 0;
|
||||
} else {
|
||||
nesterov_k++;
|
||||
}
|
||||
}
|
||||
|
||||
// increment iteration count
|
||||
iter++;
|
||||
|
||||
|
||||
@@ -2129,8 +2129,13 @@ void mju_n2d(double* res, const mjtNum* vec, int n) {
|
||||
}
|
||||
|
||||
|
||||
// gather
|
||||
// gather: res[i] = vec[ind[i]], or copy if ind is NULL
|
||||
void mju_gather(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) {
|
||||
if (!ind) {
|
||||
mju_copy(res, vec, n);
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i=0; i < n; i++) {
|
||||
res[i] = vec[ind[i]];
|
||||
}
|
||||
@@ -2146,8 +2151,13 @@ void mju_gatherMasked(mjtNum* restrict res, const mjtNum* restrict vec,
|
||||
}
|
||||
|
||||
|
||||
// scatter
|
||||
// scatter: res[ind[i]] = vec[i], or copy if ind is NULL
|
||||
void mju_scatter(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) {
|
||||
if (!ind) {
|
||||
mju_copy(res, vec, n);
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i=0; i < n; i++) {
|
||||
res[ind[i]] = vec[i];
|
||||
}
|
||||
|
||||
@@ -281,13 +281,13 @@ MJAPI void mju_d2n(mjtNum* res, const double* vec, int n);
|
||||
// convert from mjtNum to double
|
||||
MJAPI void mju_n2d(double* res, const mjtNum* vec, int n);
|
||||
|
||||
// gather mjtNums
|
||||
// gather mjtNums: res[i] = vec[ind[i]], or copy if ind is NULL
|
||||
MJAPI void mju_gather(mjtNum* res, const mjtNum* vec, const int* ind, int n);
|
||||
|
||||
// gather mjtNums, set to 0 at negative indices
|
||||
MJAPI void mju_gatherMasked(mjtNum* res, const mjtNum* vec, const int* ind, int n);
|
||||
|
||||
// scatter mjtNums
|
||||
// scatter mjtNums: res[ind[i]] = vec[i], or copy if ind is NULL
|
||||
MJAPI void mju_scatter(mjtNum* res, const mjtNum* vec, const int* ind, int n);
|
||||
|
||||
// gather integers
|
||||
|
||||
@@ -0,0 +1,274 @@
|
||||
// Copyright 2026 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
// PGS with/without Nesterov momentum: compare convergence and timing.
|
||||
//
|
||||
// Rolls out Newton ground truth on 2humanoid100.xml, then evaluates PGS with
|
||||
// and without Nesterov momentum at various iteration budgets.
|
||||
|
||||
|
||||
#include <chrono> // NOLINT
|
||||
#include <cstdio>
|
||||
#include <ratio> // NOLINT
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "test/fixture.h"
|
||||
|
||||
extern "C" thread_local int mj_nesterov_momentum;
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::NotNull;
|
||||
|
||||
mjtNum gettm(void) {
|
||||
using Clock = std::chrono::steady_clock;
|
||||
using Microseconds = std::chrono::duration<mjtNum, std::micro>;
|
||||
static const Clock::time_point tm_start = Clock::now();
|
||||
return Microseconds(Clock::now() - tm_start).count();
|
||||
}
|
||||
|
||||
using PgsConvergenceTest = MujocoTest;
|
||||
|
||||
// get total solver iterations across all islands
|
||||
int get_total_solver_iters(const mjData* d) {
|
||||
int nisland = mjMAX(1, mjMIN(d->nisland, mjNISLAND));
|
||||
int total = 0;
|
||||
for (int i = 0; i < nisland; i++) {
|
||||
total += d->solver_niter[i];
|
||||
}
|
||||
return total;
|
||||
}
|
||||
|
||||
// run solver benchmark at various iteration counts, print table
|
||||
void run_benchmark(mjModel* model, mjData* data,
|
||||
const std::vector<mjtNum>& all_qpos,
|
||||
const std::vector<mjtNum>& all_qvel,
|
||||
const std::vector<mjtNum>& all_warmstart,
|
||||
const std::vector<mjtNum>& all_qacc,
|
||||
int nq, int nv, int kStride, int kNumEval,
|
||||
const int* kIterCounts, int kNumIter,
|
||||
const char* label) {
|
||||
std::printf("\n %s:\n", label);
|
||||
std::printf(" %6s | %11s | %11s | %10s | %11s\n",
|
||||
"Iters", "Mean Err", "Max Err", "Mean Iters", "Solver us");
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+-----------");
|
||||
|
||||
for (int c = 0; c < kNumIter; c++) {
|
||||
model->opt.iterations = kIterCounts[c];
|
||||
|
||||
for (int i = 0; i < mjNTIMER; i++) {
|
||||
data->timer[i].duration = 0;
|
||||
data->timer[i].number = 0;
|
||||
}
|
||||
|
||||
int total_iters = 0;
|
||||
mjtNum sum_rel_err = 0;
|
||||
mjtNum max_rel_err = 0;
|
||||
|
||||
for (int e = 0; e < kNumEval; e++) {
|
||||
int idx = e * kStride;
|
||||
|
||||
mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
|
||||
mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
|
||||
mju_copy(data->qacc_warmstart, all_warmstart.data() + idx*nv, nv);
|
||||
|
||||
mj_forward(model, data);
|
||||
total_iters += get_total_solver_iters(data);
|
||||
|
||||
mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
|
||||
mjtNum err = 0;
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
|
||||
err += diff * diff;
|
||||
}
|
||||
mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
|
||||
sum_rel_err += rel_err;
|
||||
if (rel_err > max_rel_err) {
|
||||
max_rel_err = rel_err;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum solver_time = data->timer[mjTIMER_CONSTRAINT].duration;
|
||||
mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
|
||||
std::printf(" %6d | %11.4e | %11.4e | %10.2f | %11.2f\n",
|
||||
kIterCounts[c], sum_rel_err / kNumEval, max_rel_err,
|
||||
mean_iters, solver_time);
|
||||
}
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+-----------");
|
||||
}
|
||||
|
||||
// run pipeline mode: consecutive steps with tolerance, print summary
|
||||
void run_pipeline(mjModel* model, mjData* data, int kNumSteps,
|
||||
const char* label) {
|
||||
std::printf("\n %s Pipeline mode (mj_step, tolerance = 1e-8):\n", label);
|
||||
|
||||
mj_resetData(model, data);
|
||||
for (int i = 0; i < mjNTIMER; i++) {
|
||||
data->timer[i].duration = 0;
|
||||
data->timer[i].number = 0;
|
||||
}
|
||||
|
||||
int pipe_total_iters = 0;
|
||||
for (int i = 0; i < kNumSteps; i++) {
|
||||
mj_step(model, data);
|
||||
pipe_total_iters += get_total_solver_iters(data);
|
||||
}
|
||||
|
||||
mjtNum pipe_constraint = data->timer[mjTIMER_CONSTRAINT].duration;
|
||||
mjtNum pipe_step = data->timer[mjTIMER_STEP].duration;
|
||||
int pipe_step_count = data->timer[mjTIMER_STEP].number;
|
||||
|
||||
std::printf(" Steps/s : %.0f\n",
|
||||
pipe_step_count > 0 ? 1e6 * pipe_step_count / pipe_step : 0.0);
|
||||
std::printf(" us/step (total) : %.1f\n",
|
||||
pipe_step_count > 0 ? pipe_step / pipe_step_count : 0.0);
|
||||
std::printf(" us/step (constr) : %.1f\n",
|
||||
pipe_step_count > 0 ? pipe_constraint / pipe_step_count : 0.0);
|
||||
std::printf(" Iters/step : %.2f\n",
|
||||
pipe_step_count > 0
|
||||
? static_cast<mjtNum>(pipe_total_iters) / pipe_step_count
|
||||
: 0.0);
|
||||
}
|
||||
|
||||
TEST_F(PgsConvergenceTest, PGSConvergence) {
|
||||
static const char* const kPath =
|
||||
"engine/testdata/forward/perf/2humanoid100_PGS.xml";
|
||||
const std::string xml_path = GetTestDataFilePath(kPath);
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
// disable islands: monolithic solver makes statistics simpler
|
||||
model->opt.disableflags |= mjDSBL_ISLAND;
|
||||
|
||||
int nq = model->nq;
|
||||
int nv = model->nv;
|
||||
|
||||
// stronger sideways gravity
|
||||
model->opt.gravity[0] = -2;
|
||||
model->opt.gravity[1] = -2;
|
||||
model->opt.gravity[2] = -10;
|
||||
|
||||
// configure Newton ground truth
|
||||
model->opt.solver = mjSOL_NEWTON;
|
||||
model->opt.jacobian = mjJAC_SPARSE;
|
||||
model->opt.tolerance = 0;
|
||||
model->opt.iterations = 10;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mjcb_time = gettm;
|
||||
|
||||
// roll out Newton for kNumSteps, save pre-step state and post-step qacc
|
||||
constexpr int kNumSteps = 1000;
|
||||
std::vector<mjtNum> all_qpos(kNumSteps * nq);
|
||||
std::vector<mjtNum> all_qvel(kNumSteps * nv);
|
||||
std::vector<mjtNum> all_warmstart(kNumSteps * nv);
|
||||
std::vector<mjtNum> all_qacc(kNumSteps * nv);
|
||||
|
||||
std::printf("Rolling out ground truth Newton steps...\n");
|
||||
for (int i = 0; i < kNumSteps; i++) {
|
||||
mju_copy(all_qpos.data() + i*nq, data->qpos, nq);
|
||||
mju_copy(all_qvel.data() + i*nv, data->qvel, nv);
|
||||
mju_copy(all_warmstart.data() + i*nv, data->qacc_warmstart, nv);
|
||||
|
||||
mj_step(model, data);
|
||||
|
||||
mju_copy(all_qacc.data() + i*nv, data->qacc, nv);
|
||||
}
|
||||
|
||||
// evaluation points
|
||||
constexpr int kNumEval = 100;
|
||||
constexpr int kStride = kNumSteps / kNumEval;
|
||||
|
||||
// iteration counts to test
|
||||
constexpr int kIterCounts[] = {5, 10, 20, 40, 80, 160, 320};
|
||||
constexpr int kNumIter = sizeof(kIterCounts) / sizeof(kIterCounts[0]);
|
||||
|
||||
std::printf("\nPGS vs Nesterov PGS: 2humanoid100_PGS.xml\n");
|
||||
std::printf(" %d Newton steps, %d evaluation points\n",
|
||||
kNumSteps, kNumEval);
|
||||
std::printf(" nv = %d, nq = %d, nefc = %d\n", nv, nq, data->nefc);
|
||||
|
||||
// switch to PGS
|
||||
model->opt.solver = mjSOL_PGS;
|
||||
model->opt.tolerance = 0;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
// 1. Row PGS (default)
|
||||
mj_nesterov_momentum = 0;
|
||||
run_benchmark(model, data, all_qpos, all_qvel, all_warmstart, all_qacc,
|
||||
nq, nv, kStride, kNumEval, kIterCounts, kNumIter,
|
||||
"Row PGS (Warmstart, tolerance = 0)");
|
||||
|
||||
// 2. Nesterov PGS (via REFSAFE hack)
|
||||
mj_nesterov_momentum = 1;
|
||||
run_benchmark(model, data, all_qpos, all_qvel, all_warmstart, all_qacc,
|
||||
nq, nv, kStride, kNumEval, kIterCounts, kNumIter,
|
||||
"Nesterov PGS (Warmstart, tolerance = 0)");
|
||||
|
||||
// 3. Pipeline: Row PGS
|
||||
mj_nesterov_momentum = 0;
|
||||
model->opt.tolerance = 1e-8;
|
||||
model->opt.iterations = 100;
|
||||
run_pipeline(model, data, kNumSteps, "Row PGS");
|
||||
|
||||
// 4. Pipeline: Nesterov PGS
|
||||
mj_nesterov_momentum = 1;
|
||||
model->opt.tolerance = 1e-8;
|
||||
model->opt.iterations = 100;
|
||||
run_pipeline(model, data, kNumSteps, "Nesterov PGS");
|
||||
|
||||
std::printf("\n");
|
||||
|
||||
// ========== PASS 2: ISLANDS ENABLED ==========
|
||||
model->opt.disableflags &= ~mjDSBL_ISLAND;
|
||||
|
||||
// do one forward pass to get nisland
|
||||
mj_resetData(model, data);
|
||||
mj_forward(model, data);
|
||||
std::printf("\n============================================\n");
|
||||
std::printf("PASS 2: ISLANDS ENABLED\n");
|
||||
std::printf("============================================\n");
|
||||
|
||||
// Pipeline: Row PGS with islands
|
||||
mj_nesterov_momentum = 0;
|
||||
model->opt.tolerance = 1e-8;
|
||||
model->opt.iterations = 100;
|
||||
run_pipeline(model, data, kNumSteps, "Row PGS (islands)");
|
||||
|
||||
// Pipeline: Nesterov PGS with islands
|
||||
mj_nesterov_momentum = 1;
|
||||
model->opt.tolerance = 1e-8;
|
||||
model->opt.iterations = 100;
|
||||
run_pipeline(model, data, kNumSteps, "Nesterov PGS (islands)");
|
||||
|
||||
// Reset to default
|
||||
mj_nesterov_momentum = 1;
|
||||
|
||||
std::printf("\n");
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
@@ -241,15 +241,15 @@ TEST_F(SolverTest, SolversEquivalent) {
|
||||
{
|
||||
.newton = MjTol(1e-13, 1e-5),
|
||||
.cg = MjTol(1e-13, 1e-5),
|
||||
.pgs_pyramidal = MjTol(1e-12, 1e-5),
|
||||
.pgs_elliptic = MjTol(1e-3, 1e-2),
|
||||
.pgs_pyramidal = MjTol(1e-13, 1e-5),
|
||||
.pgs_elliptic = MjTol(1e-3, 1e-3),
|
||||
}},
|
||||
{.path = kHumanoidPath,
|
||||
.tolerances =
|
||||
{
|
||||
.newton = MjTol(1e-13, 1e-5),
|
||||
.cg = MjTol(1e-12, 1e-5),
|
||||
.pgs_pyramidal = MjTol(1e-5, 1e-5),
|
||||
.pgs_pyramidal = MjTol(1e-12, 1e-5),
|
||||
.pgs_elliptic = MjTol(1e-8, 1e-4),
|
||||
}},
|
||||
};
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
<mujoco model="2 Humanoids and 100 objects">
|
||||
<option timestep="0.005" solver="PGS" gravity="-2 -2 -10">
|
||||
<flag island="enable"/>
|
||||
</option>
|
||||
|
||||
<size memory="100M"/>
|
||||
|
||||
<default>
|
||||
<geom solimp=".9 .9 .01"/>
|
||||
<default class="capsule">
|
||||
<geom type="capsule" material="capsule" size="0.1 0.05"/>
|
||||
</default>
|
||||
<default class="ellipsoid">
|
||||
<geom type="ellipsoid" material="ellipsoid" size="0.15 0.1 0.07"/>
|
||||
</default>
|
||||
<default class="box">
|
||||
<geom type="box" material="box" size="0.15 0.1 0.05"/>
|
||||
</default>
|
||||
<default class="cylinder">
|
||||
<geom type="cylinder" material="cylinder" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
||||
</default>
|
||||
<default class="sphere">
|
||||
<geom type="sphere" material="sphere" size="0.1"/>
|
||||
</default>
|
||||
<default class="border">
|
||||
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
|
||||
</default>
|
||||
<default class="borderpost">
|
||||
<geom type="box" size="0.41 0.41 0.41" rgba=".55 .55 .55 1"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<asset>
|
||||
<model file="humanoid.xml"/>
|
||||
<texture type="skybox" builtin="gradient" width="512" height="512" rgb1=".4 .6 .8" rgb2="0 0 0"/>
|
||||
<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6" width="512" height="512"/>
|
||||
<material name="MatPlane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true" rgba=".7 .7 .7 1"/>
|
||||
<material name="capsule" texture="texgeom" texuniform="true" rgba=".4 .9 .6 1"/>
|
||||
<material name="ellipsoid" texture="texgeom" texuniform="true" rgba=".4 .6 .9 1"/>
|
||||
<material name="box" texture="texgeom" texuniform="true" rgba=".4 .9 .9 1"/>
|
||||
<material name="cylinder" texture="texgeom" texuniform="true" rgba=".8 .6 .8 1"/>
|
||||
<material name="sphere" texture="texgeom" texuniform="true" rgba=".9 .1 .1 1"/>
|
||||
</asset>
|
||||
|
||||
<visual>
|
||||
<quality shadowsize="4096" offsamples="8"/>
|
||||
<map znear="0.1" force="0.05"/>
|
||||
</visual>
|
||||
|
||||
<statistic extent="4"/>
|
||||
|
||||
<worldbody>
|
||||
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||
<geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
|
||||
<geom class="border" fromto="-3 3 0 3 3 0"/>
|
||||
<geom class="border" fromto="-3 -3 0 3 -3 0"/>
|
||||
<geom class="border" fromto="3 3 0 3 -3 0"/>
|
||||
<geom class="border" fromto="-3 3 0 -3 -3 0"/>
|
||||
<geom class="borderpost" pos="3 3 0"/>
|
||||
<geom class="borderpost" pos="-3 3 0"/>
|
||||
<geom class="borderpost" pos="3 -3 0"/>
|
||||
<geom class="borderpost" pos="-3 -3 0"/>
|
||||
|
||||
<replicate count="4" euler="0 0 90">
|
||||
<geom type="plane" size=".5 3 .05" zaxis="1 0 0" pos="-3 0 0.4"/>
|
||||
</replicate>
|
||||
|
||||
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
|
||||
<body pos="-2 0 0.5" euler="30 40 0">
|
||||
<freejoint/>
|
||||
<geom class="capsule"/>
|
||||
</body>
|
||||
</replicate>
|
||||
|
||||
<attach model="Humanoid" body="torso" prefix="1_"/>
|
||||
|
||||
<frame euler="0 0 72">
|
||||
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
|
||||
<body pos="-2 0 0.5" euler="20 40 60">
|
||||
<freejoint/>
|
||||
<geom class="ellipsoid"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<frame euler="0 0 144">
|
||||
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
|
||||
<body pos="-2 0 0.5" euler="30 70 110">
|
||||
<freejoint/>
|
||||
<geom class="box"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<frame pos="1 1 0" euler="0 0 144">
|
||||
<attach model="Humanoid" body="torso" prefix="2_"/>
|
||||
</frame>
|
||||
|
||||
<frame euler="0 0 216">
|
||||
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
|
||||
<body pos="-2 0 0.5" euler="60 30 0">
|
||||
<freejoint/>
|
||||
<geom class="cylinder"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<frame euler="0 0 288">
|
||||
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
|
||||
<body pos="-2 0 0.5" euler="60 30 0">
|
||||
<freejoint/>
|
||||
<geom class="sphere"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
+252
@@ -0,0 +1,252 @@
|
||||
<mujoco model="Humanoid">
|
||||
<option timestep="0.005"/>
|
||||
|
||||
<visual>
|
||||
<map force="0.1"/>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
|
||||
</visual>
|
||||
|
||||
<statistic center="0 0 0.7"/>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
|
||||
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
|
||||
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
|
||||
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
|
||||
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
|
||||
</asset>
|
||||
|
||||
<default>
|
||||
<motor ctrlrange="-1 1" ctrllimited="true"/>
|
||||
<default class="body">
|
||||
|
||||
<!-- geoms -->
|
||||
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
|
||||
<default class="thigh">
|
||||
<geom size=".06"/>
|
||||
</default>
|
||||
<default class="shin">
|
||||
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
|
||||
</default>
|
||||
<default class="foot">
|
||||
<geom size=".027"/>
|
||||
<default class="foot1">
|
||||
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
|
||||
</default>
|
||||
<default class="foot2">
|
||||
<geom fromto="-.07 .01 0 .14 .03 0"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="arm_upper">
|
||||
<geom size=".04"/>
|
||||
</default>
|
||||
<default class="arm_lower">
|
||||
<geom size=".031"/>
|
||||
</default>
|
||||
<default class="hand">
|
||||
<geom type="sphere" size=".04"/>
|
||||
</default>
|
||||
|
||||
<!-- joints -->
|
||||
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
|
||||
<default class="joint_big">
|
||||
<joint damping="5" stiffness="10"/>
|
||||
<default class="hip_x">
|
||||
<joint range="-30 10"/>
|
||||
</default>
|
||||
<default class="hip_z">
|
||||
<joint range="-60 35"/>
|
||||
</default>
|
||||
<default class="hip_y">
|
||||
<joint axis="0 1 0" range="-150 20"/>
|
||||
</default>
|
||||
<default class="joint_big_stiff">
|
||||
<joint stiffness="20"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="knee">
|
||||
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
|
||||
</default>
|
||||
<default class="ankle">
|
||||
<joint range="-50 50"/>
|
||||
<default class="ankle_y">
|
||||
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
|
||||
</default>
|
||||
<default class="ankle_x">
|
||||
<joint pos="0 0 .04" stiffness="3"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="shoulder">
|
||||
<joint range="-85 60"/>
|
||||
</default>
|
||||
<default class="elbow">
|
||||
<joint range="-100 50" stiffness="0"/>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
|
||||
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
|
||||
<light name="top" pos="0 0 2" mode="trackcom"/>
|
||||
<body name="torso" pos="0 0 1.282" childclass="body">
|
||||
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
|
||||
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
|
||||
<freejoint name="root"/>
|
||||
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
|
||||
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
|
||||
<body name="head" pos="0 0 .19">
|
||||
<geom name="head" type="sphere" size=".09"/>
|
||||
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
|
||||
</body>
|
||||
<body name="waist_lower" pos="-.01 0 -.26">
|
||||
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
|
||||
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
|
||||
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
|
||||
<body name="pelvis" pos="0 0 -.165">
|
||||
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
|
||||
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
|
||||
<body name="thigh_right" pos="0 -.1 -.04">
|
||||
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
|
||||
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
|
||||
<joint name="hip_y_right" class="hip_y"/>
|
||||
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
|
||||
<body name="shin_right" pos="0 .01 -.4">
|
||||
<joint name="knee_right" class="knee"/>
|
||||
<geom name="shin_right" class="shin"/>
|
||||
<body name="foot_right" pos="0 0 -.39">
|
||||
<joint name="ankle_y_right" class="ankle_y"/>
|
||||
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
|
||||
<geom name="foot1_right" class="foot1"/>
|
||||
<geom name="foot2_right" class="foot2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="thigh_left" pos="0 .1 -.04">
|
||||
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
|
||||
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
|
||||
<joint name="hip_y_left" class="hip_y"/>
|
||||
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
|
||||
<body name="shin_left" pos="0 -.01 -.4">
|
||||
<joint name="knee_left" class="knee"/>
|
||||
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
|
||||
<body name="foot_left" pos="0 0 -.39">
|
||||
<joint name="ankle_y_left" class="ankle_y"/>
|
||||
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
|
||||
<geom name="foot1_left" class="foot1"/>
|
||||
<geom name="foot2_left" class="foot2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="upper_arm_right" pos="0 -.17 .06">
|
||||
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
|
||||
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
|
||||
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
|
||||
<body name="lower_arm_right" pos=".18 -.18 -.18">
|
||||
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
|
||||
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
|
||||
<body name="hand_right" pos=".18 .18 .18">
|
||||
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="upper_arm_left" pos="0 .17 .06">
|
||||
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
|
||||
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
|
||||
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
|
||||
<body name="lower_arm_left" pos=".18 .18 -.18">
|
||||
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
|
||||
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
|
||||
<body name="hand_left" pos=".18 -.18 .18">
|
||||
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<contact>
|
||||
<exclude body1="waist_lower" body2="thigh_right"/>
|
||||
<exclude body1="waist_lower" body2="thigh_left"/>
|
||||
</contact>
|
||||
|
||||
<tendon>
|
||||
<fixed name="hamstring_right" limited="true" range="-0.3 2">
|
||||
<joint joint="hip_y_right" coef=".5"/>
|
||||
<joint joint="knee_right" coef="-.5"/>
|
||||
</fixed>
|
||||
<fixed name="hamstring_left" limited="true" range="-0.3 2">
|
||||
<joint joint="hip_y_left" coef=".5"/>
|
||||
<joint joint="knee_left" coef="-.5"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
|
||||
<actuator>
|
||||
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
|
||||
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
|
||||
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
|
||||
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
|
||||
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
|
||||
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
|
||||
<motor name="knee_right" gear="80" joint="knee_right"/>
|
||||
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
|
||||
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
|
||||
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
|
||||
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
|
||||
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
|
||||
<motor name="knee_left" gear="80" joint="knee_left"/>
|
||||
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
|
||||
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
|
||||
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
|
||||
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
|
||||
<motor name="elbow_right" gear="40" joint="elbow_right"/>
|
||||
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
|
||||
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
|
||||
<motor name="elbow_left" gear="40" joint="elbow_left"/>
|
||||
</actuator>
|
||||
|
||||
<keyframe>
|
||||
<!--
|
||||
The values below are split into rows for readibility:
|
||||
torso position
|
||||
torso orientation
|
||||
spinal
|
||||
right leg
|
||||
left leg
|
||||
arms
|
||||
-->
|
||||
<key name="squat"
|
||||
qpos="0 0 0.596
|
||||
0.988015 0 0.154359 0
|
||||
0 0.4 0
|
||||
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
|
||||
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
|
||||
0 0 0 0 0 0"/>
|
||||
<key name="stand_on_left_leg"
|
||||
qpos="0 0 1.21948
|
||||
0.971588 -0.179973 0.135318 -0.0729076
|
||||
-0.0516 -0.202 0.23
|
||||
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
|
||||
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
|
||||
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
|
||||
<key name="prone"
|
||||
qpos="0.4 0 0.0757706
|
||||
0.7325 0 0.680767 0
|
||||
0 0.0729 0
|
||||
0.0077 0.0019 -0.026 -0.351 -0.27 0
|
||||
0.0077 0.0019 -0.026 -0.351 -0.27 0
|
||||
0.56 -0.62 -1.752
|
||||
0.56 -0.62 -1.752"/>
|
||||
<key name="supine"
|
||||
qpos="-0.4 0 0.08122
|
||||
0.722788 0 -0.69107 0
|
||||
0 -0.25 0
|
||||
0.0182 0.0142 0.3 0.042 -0.44 -0.02
|
||||
0.0182 0.0142 0.3 0.042 -0.44 -0.02
|
||||
0.186 -0.73 -1.73
|
||||
0.186 -0.73 -1.73"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user