diff --git a/doc/programming/samples.rst b/doc/programming/samples.rst index ee6c7feb..122867ee 100644 --- a/doc/programming/samples.rst +++ b/doc/programming/samples.rst @@ -21,34 +21,61 @@ the console. To simulate controlled dynamics instead of passive dynamics one can .. code-block:: Shell - testspeed modelfile [nstep nthread ctrlnoise npoolthread] + testspeed [options] model -Where the command line arguments are +Where the command-line options and arguments are .. list-table:: :width: 95% :align: left - :widths: 1 1 5 + :widths: 2 1 4 :header-rows: 1 - * - Argument + * - Option - Default - Meaning - * - ``modelfile`` + * - ``model`` - (required) - - path to model - * - ``nstep`` + - path to model (positional argument) + * - ``--nstep=N`` - 10000 - number of steps per rollout - * - ``nthread`` + * - ``--nthread=N`` - 1 - number of threads running parallel rollouts - * - ``ctrlnoise`` + * - ``--noisestd=X`` - 0.01 - scale of pseudo-random noise injected into actuators - * - ``npoolthread`` - - 1 + * - ``--noiserate=X`` + - 0.1 + - rate of convergence to ctrl keyframe/midpoint + * - ``--npoolthread=N`` + - 0 - number of threads in engine-internal threadpool + * - ``--solver=S`` + - Newton + - override constraint solver algorithm (PGS, CG, Newton) + * - ``--cone=C`` + - Pyramidal + - override friction cone type (Pyramidal, Elliptic) + * - ``--jacobian=J`` + - Auto + - override constraint Jacobian type (Dense, Sparse, Auto) + * - ``--integrator=I`` + - Euler + - override integration mode (Euler, RK4, Implicit, ImplicitFast) + * - ``--iterations=N`` + - 100 + - override solver iterations limit + * - ``--tolerance=X`` + - 1e-8 + - override solver convergence tolerance + * - ``--sleep_tolerance=X`` + - 1e-4 + - override sleep tolerance + * - ``--noslip_iterations=N`` + - 0 + - override noslip solver iterations limit **Notes:** @@ -58,8 +85,10 @@ Where the command line arguments are logical cores. - By default, the simulation starts from the model reference configuration with zero velocities. However, if a keyframe named "test" is present in the model, it is used as the initial state. -- The ``ctrlnoise`` argument prevents models from settling into a static state where, due to warmstarts, one can - measure artificially faster simulation. +- The physics option override flags (such as ``--solver``) only override the model settings if they are explicitly + specified on the command line; otherwise, the model options configured in the XML file are preserved. +- The control noise arguments (``noisestd`` and ``noiserate``) prevent models from settling into a static state where, + due to warmstarts, one can measure artificially faster simulation. - When ``npoolthread > 1`` is specified, an engine-internal thread pool is created with the specified number of threads, to speed up simulation of large scenes. Note that while it is possible to use both ``nthread`` and ``npoolthread``, the scenarios for which one would want these different types of multithreading are usually mutually diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 0cedb767..4dc3e623 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -12,31 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include +#include #include +#include #include #include #include #include #include +#include +#include #include #include #include - // maximum number of threads const int maxthread = 512; -// model and per-thread data -mjModel* m = NULL; -mjData* d[maxthread]; +// rollout runner state +struct RolloutRunner { + mjModel* m = nullptr; + mjData* d[maxthread] = {nullptr}; -// per-thread statistics -int contacts[maxthread]; -int constraints[maxthread]; -mjtNum iterations[maxthread]; -mjtNum simtime[maxthread]; + // per-thread statistics + int contacts[maxthread] = {0}; + int constraints[maxthread] = {0}; + mjtNum iterations[maxthread] = {0.0}; + mjtNum simtime[maxthread] = {0.0}; +}; + +static RolloutRunner runner; // timer (microseconds) mjtNum gettm(void) { @@ -61,33 +69,36 @@ int finish(const char* msg = NULL, mjModel* m = NULL) { return EXIT_SUCCESS; } -std::vector CtrlNoise(const mjModel* m, int nsteps, mjtNum ctrl_noise_std, mjtNum ctrl_noise_rate, int key) { +std::vector CtrlNoise(const mjModel* m, int nsteps, mjtNum noise_std, mjtNum noise_rate, + int key) { std::vector ctrl; + ctrl.reserve(nsteps * m->nu); // convert rate and scale to discrete time (Ornstein–Uhlenbeck) - mjtNum rate = mju_exp(-m->opt.timestep / ctrl_noise_rate); - mjtNum scale = ctrl_noise_std * mju_sqrt(1-rate*rate); + mjtNum rate = mju_exp(-m->opt.timestep / noise_rate); + mjtNum scale = noise_std * mju_sqrt(1 - rate * rate); - for (int step=0; step < nsteps; step++) { + for (int step = 0; step < nsteps; step++) { for (int i = 0; i < m->nu; i++) { mjtNum midpoint = 0.0; mjtNum halfrange = 1.0; mjtNum* range = m->actuator_ctrlrange + 2 * i; if (m->actuator_ctrllimited[i]) { - midpoint = 0.5 * (range[1] + range[0]); + midpoint = 0.5 * (range[1] + range[0]); halfrange = 0.5 * (range[1] - range[0]); } // overwrite midpoint with keyframe, if given if (key >= 0) { - midpoint = m->key_ctrl[key*m->nu+i]; + midpoint = m->key_ctrl[key * m->nu + i]; } // exponential convergence to midpoint at ctrl_noise_rate - mjtNum ctrl_ = step > 0 ? rate * ctrl[(step-1)*m->nu+i] + (1-rate) * midpoint : midpoint; + mjtNum ctrl_ = + step > 0 ? rate * ctrl[(step - 1) * m->nu + i] + (1 - rate) * midpoint : midpoint; // add noise - ctrl_ += scale * halfrange * (2 * mju_Halton(step, i+2) - 1); + ctrl_ += scale * halfrange * (2 * mju_Halton(step, i + 2) - 1); // clip to range if limited if (m->actuator_ctrllimited[i]) { @@ -100,136 +111,392 @@ std::vector CtrlNoise(const mjModel* m, int nsteps, mjtNum ctrl_noise_st return ctrl; } - // thread function void simulate(int id, int nstep, mjtNum* ctrl) { // clear statistics - contacts[id] = 0; - constraints[id] = 0; - iterations[id] = 0; + runner.contacts[id] = 0; + runner.constraints[id] = 0; + runner.iterations[id] = 0; // run and time mjtNum start = gettm(); - for (int i=0; i < nstep; i++) { + for (int i = 0; i < nstep; i++) { // inject pseudo-random control noise - mju_copy(d[id]->ctrl, ctrl + i*m->nu, m->nu); + mju_copy(runner.d[id]->ctrl, ctrl + i * runner.m->nu, runner.m->nu); // advance simulation - mj_step(m, d[id]); + mj_step(runner.m, runner.d[id]); // accumulate statistics - contacts[id] += d[id]->ncon; - constraints[id] += d[id]->nefc; - int nisland = mjMAX(1, mjMIN(d[id]->nisland, mjNISLAND)); + runner.contacts[id] += runner.d[id]->ncon; + runner.constraints[id] += runner.d[id]->nefc; + int nisland = mjMAX(1, mjMIN(runner.d[id]->nisland, mjNISLAND)); if (nisland == 1 || nisland == 0) { - iterations[id] += d[id]->solver_niter[0]; + runner.iterations[id] += runner.d[id]->solver_niter[0]; } else { mjtNum niter = 0; - for (int j=0; j < nisland; j++) { - niter += d[id]->solver_niter[j]; + for (int j = 0; j < nisland; j++) { + niter += runner.d[id]->solver_niter[j]; } - iterations[id] += niter / nisland; + runner.iterations[id] += niter / nisland; } } - simtime[id] = 1e-6 * (gettm() - start); + runner.simtime[id] = 1e-6 * (gettm() - start); } +// print non-default options +static void PrintOptions(const mjModel* m) { + mjOption optd; + mj_defaultOption(&optd); + + bool header_printed = false; + auto print_header = [&]() { + if (!header_printed) { + std::printf("\nPhysics options (non-default):\n"); + header_printed = true; + } + }; + +#define X(type, name, size) \ + if (std::strcmp(#name, "disableflags") != 0 && std::strcmp(#name, "enableflags") != 0 && \ + std::strcmp(#name, "disableactuator") != 0) { \ + if (m->opt.name != optd.name) { \ + print_header(); \ + std::printf(" %-18s: ", #name); \ + if (std::strcmp(#name, "integrator") == 0) { \ + const char* names[] = {"Euler", "RK4", "Implicit", "ImplicitFast"}; \ + int val = (int)m->opt.name; \ + if (val >= 0 && val < 4) { \ + std::printf("%s", names[val]); \ + } else { \ + std::printf("%d", val); \ + } \ + } else if (std::strcmp(#name, "cone") == 0) { \ + const char* names[] = {"Pyramidal", "Elliptic"}; \ + int val = (int)m->opt.name; \ + if (val >= 0 && val < 2) { \ + std::printf("%s", names[val]); \ + } else { \ + std::printf("%d", val); \ + } \ + } else if (std::strcmp(#name, "jacobian") == 0) { \ + const char* names[] = {"Dense", "Sparse", "Auto"}; \ + int val = (int)m->opt.name; \ + if (val >= 0 && val < 3) { \ + std::printf("%s", names[val]); \ + } else { \ + std::printf("%d", val); \ + } \ + } else if (std::strcmp(#name, "solver") == 0) { \ + const char* names[] = {"PGS", "CG", "Newton"}; \ + int val = (int)m->opt.name; \ + if (val >= 0 && val < 3) { \ + std::printf("%s", names[val]); \ + } else { \ + std::printf("%d", val); \ + } \ + } else { \ + if (std::strcmp(#type, "int") == 0) { \ + std::printf("%d", (int)m->opt.name); \ + } else { \ + std::printf("%g", (double)m->opt.name); \ + } \ + } \ + std::printf("\n"); \ + } \ + } + +#define XVEC(type, name, size) \ + { \ + bool diff = false; \ + for (int i = 0; i < size; ++i) { \ + if (m->opt.name[i] != optd.name[i]) { \ + diff = true; \ + break; \ + } \ + } \ + if (diff) { \ + print_header(); \ + std::printf(" %-18s:", #name); \ + for (int i = 0; i < size; ++i) { \ + std::printf(" %g", (double)m->opt.name[i]); \ + } \ + std::printf("\n"); \ + } \ + } + + // option fields +#include // NOLINT(build/include) + MJOPTION_FIELDS + +#undef X +#undef XVEC + + // disableflags + for (int i = 0; i < mjNDISABLE; ++i) { + bool current = (m->opt.disableflags & (1 << i)) != 0; + bool def = (optd.disableflags & (1 << i)) != 0; + if (current != def) { + print_header(); + std::printf(" %-18s: %s\n", mjDISABLESTRING[i], current ? "Disabled" : "Enabled"); + } + } + + // enableflags + for (int i = 0; i < mjNENABLE; ++i) { + bool current = (m->opt.enableflags & (1 << i)) != 0; + bool def = (optd.enableflags & (1 << i)) != 0; + if (current != def) { + print_header(); + std::printf(" %-18s: %s\n", mjENABLESTRING[i], current ? "Enabled" : "Disabled"); + } + } + + // disableactuator + if (m->opt.disableactuator != optd.disableactuator) { + print_header(); + std::printf(" disableactuator :"); + bool first = true; + for (int i = 0; i < 32; ++i) { + if (m->opt.disableactuator & (1 << i)) { + if (!first) std::printf(","); + std::printf(" %d", i); + first = false; + } + } + if (first) { + std::printf(" none"); + } + std::printf("\n"); + } +} + +// helper enum parser: returns the matched enum index from the names vector (case-insensitive) +// or the parsed integer value if it represents a valid number. Returns -1 on failure. +static int ParseEnum(std::string_view val, const std::vector& names) { + int int_val; + auto [ptr, ec] = std::from_chars(val.data(), val.data() + val.size(), int_val); + if (ec == std::errc()) { + return int_val; + } + for (size_t i = 0; i < names.size(); ++i) { + if (val.size() == names[i].size() && + std::equal(val.begin(), val.end(), names[i].begin(), [](unsigned char a, unsigned char b) { + return std::tolower(a) == std::tolower(b); + })) { + return i; + } + } + return -1; +} // main function int main(int argc, char** argv) { + static const char* help_msg = + "\n" + "Usage: testspeed [options] model\n" + "\n" + " option default semantic\n" + " ------ ------- --------\n" + " model path to model (required, positional)\n" + " --nstep=N 10000 number of steps per rollout\n" + " --nthread=N 1 number of threads running parallel rollouts\n" + " --noisestd=X 0.01 scale of pseudo-random noise injected into actuators\n" + " --noiserate=X 0.1 rate of convergence to ctrl keyframe/midpoint\n" + " --npoolthread=N 0 number of threads in engine-internal threadpool\n" + " --solver=S Newton PGS, CG, Newton\n" + " --cone=C Pyramidal Pyramidal, Elliptic\n" + " --jacobian=J Auto Dense, Sparse, Auto\n" + " --integrator=I Euler Euler, RK4, Implicit, ImplicitFast\n" + " --iterations=N 100 solver iterations limit\n" + " --tolerance=X 1e-8 solver tolerance\n" + " --sleep_tolerance=X 1e-4 sleep tolerance\n" + " --noslip_iterations=N 0 noslip solver iterations limit\n" + " --help (or no arguments) print this help message\n" + "\n" + "Note: If the model has a keyframe named \"test\", it will be loaded prior to simulation\n"; - // print help if arguments are missing - if (argc < 2 || argc > 7) { - return finish( - "\n" - "Usage: testspeed modelfile [nstep nthread ctrlnoisestd ctrlnoiserate npoolthread]\n" - "\n" - " argument default semantic\n" - " -------- ------- --------\n" - " modelfile path to model (required)\n" - " nstep 10000 number of steps per rollout\n" - " nthread 1 number of threads running parallel rollouts\n" - " ctrlnoisestd 0.01 scale of pseudo-random noise injected into actuators\n" - " ctrlnoiserate 0.1 rate of convergence to ctrl keyframe/midpoint\n" - " npoolthread 0 number of threads in engine-internal threadpool\n" - "\n" - "Note: If the model has a keyframe named \"test\", it will be loaded prior to simulation\n"); - } - - // read arguments + // default values int nstep = 10000, nthread = 0, npoolthread = 0; // inject small noise by default, to avoid fixed contact state - double ctrlnoisestd = 0.01; - double ctrlnoiserate = 0.1; - if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) { - return finish("Invalid nstep argument"); - } - if (argc > 3 && std::sscanf(argv[3], "%d", &nthread) != 1) { - return finish("Invalid nthread argument"); - } - if (argc > 4 && std::sscanf(argv[4], "%lf", &ctrlnoisestd) != 1) { - return finish("Invalid ctrlnoisestd argument"); - } - if (argc > 5 && std::sscanf(argv[5], "%lf", &ctrlnoiserate) != 1) { - return finish("Invalid ctrlnoiserate argument"); - } - if (argc > 6 && std::sscanf(argv[6], "%d", &npoolthread) != 1) { - return finish("Invalid npoolthread argument"); + double noisestd = 0.01; + double noiserate = 0.1; + + // option override settings + bool set_solver = false, set_cone = false, set_jacobian = false, set_integrator = false; + bool set_iterations = false, set_tolerance = false, set_sleep_tolerance = false; + bool set_noslip_iterations = false; + int opt_solver = -1, opt_cone = -1, opt_jacobian = -1, opt_integrator = -1; + int opt_iterations = -1, opt_noslip_iterations = -1; + double opt_tolerance = -1.0, opt_sleep_tolerance = -1.0; + + const char* model = nullptr; + for (int i = 1; i < argc; i++) { + // helper: given "--key=value" or "--key value", extract the value string + auto getarg = [&](const char* key) -> const char* { + std::string prefix = std::string("--") + key + "="; + if (std::strncmp(argv[i], prefix.c_str(), prefix.size()) == 0) { + return argv[i] + prefix.size(); + } + if (std::strcmp(argv[i], (std::string("--") + key).c_str()) == 0) { + if (i + 1 < argc) { + return argv[++i]; + } + } + return nullptr; + }; + + if (std::strcmp(argv[i], "--help") == 0 || std::strcmp(argv[i], "-h") == 0) { + return finish(help_msg); + } + + const char* val; + if ((val = getarg("nstep"))) { + if (std::sscanf(val, "%d", &nstep) != 1 || nstep <= 0) { + return finish("Invalid --nstep argument"); + } + } else if ((val = getarg("nthread"))) { + if (std::sscanf(val, "%d", &nthread) != 1) { + return finish("Invalid --nthread argument"); + } + } else if ((val = getarg("noisestd"))) { + if (std::sscanf(val, "%lf", &noisestd) != 1) { + return finish("Invalid --noisestd argument"); + } + } else if ((val = getarg("noiserate"))) { + if (std::sscanf(val, "%lf", &noiserate) != 1) { + return finish("Invalid --noiserate argument"); + } + } else if ((val = getarg("npoolthread"))) { + if (std::sscanf(val, "%d", &npoolthread) != 1) { + return finish("Invalid --npoolthread argument"); + } + } else if ((val = getarg("solver"))) { + int parsed = ParseEnum(val, {"PGS", "CG", "Newton"}); + if (parsed < 0 || parsed > 2) { + return finish("Invalid --solver argument"); + } + opt_solver = parsed; + set_solver = true; + } else if ((val = getarg("cone"))) { + int parsed = ParseEnum(val, {"Pyramidal", "Elliptic"}); + if (parsed < 0 || parsed > 1) { + return finish("Invalid --cone argument"); + } + opt_cone = parsed; + set_cone = true; + } else if ((val = getarg("jacobian"))) { + int parsed = ParseEnum(val, {"Dense", "Sparse", "Auto"}); + if (parsed < 0 || parsed > 2) { + return finish("Invalid --jacobian argument"); + } + opt_jacobian = parsed; + set_jacobian = true; + } else if ((val = getarg("integrator"))) { + int parsed = ParseEnum(val, {"Euler", "RK4", "Implicit", "ImplicitFast"}); + if (parsed < 0 || parsed > 3) { + return finish("Invalid --integrator argument"); + } + opt_integrator = parsed; + set_integrator = true; + } else if ((val = getarg("iterations"))) { + if (std::sscanf(val, "%d", &opt_iterations) != 1 || opt_iterations <= 0) { + return finish("Invalid --iterations argument"); + } + set_iterations = true; + } else if ((val = getarg("tolerance"))) { + if (std::sscanf(val, "%lf", &opt_tolerance) != 1 || opt_tolerance < 0) { + return finish("Invalid --tolerance argument"); + } + set_tolerance = true; + } else if ((val = getarg("sleep_tolerance"))) { + if (std::sscanf(val, "%lf", &opt_sleep_tolerance) != 1 || opt_sleep_tolerance < 0) { + return finish("Invalid --sleep_tolerance argument"); + } + set_sleep_tolerance = true; + } else if ((val = getarg("noslip_iterations"))) { + if (std::sscanf(val, "%d", &opt_noslip_iterations) != 1 || opt_noslip_iterations < 0) { + return finish("Invalid --noslip_iterations argument"); + } + set_noslip_iterations = true; + } else if (argv[i][0] != '-') { + // positional argument: model file + model = argv[i]; + } else { + std::printf("Unknown option: %s\n", argv[i]); + return finish(help_msg); + } } - // clamp ctrlnoisestd to [0.0, 1.0] - ctrlnoisestd = mju_clip(ctrlnoisestd, 0.0, 1.0); + // model file is required + if (!model) { + return finish(help_msg); + } - // clamp ctrlnoiserate to [0.0, 1.0] - ctrlnoiserate = mju_clip(ctrlnoiserate, 0.0, 1.0); + // clamp noisestd to [0.0, 1.0] + noisestd = mju_clip(noisestd, 0.0, 1.0); + + // clamp noiserate to [0.0, 1.0] + noiserate = mju_clip(noiserate, 0.0, 1.0); // clamp nthread to [1, maxthread] nthread = mjMAX(1, mjMIN(maxthread, nthread)); npoolthread = mjMAX(1, mjMIN(maxthread, npoolthread)); // get filename, determine file type - std::string filename(argv[1]); + std::string filename(model); bool binary = (filename.find(".mjb") != std::string::npos); // NOLINT // load model char error[1000] = "Could not load binary model"; if (binary) { - m = mj_loadModel(argv[1], 0); + runner.m = mj_loadModel(model, 0); } else { - m = mj_loadXML(argv[1], 0, error, 1000); + runner.m = mj_loadXML(model, 0, error, 1000); } - if (!m) { + if (!runner.m) { return finish(error); } + // apply command-line option overrides + if (set_solver) runner.m->opt.solver = opt_solver; + if (set_cone) runner.m->opt.cone = opt_cone; + if (set_jacobian) runner.m->opt.jacobian = opt_jacobian; + if (set_integrator) runner.m->opt.integrator = opt_integrator; + if (set_iterations) runner.m->opt.iterations = opt_iterations; + if (set_tolerance) runner.m->opt.tolerance = opt_tolerance; + if (set_sleep_tolerance) runner.m->opt.sleep_tolerance = opt_sleep_tolerance; + if (set_noslip_iterations) runner.m->opt.noslip_iterations = opt_noslip_iterations; + // make per-thread data - int testkey = mj_name2id(m, mjOBJ_KEY, "test"); - for (int id=0; id < nthread; id++) { + int testkey = mj_name2id(runner.m, mjOBJ_KEY, "test"); + for (int id = 0; id < nthread; id++) { // make mjData(s) - d[id] = mj_makeData(m); - if (!d[id]) { - return finish("Could not allocate mjData", m); + runner.d[id] = mj_makeData(runner.m); + if (!runner.d[id]) { + return finish("Could not allocate mjData", runner.m); } // reset to keyframe if (testkey >= 0) { - mj_resetDataKeyframe(m, d[id], testkey); + mj_resetDataKeyframe(runner.m, runner.d[id], testkey); } // make and bind threadpool if (npoolthread > 1) { - mju_threadpool(d[id], npoolthread); + mju_threadpool(runner.d[id], npoolthread); } } // install timer callback for profiling mjcb_time = gettm; + // print physics options if not default + PrintOptions(runner.m); + // print start - std::printf("\nRolling out %d steps%s at dt = %g", - nstep, - nthread > 1 ? " per thread" : "", - m->opt.timestep); + std::printf("\nRolling out %d steps%s at dt = %g", nstep, nthread > 1 ? " per thread" : "", + runner.m->opt.timestep); // print precision if (sizeof(mjtNum) == 4) { @@ -238,22 +505,22 @@ int main(int argc, char** argv) { std::printf(", using double precision"); } - // print threadpool size + // print thread pool size if (npoolthread > 1) { std::printf(", using %d threads", npoolthread); } - std::printf("...\n\n"); + std::printf("...\n"); // create pseudo-random control sequence - std::vector ctrl = CtrlNoise(m, nstep, ctrlnoisestd, ctrlnoiserate, testkey); + std::vector ctrl = CtrlNoise(runner.m, nstep, noisestd, noiserate, testkey); // run simulation, record total time std::thread th[maxthread]; double starttime = gettm(); - for (int id=0; id < nthread; id++) { + for (int id = 0; id < nthread; id++) { th[id] = std::thread(simulate, id, nstep, ctrl.data()); } - for (int id=0; id < nthread; id++) { + for (int id = 0; id < nthread; id++) { th[id].join(); } double tottime = 1e-6 * (gettm() - starttime); // total time, in seconds @@ -263,41 +530,44 @@ int main(int argc, char** argv) { if (nthread > 1) { std::printf("Summary for all %d threads\n\n", nthread); std::printf(" Total simulation time : %.2f s\n", tottime); - std::printf(" Total steps per second : %.0f\n", nthread*nstep/tottime); - std::printf(" Total realtime factor : %.2f x\n", nthread*nstep*m->opt.timestep/tottime); - std::printf(" Total time per step : %.1f %ss\n\n", 1e6*tottime/(nthread*nstep), mu_str); + std::printf(" Total steps per second : %.0f\n", nthread * nstep / tottime); + std::printf(" Total realtime factor : %.2f x\n", + nthread * nstep * runner.m->opt.timestep / tottime); + std::printf(" Total time per step : %.1f %ss\n\n", 1e6 * tottime / (nthread * nstep), + mu_str); std::printf("Details for thread 0\n\n"); } // solver names indexed by mjtSolver - const char* solver[] = {"PGS", "CG", "Newton"}; + const char* solver_names[] = {"PGS", "CG", "Newton"}; const char* solto6[] = {" ", " ", ""}; // complete to 6 characters // details for thread 0 - std::printf(" Simulation time : %.2f s\n", simtime[0]); - std::printf(" Steps per second : %.0f\n", nstep/simtime[0]); - std::printf(" Realtime factor : %.2f x\n", nstep*m->opt.timestep/simtime[0]); - std::printf(" Time per step : %.1f %ss\n\n", 1e6*simtime[0]/nstep, mu_str); - std::printf(" %s iters / step %s: %.2f\n", - solver[m->opt.solver], solto6[m->opt.solver], iterations[0]/nstep); - std::printf(" Contacts / step : %.2f\n", static_cast(contacts[0])/nstep); - std::printf(" Constraints / step : %.2f\n", static_cast(constraints[0])/nstep); - std::printf(" Degrees of freedom : %" PRId64 "\n", m->nv); + std::printf(" Simulation time : %.2f s\n", runner.simtime[0]); + std::printf(" Steps per second : %.0f\n", nstep / runner.simtime[0]); + std::printf(" Realtime factor : %.2f x\n", + nstep * runner.m->opt.timestep / runner.simtime[0]); + std::printf(" Time per step : %.1f %ss\n\n", 1e6 * runner.simtime[0] / nstep, mu_str); + std::printf(" %s iters / step %s: %.2f\n", solver_names[runner.m->opt.solver], + solto6[runner.m->opt.solver], runner.iterations[0] / nstep); + std::printf(" Contacts / step : %.2f\n", static_cast(runner.contacts[0]) / nstep); + std::printf(" Constraints / step : %.2f\n", static_cast(runner.constraints[0]) / nstep); + std::printf(" Degrees of freedom : %" PRId64 "\n", runner.m->nv); std::printf(" Dynamic memory usage : %.1f%% of %s\n\n", - 100 * d[0]->maxuse_arena / (double)(d[0]->narena), - mju_writeNumBytes(d[0]->narena)); + 100 * runner.d[0]->maxuse_arena / (double)(runner.d[0]->narena), + mju_writeNumBytes(runner.d[0]->narena)); // profiler, top-level printf(" Internal profiler%s, %ss per step\n", nthread > 1 ? " for thread 0" : "", mu_str); - int number = d[0]->timer[mjTIMER_STEP].number; - mjtNum tstep = number ? d[0]->timer[mjTIMER_STEP].duration/number : 0.0; + int number = runner.d[0]->timer[mjTIMER_STEP].number; + mjtNum tstep = number ? runner.d[0]->timer[mjTIMER_STEP].duration / number : 0.0; mjtNum components = 0, total = 0; - for (int i=0; i <= mjTIMER_ADVANCE; i++) { - if (d[0]->timer[i].number > 0) { - int number = d[0]->timer[i].number; - mjtNum istep = number ? d[0]->timer[i].duration/number : 0.0; - mjtNum percent = number ? 100*istep/tstep : 0.0; + for (int i = 0; i <= mjTIMER_ADVANCE; i++) { + if (runner.d[0]->timer[i].number > 0) { + int number = runner.d[0]->timer[i].number; + mjtNum istep = number ? runner.d[0]->timer[i].duration / number : 0.0; + mjtNum percent = number ? 100 * istep / tstep : 0.0; std::printf(" %17s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[i], istep, percent); // save step time, add up timing of components @@ -311,43 +581,38 @@ int main(int argc, char** argv) { // "other" (computation not covered by timers) if (tstep > 0) { mjtNum other = total - components; - std::printf(" %17s : %6.1f (%6.2f %%)\n", "other", other, 100*other/tstep); + std::printf(" %17s : %6.1f (%6.2f %%)\n", "other", other, 100 * other / tstep); } std::printf("\n"); // mjTIMER_POSITION and its components - for (int i : {mjTIMER_POSITION, - mjTIMER_POS_KINEMATICS, - mjTIMER_POS_INERTIA, - mjTIMER_POS_COLLISION, - mjTIMER_POS_MAKE, - mjTIMER_POS_PROJECT}) { - if (d[0]->timer[i].number > 0) { - mjtNum istep = d[0]->timer[i].duration/d[0]->timer[i].number; + for (int i : {mjTIMER_POSITION, mjTIMER_POS_KINEMATICS, mjTIMER_POS_INERTIA, + mjTIMER_POS_COLLISION, mjTIMER_POS_MAKE, mjTIMER_POS_PROJECT}) { + if (runner.d[0]->timer[i].number > 0) { + mjtNum istep = runner.d[0]->timer[i].duration / runner.d[0]->timer[i].number; if (i == mjTIMER_POSITION) { - std::printf(" position total : %6.1f (%6.2f %%)\n", istep, 100*istep/tstep); + std::printf(" position total : %6.1f (%6.2f %%)\n", istep, 100 * istep / tstep); } else { - std::printf(" %-10s : %6.1f (%6.2f %%)\n", - mjTIMERSTRING[i]+4, istep, 100*istep/tstep); + std::printf(" %-10s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[i] + 4, istep, + 100 * istep / tstep); } } // components of mjTIMER_POS_COLLISION if (i == mjTIMER_POS_COLLISION) { for (int j : {mjTIMER_COL_BROAD, mjTIMER_COL_NARROW}) { - int number = d[0]->timer[j].number; - mjtNum jstep = number ? d[0]->timer[j].duration/number : 0.0; - mjtNum percent = number ? 100*jstep/tstep : 0.0; - std::printf(" %-11s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[j]+4, jstep, percent); + int number = runner.d[0]->timer[j].number; + mjtNum jstep = number ? runner.d[0]->timer[j].duration / number : 0.0; + mjtNum percent = number ? 100 * jstep / tstep : 0.0; + std::printf(" %-11s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[j] + 4, jstep, percent); } } } - // free per-thread data - for (int id=0; id < nthread; id++) { - mj_deleteData(d[id]); + for (int id = 0; id < nthread; id++) { + mj_deleteData(runner.d[id]); } // finalize diff --git a/test/sample/CMakeLists.txt b/test/sample/CMakeLists.txt index 15f87dc2..0d1061e3 100644 --- a/test/sample/CMakeLists.txt +++ b/test/sample/CMakeLists.txt @@ -16,4 +16,5 @@ if(MUJOCO_BUILD_EXAMPLES) include(ShellTests) add_mujoco_shell_test(compile_test compile) + add_mujoco_shell_test(testspeed_test testspeed) endif() diff --git a/test/sample/testspeed_test.sh b/test/sample/testspeed_test.sh new file mode 100755 index 00000000..3448ac95 --- /dev/null +++ b/test/sample/testspeed_test.sh @@ -0,0 +1,72 @@ +#!/bin/bash +# 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. + +MODEL="${CMAKE_SOURCE_DIR}/model/sleep/dominos.xml" + +die() { echo "$*" 1>&2 ; exit 1; } + +if [ -z "$TARGET_BINARY" ]; then + die "Expecting environment variable TARGET_BINARY." +fi + +if [ -z "$MUJOCO_DLL_DIR" ]; then + # Extend PATH to include the directory containing the mujoco DLL. + # This is needed on Windows. + PATH=$PATH:$MUJOCO_DLL_DIR +fi + +# Run testspeed with override flags and verify they get applied +echo "Running testspeed with command-line overrides on dominos.xml..." +echo "Command: $TARGET_BINARY --nstep=10 --nthread=1 --noisestd=0.02 --noiserate=0.2 --solver=PGS --jacobian=sparse --integrator=rk4 --iterations=12 --tolerance=1e-7 --sleep_tolerance=2e-4 --noslip_iterations=5 $MODEL" + +OUTPUT=$("$TARGET_BINARY" --nstep=10 --nthread=1 --noisestd=0.02 --noiserate=0.2 --solver=PGS --jacobian=sparse --integrator=rk4 --iterations=12 --tolerance=1e-7 --sleep_tolerance=2e-4 --noslip_iterations=5 "$MODEL") || die "testspeed failed" + +# Verify option printing works. +# Dominos.xml has: +# cone="elliptic" -> cone: Elliptic +# impratio="10" -> impratio: 10 +# -> Sleep: Enabled +# +# Overridden via command line options: +# solver=PGS +# jacobian=sparse -> jacobian: Sparse +# integrator=rk4 -> integrator: RK4 +# iterations=12 +# tolerance=1e-7 -> tolerance: 1e-07 +# sleep_tolerance=2e-4 (overwrites XML's 3e-4) -> sleep_tolerance: 0.0002 +# noslip_iterations=5 + +echo "Full testspeed Output:" +echo "--------------------------------------------------------" +echo "$OUTPUT" +echo "--------------------------------------------------------" + +echo "Verifying printed non-default option block:" +echo "$OUTPUT" | grep -A 10 "Physics options (non-default):" || die "Missing physics options block!" +echo "--------------------------------------------------------" + +echo "$OUTPUT" | grep -F "Physics options (non-default):" || die "Missing non-default header" +echo "$OUTPUT" | grep -F " cone : Elliptic" || die "Missing cone print" +echo "$OUTPUT" | grep -F " impratio : 10" || die "Missing impratio print" +echo "$OUTPUT" | grep -E " sleep_tolerance : (0.0002|2e-0?4)" || die "Missing sleep_tolerance print" +echo "$OUTPUT" | grep -F " Sleep : Enabled" || die "Missing Sleep print" +echo "$OUTPUT" | grep -F " solver : PGS" || die "Missing solver print" +echo "$OUTPUT" | grep -F " jacobian : Sparse" || die "Missing jacobian print" +echo "$OUTPUT" | grep -F " integrator : RK4" || die "Missing integrator print" +echo "$OUTPUT" | grep -F " iterations : 12" || die "Missing iterations print" +echo "$OUTPUT" | grep -E " tolerance : (1e-0?7|0.0000001|1e-7)" || die "Missing tolerance print" +echo "$OUTPUT" | grep -F " noslip_iterations : 5" || die "Missing noslip_iterations print" + +echo "PASS"