Yuval Tassa
abc02b736e
- When `mjData.nefc` was 0 (solver not run at all), we were plotting the stale diagnostics from the last run; now fixed. - In the case where islands were used, the semantic of the plotted diagnostic `sqrt_nnz` (square root of Hessian nonzeros) is changed. It used to be the sum of square roots of solver nonzeros per island, it is now the square root of the sum of all nonzeros in all islands. The semantic `sqrt(sum(nnz_i))` was found to be more consistent than `sum(sqrt(nnz_i))`, in the sense that switching islanding on/off leads to smaller changes and is more in line with the interpretation of islanding as [block-diagonalization](https://mujoco.readthedocs.io/en/3.3.7/computation/#constraint-islands). PiperOrigin-RevId: 827624908 Change-Id: I5fc86364ead585a4752a6d44823fb75cf6c7c318
2984 lines
96 KiB
C++
2984 lines
96 KiB
C++
// Copyright 2021 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.
|
||
|
||
#include "simulate.h"
|
||
|
||
#include <algorithm>
|
||
#include <atomic>
|
||
#include <chrono>
|
||
#include <climits>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <memory>
|
||
#include <optional>
|
||
#include <ratio>
|
||
#include <string>
|
||
#include <type_traits>
|
||
#include <utility>
|
||
|
||
#include "lodepng.h"
|
||
#include <mujoco/mjdata.h>
|
||
#include <mujoco/mjui.h>
|
||
#include <mujoco/mjvisualize.h>
|
||
#include <mujoco/mjxmacro.h>
|
||
#include <mujoco/mujoco.h>
|
||
#include "platform_ui_adapter.h"
|
||
#include "array_safety.h"
|
||
|
||
// When launched via an App Bundle on macOS, the working directory is the path to the App Bundle's
|
||
// resource directory. This causes files to be saved into the bundle, which is not the desired
|
||
// behavior. Instead, we open a save dialog box to ask the user where to put the file.
|
||
// Since the dialog box logic needs to be written in Objective-C, we separate it into a different
|
||
// source file.
|
||
#ifdef __APPLE__
|
||
std::string GetSavePath(const char* filename);
|
||
#else
|
||
static std::string GetSavePath(const char* filename) {
|
||
return filename;
|
||
}
|
||
#endif
|
||
|
||
namespace {
|
||
namespace mj = ::mujoco;
|
||
namespace mju = ::mujoco::sample_util;
|
||
|
||
using Seconds = std::chrono::duration<double>;
|
||
using Milliseconds = std::chrono::duration<double, std::milli>;
|
||
|
||
template <typename T>
|
||
inline bool IsDifferent(const T& a, const T& b) {
|
||
if constexpr (std::is_array_v<T>) {
|
||
static_assert(std::rank_v<T> == 1);
|
||
for (int i = 0; i < std::extent_v<T>; ++i) {
|
||
if (a[i] != b[i]) {
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
} else {
|
||
return a != b;
|
||
}
|
||
}
|
||
|
||
template <typename T>
|
||
inline void CopyScalar(T& dst, const T& src) {
|
||
dst = src;
|
||
}
|
||
|
||
template <typename T, int N>
|
||
inline void CopyArray(T (&dst)[N], const T (&src)[N]) {
|
||
for (int i = 0; i < N; ++i) {
|
||
dst[i] = src[i];
|
||
}
|
||
}
|
||
|
||
template <typename T>
|
||
inline void Copy(T& dst, const T& src) {
|
||
if constexpr (std::is_array_v<T>) {
|
||
CopyArray(dst, src);
|
||
} else {
|
||
CopyScalar(dst, src);
|
||
}
|
||
}
|
||
|
||
//------------------------------------------- global -----------------------------------------------
|
||
|
||
const double zoom_increment = 0.02; // ratio of one click-wheel zoom increment to vertical extent
|
||
|
||
// section ids
|
||
enum {
|
||
// left ui
|
||
SECT_FILE = 0,
|
||
SECT_OPTION,
|
||
SECT_SIMULATION,
|
||
SECT_WATCH,
|
||
SECT_PHYSICS,
|
||
SECT_RENDERING,
|
||
SECT_VISUALIZATION,
|
||
SECT_GROUP,
|
||
NSECT0,
|
||
|
||
// right ui
|
||
SECT_JOINT = 0,
|
||
SECT_CONTROL,
|
||
SECT_EQUALITY,
|
||
NSECT1
|
||
};
|
||
|
||
// file section of UI
|
||
const mjuiDef defFile[] = {
|
||
{mjITEM_SECTION, "File", mjPRESERVE, nullptr, "AF"},
|
||
{mjITEM_BUTTON, "Save xml", 2, nullptr, ""},
|
||
{mjITEM_BUTTON, "Save mjb", 2, nullptr, ""},
|
||
{mjITEM_BUTTON, "Print model", 2, nullptr, "CM"},
|
||
{mjITEM_BUTTON, "Print data", 2, nullptr, "CD"},
|
||
{mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"},
|
||
{mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// help strings
|
||
const char help_content[] =
|
||
"Space\n"
|
||
"+ -\n"
|
||
"Left / Right arrow\n"
|
||
"Tab / Shift-Tab\n"
|
||
"[ ]\n"
|
||
"Esc\n"
|
||
"Double-click\n"
|
||
"Page Up\n"
|
||
"Right double-click\n"
|
||
"Ctrl Right double-click\n"
|
||
"Scroll, middle drag\n"
|
||
"Left drag\n"
|
||
"[Shift] right drag\n"
|
||
"Ctrl [Shift] drag\n"
|
||
"Ctrl [Shift] right drag\n"
|
||
"F1\n"
|
||
"F2\n"
|
||
"F3\n"
|
||
"F4\n"
|
||
"F5\n"
|
||
"UI right-button hold\n"
|
||
"UI title double-click";
|
||
|
||
const char help_title[] =
|
||
"Play / Pause\n"
|
||
"Speed Up / Down\n"
|
||
"Step Back / Forward\n"
|
||
"Toggle Left / Right UI\n"
|
||
"Cycle cameras\n"
|
||
"Free camera\n"
|
||
"Select\n"
|
||
"Select parent\n"
|
||
"Center camera\n"
|
||
"Tracking camera\n"
|
||
"Zoom\n"
|
||
"View Orbit\n"
|
||
"View Pan\n"
|
||
"Object Rotate\n"
|
||
"Object Translate\n"
|
||
"Help\n"
|
||
"Info\n"
|
||
"Profiler\n"
|
||
"Sensors\n"
|
||
"Full screen\n"
|
||
"Show UI shortcuts\n"
|
||
"Expand/collapse all";
|
||
|
||
|
||
//-------------------------------- profiler, sensor, info, watch -----------------------------------
|
||
|
||
// number of lines in the Constraint ("Counts") and Cost ("Convergence") figures
|
||
static constexpr int kConstraintNum = 5;
|
||
static constexpr int kCostNum = 3;
|
||
|
||
// init profiler figures
|
||
void InitializeProfiler(mj::Simulate* sim) {
|
||
// set figures to default
|
||
mjv_defaultFigure(&sim->figconstraint);
|
||
mjv_defaultFigure(&sim->figcost);
|
||
mjv_defaultFigure(&sim->figtimer);
|
||
mjv_defaultFigure(&sim->figsize);
|
||
|
||
// titles
|
||
mju::strcpy_arr(sim->figconstraint.title, "Counts");
|
||
mju::strcpy_arr(sim->figcost.title, "Convergence (log 10)");
|
||
mju::strcpy_arr(sim->figsize.title, "Dimensions");
|
||
mju::strcpy_arr(sim->figtimer.title, "CPU time (msec)");
|
||
|
||
// x-labels
|
||
mju::strcpy_arr(sim->figconstraint.xlabel, "Solver iteration");
|
||
mju::strcpy_arr(sim->figcost.xlabel, "Solver iteration");
|
||
mju::strcpy_arr(sim->figsize.xlabel, "Video frame");
|
||
mju::strcpy_arr(sim->figtimer.xlabel, "Video frame");
|
||
|
||
// y-tick number formats
|
||
mju::strcpy_arr(sim->figconstraint.yformat, "%.0f");
|
||
mju::strcpy_arr(sim->figcost.yformat, "%.1f");
|
||
mju::strcpy_arr(sim->figsize.yformat, "%.0f");
|
||
mju::strcpy_arr(sim->figtimer.yformat, "%.2f");
|
||
|
||
// colors
|
||
sim->figconstraint.figurergba[0] = 0.1f;
|
||
sim->figcost.figurergba[2] = 0.2f;
|
||
sim->figsize.figurergba[0] = 0.1f;
|
||
sim->figtimer.figurergba[2] = 0.2f;
|
||
sim->figconstraint.figurergba[3] = 0.5f;
|
||
sim->figcost.figurergba[3] = 0.5f;
|
||
sim->figsize.figurergba[3] = 0.5f;
|
||
sim->figtimer.figurergba[3] = 0.5f;
|
||
|
||
// repeat line colors for constraint and cost figures
|
||
mjvFigure* fig = &sim->figcost;
|
||
for (int i=kCostNum; i<mjMAXLINE; i++) {
|
||
fig->linergb[i][0] = fig->linergb[i - kCostNum][0];
|
||
fig->linergb[i][1] = fig->linergb[i - kCostNum][1];
|
||
fig->linergb[i][2] = fig->linergb[i - kCostNum][2];
|
||
}
|
||
fig = &sim->figconstraint;
|
||
for (int i=kConstraintNum; i<mjMAXLINE; i++) {
|
||
fig->linergb[i][0] = fig->linergb[i - kConstraintNum][0];
|
||
fig->linergb[i][1] = fig->linergb[i - kConstraintNum][1];
|
||
fig->linergb[i][2] = fig->linergb[i - kConstraintNum][2];
|
||
}
|
||
|
||
// legends
|
||
mju::strcpy_arr(sim->figconstraint.linename[0], "total");
|
||
mju::strcpy_arr(sim->figconstraint.linename[1], "active");
|
||
mju::strcpy_arr(sim->figconstraint.linename[2], "changed");
|
||
mju::strcpy_arr(sim->figconstraint.linename[3], "evals");
|
||
mju::strcpy_arr(sim->figconstraint.linename[4], "updates");
|
||
mju::strcpy_arr(sim->figcost.linename[0], "improvement");
|
||
mju::strcpy_arr(sim->figcost.linename[1], "gradient");
|
||
mju::strcpy_arr(sim->figcost.linename[2], "lineslope");
|
||
mju::strcpy_arr(sim->figsize.linename[0], "dof");
|
||
mju::strcpy_arr(sim->figsize.linename[1], "body");
|
||
mju::strcpy_arr(sim->figsize.linename[2], "constraint");
|
||
mju::strcpy_arr(sim->figsize.linename[3], "sqrt(nnz)");
|
||
mju::strcpy_arr(sim->figsize.linename[4], "contact");
|
||
mju::strcpy_arr(sim->figsize.linename[5], "iteration");
|
||
mju::strcpy_arr(sim->figtimer.linename[0], "total");
|
||
mju::strcpy_arr(sim->figtimer.linename[1], "collision");
|
||
mju::strcpy_arr(sim->figtimer.linename[2], "prepare");
|
||
mju::strcpy_arr(sim->figtimer.linename[3], "solve");
|
||
mju::strcpy_arr(sim->figtimer.linename[4], "other");
|
||
|
||
// grid sizes
|
||
sim->figconstraint.gridsize[0] = 5;
|
||
sim->figconstraint.gridsize[1] = 5;
|
||
sim->figcost.gridsize[0] = 5;
|
||
sim->figcost.gridsize[1] = 5;
|
||
sim->figsize.gridsize[0] = 3;
|
||
sim->figsize.gridsize[1] = 5;
|
||
sim->figtimer.gridsize[0] = 3;
|
||
sim->figtimer.gridsize[1] = 5;
|
||
|
||
// minimum ranges
|
||
sim->figconstraint.range[0][0] = 0;
|
||
sim->figconstraint.range[0][1] = 20;
|
||
sim->figconstraint.range[1][0] = 0;
|
||
sim->figconstraint.range[1][1] = 80;
|
||
sim->figcost.range[0][0] = 0;
|
||
sim->figcost.range[0][1] = 20;
|
||
sim->figcost.range[1][0] = -15;
|
||
sim->figcost.range[1][1] = 5;
|
||
sim->figsize.range[0][0] = -200;
|
||
sim->figsize.range[0][1] = 0;
|
||
sim->figsize.range[1][0] = 0;
|
||
sim->figsize.range[1][1] = 100;
|
||
sim->figtimer.range[0][0] = -200;
|
||
sim->figtimer.range[0][1] = 0;
|
||
sim->figtimer.range[1][0] = 0;
|
||
sim->figtimer.range[1][1] = 0.4f;
|
||
|
||
// init x axis on history figures (do not show yet)
|
||
for (int n=0; n<6; n++) {
|
||
for (int i=0; i<mjMAXLINEPNT; i++) {
|
||
sim->figtimer.linedata[n][2*i] = -i;
|
||
sim->figsize.linedata[n][2*i] = -i;
|
||
}
|
||
}
|
||
}
|
||
|
||
// update profiler figures
|
||
void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
|
||
// reset lines in Constraint and Cost figures
|
||
memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
|
||
memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
|
||
|
||
// number of islands that have diagnostics
|
||
int nisland = d->nefc ? mjMAX(1, mjMIN(d->nisland, mjNISLAND)) : 0;
|
||
|
||
// iterate over islands
|
||
for (int k=0; k < nisland; k++) {
|
||
// ==== update Constraint ("Counts") figure
|
||
|
||
// number of points to plot, starting line
|
||
int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
|
||
int start = kConstraintNum * k;
|
||
|
||
sim->figconstraint.linepnt[start + 0] = npoints;
|
||
for (int i=1; i < kConstraintNum; i++) {
|
||
sim->figconstraint.linepnt[start + i] = npoints;
|
||
}
|
||
if (m->opt.solver == mjSOL_PGS) {
|
||
sim->figconstraint.linepnt[start + 3] = 0;
|
||
sim->figconstraint.linepnt[start + 4] = 0;
|
||
}
|
||
if (m->opt.solver == mjSOL_CG) {
|
||
sim->figconstraint.linepnt[start + 4] = 0;
|
||
}
|
||
for (int i=0; i<npoints; i++) {
|
||
// x
|
||
sim->figconstraint.linedata[start + 0][2*i] = i;
|
||
sim->figconstraint.linedata[start + 1][2*i] = i;
|
||
sim->figconstraint.linedata[start + 2][2*i] = i;
|
||
sim->figconstraint.linedata[start + 3][2*i] = i;
|
||
sim->figconstraint.linedata[start + 4][2*i] = i;
|
||
|
||
// y
|
||
int nefc = nisland == 1 ? d->nefc : d->island_nefc[k];
|
||
sim->figconstraint.linedata[start + 0][2*i+1] = nefc;
|
||
const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
|
||
sim->figconstraint.linedata[start + 1][2*i+1] = stat->nactive;
|
||
sim->figconstraint.linedata[start + 2][2*i+1] = stat->nchange;
|
||
sim->figconstraint.linedata[start + 3][2*i+1] = stat->neval;
|
||
sim->figconstraint.linedata[start + 4][2*i+1] = stat->nupdate;
|
||
}
|
||
|
||
// update cost figure
|
||
start = kCostNum * k;
|
||
sim->figcost.linepnt[start + 0] = npoints;
|
||
for (int i=1; i<kCostNum; i++) {
|
||
sim->figcost.linepnt[start + i] = npoints;
|
||
}
|
||
if (m->opt.solver==mjSOL_PGS) {
|
||
sim->figcost.linepnt[start + 1] = 0;
|
||
sim->figcost.linepnt[start + 2] = 0;
|
||
}
|
||
|
||
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
|
||
// x
|
||
sim->figcost.linedata[start + 0][2*i] = i;
|
||
sim->figcost.linedata[start + 1][2*i] = i;
|
||
sim->figcost.linedata[start + 2][2*i] = i;
|
||
|
||
// y
|
||
const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
|
||
sim->figcost.linedata[start + 0][2*i + 1] =
|
||
mju_log10(mju_max(mjMINVAL, stat->improvement));
|
||
sim->figcost.linedata[start + 1][2*i + 1] =
|
||
mju_log10(mju_max(mjMINVAL, stat->gradient));
|
||
sim->figcost.linedata[start + 2][2*i + 1] =
|
||
mju_log10(mju_max(mjMINVAL, stat->lineslope));
|
||
}
|
||
}
|
||
|
||
// get timers: total, collision, prepare, solve, other
|
||
mjtNum total = d->timer[mjTIMER_STEP].duration;
|
||
int number = d->timer[mjTIMER_STEP].number;
|
||
if (!number) {
|
||
total = d->timer[mjTIMER_FORWARD].duration;
|
||
number = d->timer[mjTIMER_FORWARD].number;
|
||
}
|
||
|
||
if (number) { // skip update if no measurements
|
||
float tdata[5] = {
|
||
static_cast<float>(total/number),
|
||
static_cast<float>(d->timer[mjTIMER_POS_COLLISION].duration/number),
|
||
static_cast<float>(d->timer[mjTIMER_POS_MAKE].duration/number) +
|
||
static_cast<float>(d->timer[mjTIMER_POS_PROJECT].duration/number),
|
||
static_cast<float>(d->timer[mjTIMER_CONSTRAINT].duration/number),
|
||
0
|
||
};
|
||
tdata[4] = tdata[0] - tdata[1] - tdata[2] - tdata[3];
|
||
|
||
// update figtimer
|
||
int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1);
|
||
for (int n=0; n<5; n++) {
|
||
// shift data
|
||
for (int i=pnt-1; i>0; i--) {
|
||
sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1];
|
||
}
|
||
|
||
// assign new
|
||
sim->figtimer.linepnt[n] = pnt;
|
||
sim->figtimer.linedata[n][1] = tdata[n];
|
||
}
|
||
}
|
||
|
||
// get total number of iterations and nonzeros
|
||
mjtNum sqrt_nnz = 0;
|
||
int solver_niter = 0;
|
||
for (int island=0; island < nisland; island++) {
|
||
sqrt_nnz += d->solver_nnz[island];
|
||
solver_niter += d->solver_niter[island];
|
||
}
|
||
sqrt_nnz = mju_sqrt(sqrt_nnz);
|
||
|
||
// get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
|
||
float sdata[6] = {
|
||
static_cast<float>(m->nv),
|
||
static_cast<float>(m->nbody),
|
||
static_cast<float>(d->nefc),
|
||
static_cast<float>(sqrt_nnz),
|
||
static_cast<float>(d->ncon),
|
||
static_cast<float>(solver_niter) / nisland
|
||
};
|
||
|
||
// update figsize
|
||
int pnt = mjMIN(201, sim->figsize.linepnt[0]+1);
|
||
for (int n=0; n<6; n++) {
|
||
// shift data
|
||
for (int i=pnt-1; i>0; i--) {
|
||
sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1];
|
||
}
|
||
|
||
// assign new
|
||
sim->figsize.linepnt[n] = pnt;
|
||
sim->figsize.linedata[n][1] = sdata[n];
|
||
}
|
||
}
|
||
|
||
// show profiler figures
|
||
void ShowProfiler(mj::Simulate* sim, mjrRect rect) {
|
||
mjrRect viewport = {
|
||
rect.left + rect.width - rect.width/4,
|
||
rect.bottom,
|
||
rect.width/4,
|
||
rect.height/4
|
||
};
|
||
mjr_figure(viewport, &sim->figtimer, &sim->platform_ui->mjr_context());
|
||
viewport.bottom += rect.height/4;
|
||
mjr_figure(viewport, &sim->figsize, &sim->platform_ui->mjr_context());
|
||
viewport.bottom += rect.height/4;
|
||
mjr_figure(viewport, &sim->figcost, &sim->platform_ui->mjr_context());
|
||
viewport.bottom += rect.height/4;
|
||
mjr_figure(viewport, &sim->figconstraint, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
|
||
// init sensor figure
|
||
void InitializeSensor(mj::Simulate* sim) {
|
||
mjvFigure& figsensor = sim->figsensor;
|
||
|
||
// set figure to default
|
||
mjv_defaultFigure(&figsensor);
|
||
figsensor.figurergba[3] = 0.5f;
|
||
|
||
// set flags
|
||
figsensor.flg_extend = 1;
|
||
figsensor.flg_barplot = 1;
|
||
figsensor.flg_symmetric = 1;
|
||
|
||
// title
|
||
mju::strcpy_arr(figsensor.title, "Sensor data");
|
||
|
||
// y-tick number format
|
||
mju::strcpy_arr(figsensor.yformat, "%.1f");
|
||
|
||
// grid size
|
||
figsensor.gridsize[0] = 2;
|
||
figsensor.gridsize[1] = 3;
|
||
|
||
// minimum range
|
||
figsensor.range[0][0] = 0;
|
||
figsensor.range[0][1] = 0;
|
||
figsensor.range[1][0] = -1;
|
||
figsensor.range[1][1] = 1;
|
||
}
|
||
|
||
// update sensor figure
|
||
void UpdateSensor(mj::Simulate* sim, const mjModel* m, const mjData* d) {
|
||
mjvFigure& figsensor = sim->figsensor;
|
||
static const int maxline = 10;
|
||
|
||
// clear linepnt
|
||
for (int i=0; i<maxline; i++) {
|
||
figsensor.linepnt[i] = 0;
|
||
}
|
||
|
||
// start with line 0
|
||
int lineid = 0;
|
||
|
||
// loop over sensors
|
||
for (int n=0; n<m->nsensor; n++) {
|
||
// go to next line if type is different
|
||
if (n>0 && m->sensor_type[n]!=m->sensor_type[n-1]) {
|
||
lineid = mjMIN(lineid+1, maxline-1);
|
||
}
|
||
|
||
// get info about this sensor
|
||
mjtNum cutoff = (m->sensor_cutoff[n]>0 ? m->sensor_cutoff[n] : 1);
|
||
int adr = m->sensor_adr[n];
|
||
int dim = m->sensor_dim[n];
|
||
|
||
// data pointer in line
|
||
int p = figsensor.linepnt[lineid];
|
||
|
||
// fill in data for this sensor
|
||
for (int i=0; i<dim; i++) {
|
||
// check size
|
||
if ((p+2*i)>=mjMAXLINEPNT/2) {
|
||
break;
|
||
}
|
||
|
||
// x
|
||
figsensor.linedata[lineid][2*p+4*i] = adr+i;
|
||
figsensor.linedata[lineid][2*p+4*i+2] = adr+i;
|
||
|
||
// y
|
||
figsensor.linedata[lineid][2*p+4*i+1] = 0;
|
||
figsensor.linedata[lineid][2*p+4*i+3] = d->sensordata[adr+i]/cutoff;
|
||
}
|
||
|
||
// update linepnt
|
||
figsensor.linepnt[lineid] = mjMIN(mjMAXLINEPNT-1, figsensor.linepnt[lineid]+2*dim);
|
||
}
|
||
}
|
||
|
||
// show sensor figure
|
||
void ShowSensor(mj::Simulate* sim, mjrRect rect) {
|
||
// constant width with and without profiler
|
||
int width = sim->profiler ? rect.width/3 : rect.width/4;
|
||
|
||
// render figure on the right
|
||
mjrRect viewport = {
|
||
rect.left + rect.width - width,
|
||
rect.bottom,
|
||
width,
|
||
rect.height/3
|
||
};
|
||
mjr_figure(viewport, &sim->figsensor, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
void ShowFigure(mj::Simulate* sim, mjrRect viewport, mjvFigure* fig){
|
||
mjr_figure(viewport, fig, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
void ShowOverlayText(mj::Simulate* sim, mjrRect viewport, int font, int gridpos,
|
||
std::string text1, std::string text2) {
|
||
mjr_overlay(font, gridpos, viewport, text1.c_str(), text2.c_str(),
|
||
&sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
void ShowImage(mj::Simulate* sim, mjrRect viewport, const unsigned char* image) {
|
||
mjr_drawPixels(image, nullptr, viewport, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
// load state from history buffer
|
||
static void LoadScrubState(mj::Simulate* sim) {
|
||
// get index into circular buffer
|
||
int i = (sim->scrub_index + sim->history_cursor_) % sim->nhistory_;
|
||
i = (i + sim->nhistory_) % sim->nhistory_;
|
||
|
||
// load state
|
||
mjtNum* state = &sim->history_[i * sim->state_size_];
|
||
mj_setState(sim->m_, sim->d_, state, mjSTATE_INTEGRATION);
|
||
|
||
// call forward dynamics
|
||
mj_forward(sim->m_, sim->d_);
|
||
}
|
||
|
||
// update an entire section of ui0
|
||
static void mjui0_update_section(mj::Simulate* sim, int section) {
|
||
mjui_update(section, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
// prepare info text
|
||
void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
|
||
char (&title)[mj::Simulate::kMaxFilenameLength],
|
||
char (&content)[mj::Simulate::kMaxFilenameLength]) {
|
||
char tmp[20];
|
||
|
||
// number of islands with statistics
|
||
int nisland = mjMAX(1, mjMIN(d->nisland, mjNISLAND));
|
||
|
||
// compute solver error (maximum over islands)
|
||
mjtNum solerr = 0;
|
||
for (int i=0; i < nisland; i++) {
|
||
mjtNum solerr_i = 0;
|
||
if (d->solver_niter[i]) {
|
||
int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
|
||
const mjSolverStat* stat = d->solver + i*mjNSOLVER + ind;
|
||
solerr_i = mju_min(stat->improvement, stat->gradient);
|
||
if (solerr_i==0) {
|
||
solerr_i = mju_max(stat->improvement, stat->gradient);
|
||
}
|
||
}
|
||
solerr = mju_max(solerr, solerr_i);
|
||
}
|
||
solerr = mju_log10(mju_max(mjMINVAL, solerr));
|
||
|
||
// format FPS text
|
||
char fps[10];
|
||
if (sim->fps_ < 1) {
|
||
mju::sprintf_arr(fps, "%0.1f ", sim->fps_);
|
||
} else {
|
||
mju::sprintf_arr(fps, "%.0f ", sim->fps_);
|
||
}
|
||
|
||
// total iterations of all islands with statistics
|
||
int solver_niter = 0;
|
||
for (int i=0; i < nisland; i++) {
|
||
solver_niter += d->solver_niter[i];
|
||
}
|
||
|
||
// prepare info text
|
||
mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory");
|
||
mju::sprintf_arr(content,
|
||
"%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%s\n%.1f%% of %s",
|
||
d->time,
|
||
d->nefc, d->ncon,
|
||
sim->run ?
|
||
d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) :
|
||
d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number),
|
||
solerr, solver_niter,
|
||
fps,
|
||
100*d->maxuse_arena/(double)(d->narena),
|
||
mju_writeNumBytes(d->narena));
|
||
|
||
// add Energy if enabled
|
||
{
|
||
if (mjENABLED(mjENBL_ENERGY)) {
|
||
mju::sprintf_arr(tmp, "\n%.3f", d->energy[0]+d->energy[1]);
|
||
mju::strcat_arr(content, tmp);
|
||
mju::strcat_arr(title, "\nEnergy");
|
||
}
|
||
|
||
// add FwdInv if enabled
|
||
if (mjENABLED(mjENBL_FWDINV)) {
|
||
mju::sprintf_arr(tmp, "\n%.1f %.1f",
|
||
mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[0])),
|
||
mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[1])));
|
||
mju::strcat_arr(content, tmp);
|
||
mju::strcat_arr(title, "\nFwdInv");
|
||
}
|
||
|
||
// add islands if enabled
|
||
if (!mjDISABLED(mjDSBL_ISLAND) && d->nisland > 0) {
|
||
mju::sprintf_arr(tmp, "\n%d", d->nisland);
|
||
mju::strcat_arr(content, tmp);
|
||
mju::strcat_arr(title, "\nIslands");
|
||
}
|
||
}
|
||
}
|
||
|
||
// sprintf forwarding, to avoid compiler warning in x-macro
|
||
void PrintField(char (&str)[mjMAXUINAME], void* ptr) {
|
||
mju::sprintf_arr(str, "%g", *static_cast<mjtNum*>(ptr));
|
||
}
|
||
|
||
// update watch
|
||
void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) {
|
||
// clear
|
||
sim->ui0.sect[SECT_WATCH].item[2].multi.nelem = 1;
|
||
mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid field");
|
||
|
||
// prepare symbols needed by xmacro
|
||
MJDATA_POINTERS_PREAMBLE(m);
|
||
|
||
// find specified field in mjData arrays, update value
|
||
#define X(TYPE, NAME, NR, NC) \
|
||
if (!mju::strcmp_arr(#NAME, sim->field) && \
|
||
!mju::strcmp_arr(#TYPE, "mjtNum")) { \
|
||
if (sim->index >= 0 && sim->index < m->NR * NC) { \
|
||
PrintField(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], d->NAME + sim->index); \
|
||
} else { \
|
||
mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid index"); \
|
||
} \
|
||
return; \
|
||
}
|
||
|
||
MJDATA_POINTERS
|
||
#undef X
|
||
}
|
||
|
||
|
||
//---------------------------------- UI construction -----------------------------------------------
|
||
|
||
// make physics section of UI
|
||
void MakePhysicsSection(mj::Simulate* sim) {
|
||
mjOption* opt = sim->is_passive_ ? &sim->m_passive_->opt : &sim->m_->opt;
|
||
mjuiDef defPhysics[] = {
|
||
{mjITEM_SECTION, "Physics", mjPRESERVE, nullptr, "AP"},
|
||
{mjITEM_SELECT, "Integrator", 2, &(opt->integrator), "Euler\nRK4\nimplicit\nimplicitfast"},
|
||
{mjITEM_SELECT, "Cone", 2, &(opt->cone), "Pyramidal\nElliptic"},
|
||
{mjITEM_SELECT, "Jacobian", 2, &(opt->jacobian), "Dense\nSparse\nAuto"},
|
||
{mjITEM_SELECT, "Solver", 2, &(opt->solver), "PGS\nCG\nNewton"},
|
||
{mjITEM_SEPARATOR, "Algorithmic Parameters", mjPRESERVE},
|
||
{mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"},
|
||
{mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"},
|
||
{mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"},
|
||
{mjITEM_EDITINT, "LS Iter", 2, &(opt->ls_iterations), "1 0 100"},
|
||
{mjITEM_EDITNUM, "LS Tol", 2, &(opt->ls_tolerance), "1 0 0.1"},
|
||
{mjITEM_EDITINT, "Noslip Iter", 2, &(opt->noslip_iterations), "1 0 1000"},
|
||
{mjITEM_EDITNUM, "Noslip Tol", 2, &(opt->noslip_tolerance), "1 0 1"},
|
||
{mjITEM_EDITINT, "CCD Iter", 2, &(opt->ccd_iterations), "1 0 1000"},
|
||
{mjITEM_EDITNUM, "CCD Tol", 2, &(opt->ccd_tolerance), "1 0 1"},
|
||
{mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"},
|
||
{mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"},
|
||
{mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE},
|
||
{mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"},
|
||
{mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"},
|
||
{mjITEM_EDITNUM, "Magnetic", 2, opt->magnetic, "3"},
|
||
{mjITEM_EDITNUM, "Density", 2, &(opt->density), "1"},
|
||
{mjITEM_EDITNUM, "Viscosity", 2, &(opt->viscosity), "1"},
|
||
{mjITEM_EDITNUM, "Imp Ratio", 2, &(opt->impratio), "1"},
|
||
{mjITEM_SEPARATOR, "Disable Flags", mjPRESERVE},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defEnableFlags[] = {
|
||
{mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defOverride[] = {
|
||
{mjITEM_SEPARATOR, "Contact Override", mjPRESERVE},
|
||
{mjITEM_EDITNUM, "Margin", 2, &(opt->o_margin), "1"},
|
||
{mjITEM_EDITNUM, "Sol Imp", 2, &(opt->o_solimp), "5"},
|
||
{mjITEM_EDITNUM, "Sol Ref", 2, &(opt->o_solref), "2"},
|
||
{mjITEM_EDITNUM, "Friction", 2, &(opt->o_friction), "5"},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defDisableActuator[] = {
|
||
{mjITEM_SEPARATOR, "Actuator Group Enable", mjPRESERVE},
|
||
{mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator+0, ""},
|
||
{mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator+1, ""},
|
||
{mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator+2, ""},
|
||
{mjITEM_CHECKBYTE, "Act Group 3", 2, sim->enableactuator+3, ""},
|
||
{mjITEM_CHECKBYTE, "Act Group 4", 2, sim->enableactuator+4, ""},
|
||
{mjITEM_CHECKBYTE, "Act Group 5", 2, sim->enableactuator+5, ""},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add physics
|
||
mjui_add(&sim->ui0, defPhysics);
|
||
|
||
// add flags programmatically
|
||
mjuiDef defFlag[] = {
|
||
{mjITEM_CHECKINT, "", 2, nullptr, ""},
|
||
{mjITEM_END}
|
||
};
|
||
for (int i=0; i<mjNDISABLE; i++) {
|
||
mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
|
||
defFlag[0].pdata = sim->disable + i;
|
||
mjui_add(&sim->ui0, defFlag);
|
||
}
|
||
mjui_add(&sim->ui0, defEnableFlags);
|
||
for (int i=0; i<mjNENABLE; i++) {
|
||
mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
|
||
defFlag[0].pdata = sim->enable + i;
|
||
mjui_add(&sim->ui0, defFlag);
|
||
}
|
||
// add contact override
|
||
mjui_add(&sim->ui0, defOverride);
|
||
|
||
// add actuator group enable/disable
|
||
mjui_add(&sim->ui0, defDisableActuator);
|
||
|
||
// make some subsections closed by default
|
||
for (int i=0; i < sim->ui0.sect[SECT_PHYSICS].nitem; i++) {
|
||
mjuiItem* it = sim->ui0.sect[SECT_PHYSICS].item + i;
|
||
|
||
// close less useful subsections
|
||
if (it->type == mjITEM_SEPARATOR) {
|
||
if (mju::strcmp_arr(it->name, "Actuator Group Enable") &&
|
||
mju::strcmp_arr(it->name, "Contact Override") &&
|
||
mju::strcmp_arr(it->name, "Physical Parameters")) {
|
||
it->state = mjSEPCLOSED+1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// make rendering section of UI
|
||
void MakeRenderingSection(mj::Simulate* sim, const mjModel* m) {
|
||
mjuiDef defRendering[] = {
|
||
{mjITEM_SECTION, "Rendering", mjPRESERVE, nullptr, "AR"},
|
||
{mjITEM_SELECT, "Camera", 2, &(sim->camera), "Free\nTracking"},
|
||
{mjITEM_SELECT, "Label", 2, &(sim->opt.label),
|
||
"None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
|
||
"Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"
|
||
},
|
||
{mjITEM_SELECT, "Frame", 2, &(sim->opt.frame),
|
||
"None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld"
|
||
},
|
||
{mjITEM_BUTTON, "Copy camera", 2, nullptr, ""},
|
||
{mjITEM_SEPARATOR, "Model Elements", 1},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defOpenGL[] = {
|
||
{mjITEM_SEPARATOR, "OpenGL Effects", 1},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add model cameras, up to UI limit
|
||
for (int i=0; i<mjMIN(m->ncam, mjMAXUIMULTI-2); i++) {
|
||
// prepare name
|
||
char camname[mjMAXUINAME] = "\n";
|
||
if (m->names[m->name_camadr[i]]) {
|
||
mju::strcat_arr(camname, m->names+m->name_camadr[i]);
|
||
} else {
|
||
mju::sprintf_arr(camname, "\nCamera %d", i);
|
||
}
|
||
|
||
// check string length
|
||
if (mju::strlen_arr(camname) + mju::strlen_arr(defRendering[1].other)>=mjMAXUITEXT-1) {
|
||
break;
|
||
}
|
||
|
||
// add camera
|
||
mju::strcat_arr(defRendering[1].other, camname);
|
||
}
|
||
|
||
// add rendering standard
|
||
mjui_add(&sim->ui0, defRendering);
|
||
|
||
// add flags programmatically
|
||
mjuiDef defFlag[] = {
|
||
{mjITEM_CHECKBYTE, "", 2, nullptr, ""},
|
||
{mjITEM_END}
|
||
};
|
||
for (int i=0; i<mjNVISFLAG; i++) {
|
||
// set name
|
||
mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
|
||
|
||
// set shortcut and data
|
||
if (mjVISSTRING[i][2][0]) {
|
||
mju::sprintf_arr(defFlag[0].other, " %s", mjVISSTRING[i][2]);
|
||
} else {
|
||
mju::sprintf_arr(defFlag[0].other, "");
|
||
}
|
||
defFlag[0].pdata = sim->opt.flags + i;
|
||
mjui_add(&sim->ui0, defFlag);
|
||
}
|
||
|
||
// create tree slider
|
||
mjuiDef defTree[] = {
|
||
{mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 20"},
|
||
{mjITEM_SLIDERINT, "Flex layer", 2, &sim->opt.flex_layer, "0 10"},
|
||
{mjITEM_END}
|
||
};
|
||
mjui_add(&sim->ui0, defTree);
|
||
|
||
// add rendering flags
|
||
mjui_add(&sim->ui0, defOpenGL);
|
||
for (int i=0; i<mjNRNDFLAG; i++) {
|
||
// set name
|
||
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
|
||
|
||
// set shortcut and data
|
||
if (mjRNDSTRING[i][2][0]) {
|
||
mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
|
||
} else {
|
||
mju::sprintf_arr(defFlag[0].other, "");
|
||
}
|
||
defFlag[0].pdata = sim->scn.flags + i;
|
||
mjui_add(&sim->ui0, defFlag);
|
||
}
|
||
}
|
||
|
||
// make visualization section of UI
|
||
void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) {
|
||
mjStatistic* stat = sim->is_passive_ ? &sim->m_passive_->stat : &sim->m_->stat;
|
||
mjVisual* vis = sim->is_passive_ ? &sim->m_passive_->vis : &sim->m_->vis;
|
||
|
||
mjuiDef defVisualization[] = {
|
||
{mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"},
|
||
{mjITEM_SEPARATOR, "Headlight", 1},
|
||
{mjITEM_RADIO, "Active", 2, &(vis->headlight.active), "Off\nOn"},
|
||
{mjITEM_EDITFLOAT, "Ambient", 2, &(vis->headlight.ambient), "3"},
|
||
{mjITEM_EDITFLOAT, "Diffuse", 2, &(vis->headlight.diffuse), "3"},
|
||
{mjITEM_EDITFLOAT, "Specular", 2, &(vis->headlight.specular), "3"},
|
||
{mjITEM_SEPARATOR, "Free Camera", 1},
|
||
{mjITEM_RADIO, "Orthographic", 2, &(vis->global.orthographic), "No\nYes"},
|
||
{mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"},
|
||
{mjITEM_EDITNUM, "Center", 2, &(stat->center), "3"},
|
||
{mjITEM_EDITFLOAT, "Azimuth", 2, &(vis->global.azimuth), "1"},
|
||
{mjITEM_EDITFLOAT, "Elevation", 2, &(vis->global.elevation), "1"},
|
||
{mjITEM_BUTTON, "Align", 2, nullptr, "CA"},
|
||
{mjITEM_SEPARATOR, "Global", 1},
|
||
{mjITEM_EDITNUM, "Extent", 2, &(stat->extent), "1"},
|
||
{mjITEM_RADIO, "Inertia", 2, &(vis->global.ellipsoidinertia), "Box\nEllipsoid"},
|
||
{mjITEM_RADIO, "BVH active", 5, &(vis->global.bvactive), "False\nTrue"},
|
||
{mjITEM_SEPARATOR, "Map", 1},
|
||
{mjITEM_EDITFLOAT, "Stiffness", 2, &(vis->map.stiffness), "1"},
|
||
{mjITEM_EDITFLOAT, "Rot stiffness", 2, &(vis->map.stiffnessrot), "1"},
|
||
{mjITEM_EDITFLOAT, "Force", 2, &(vis->map.force), "1"},
|
||
{mjITEM_EDITFLOAT, "Torque", 2, &(vis->map.torque), "1"},
|
||
{mjITEM_EDITFLOAT, "Alpha", 2, &(vis->map.alpha), "1"},
|
||
{mjITEM_EDITFLOAT, "Fog start", 2, &(vis->map.fogstart), "1"},
|
||
{mjITEM_EDITFLOAT, "Fog end", 2, &(vis->map.fogend), "1"},
|
||
{mjITEM_EDITFLOAT, "Z near", 2, &(vis->map.znear), "1"},
|
||
{mjITEM_EDITFLOAT, "Z far", 2, &(vis->map.zfar), "1"},
|
||
{mjITEM_EDITFLOAT, "Haze", 2, &(vis->map.haze), "1"},
|
||
{mjITEM_EDITFLOAT, "Shadow clip", 2, &(vis->map.shadowclip), "1"},
|
||
{mjITEM_EDITFLOAT, "Shadow scale", 2, &(vis->map.shadowscale), "1"},
|
||
{mjITEM_SEPARATOR, "Scale", mjPRESERVE},
|
||
{mjITEM_EDITNUM, "All (meansize)", 2, &(stat->meansize), "1"},
|
||
{mjITEM_EDITFLOAT, "Force width", 2, &(vis->scale.forcewidth), "1"},
|
||
{mjITEM_EDITFLOAT, "Contact width", 2, &(vis->scale.contactwidth), "1"},
|
||
{mjITEM_EDITFLOAT, "Contact height", 2, &(vis->scale.contactheight), "1"},
|
||
{mjITEM_EDITFLOAT, "Connect", 2, &(vis->scale.connect), "1"},
|
||
{mjITEM_EDITFLOAT, "Com", 2, &(vis->scale.com), "1"},
|
||
{mjITEM_EDITFLOAT, "Camera", 2, &(vis->scale.camera), "1"},
|
||
{mjITEM_EDITFLOAT, "Light", 2, &(vis->scale.light), "1"},
|
||
{mjITEM_EDITFLOAT, "Select point", 2, &(vis->scale.selectpoint), "1"},
|
||
{mjITEM_EDITFLOAT, "Joint length", 2, &(vis->scale.jointlength), "1"},
|
||
{mjITEM_EDITFLOAT, "Joint width", 2, &(vis->scale.jointwidth), "1"},
|
||
{mjITEM_EDITFLOAT, "Actuator length", 2, &(vis->scale.actuatorlength), "1"},
|
||
{mjITEM_EDITFLOAT, "Actuator width", 2, &(vis->scale.actuatorwidth), "1"},
|
||
{mjITEM_EDITFLOAT, "Frame length", 2, &(vis->scale.framelength), "1"},
|
||
{mjITEM_EDITFLOAT, "Frame width", 2, &(vis->scale.framewidth), "1"},
|
||
{mjITEM_EDITFLOAT, "Constraint", 2, &(vis->scale.constraint), "1"},
|
||
{mjITEM_EDITFLOAT, "Slider-crank", 2, &(vis->scale.slidercrank), "1"},
|
||
{mjITEM_SEPARATOR, "RGBA", mjPRESERVE},
|
||
{mjITEM_EDITFLOAT, "fog", 2, &(vis->rgba.fog), "4"},
|
||
{mjITEM_EDITFLOAT, "haze", 2, &(vis->rgba.haze), "4"},
|
||
{mjITEM_EDITFLOAT, "force", 2, &(vis->rgba.force), "4"},
|
||
{mjITEM_EDITFLOAT, "inertia", 2, &(vis->rgba.inertia), "4"},
|
||
{mjITEM_EDITFLOAT, "joint", 2, &(vis->rgba.joint), "4"},
|
||
{mjITEM_EDITFLOAT, "actuator", 2, &(vis->rgba.actuator), "4"},
|
||
{mjITEM_EDITFLOAT, "actnegative", 2, &(vis->rgba.actuatornegative), "4"},
|
||
{mjITEM_EDITFLOAT, "actpositive", 2, &(vis->rgba.actuatorpositive), "4"},
|
||
{mjITEM_EDITFLOAT, "com", 2, &(vis->rgba.com), "4"},
|
||
{mjITEM_EDITFLOAT, "camera", 2, &(vis->rgba.camera), "4"},
|
||
{mjITEM_EDITFLOAT, "light", 2, &(vis->rgba.light), "4"},
|
||
{mjITEM_EDITFLOAT, "selectpoint", 2, &(vis->rgba.selectpoint), "4"},
|
||
{mjITEM_EDITFLOAT, "connect", 2, &(vis->rgba.connect), "4"},
|
||
{mjITEM_EDITFLOAT, "contactpoint", 2, &(vis->rgba.contactpoint), "4"},
|
||
{mjITEM_EDITFLOAT, "contactforce", 2, &(vis->rgba.contactforce), "4"},
|
||
{mjITEM_EDITFLOAT, "contactfriction", 2, &(vis->rgba.contactfriction), "4"},
|
||
{mjITEM_EDITFLOAT, "contacttorque", 2, &(vis->rgba.contacttorque), "4"},
|
||
{mjITEM_EDITFLOAT, "contactgap", 2, &(vis->rgba.contactgap), "4"},
|
||
{mjITEM_EDITFLOAT, "rangefinder", 2, &(vis->rgba.rangefinder), "4"},
|
||
{mjITEM_EDITFLOAT, "constraint", 2, &(vis->rgba.constraint), "4"},
|
||
{mjITEM_EDITFLOAT, "slidercrank", 2, &(vis->rgba.slidercrank), "4"},
|
||
{mjITEM_EDITFLOAT, "crankbroken", 2, &(vis->rgba.crankbroken), "4"},
|
||
{mjITEM_EDITFLOAT, "frustum", 2, &(vis->rgba.frustum), "4"},
|
||
{mjITEM_EDITFLOAT, "bv", 2, &(vis->rgba.bv), "4"},
|
||
{mjITEM_EDITFLOAT, "bvactive", 2, &(vis->rgba.bvactive), "4"},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add visualization section
|
||
mjui_add(&sim->ui0, defVisualization);
|
||
}
|
||
|
||
// make group section of UI
|
||
void MakeGroupSection(mj::Simulate* sim) {
|
||
mjuiDef defGroup[] = {
|
||
{mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"},
|
||
{mjITEM_SEPARATOR, "Geom groups", 1},
|
||
{mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"},
|
||
{mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup+1, " 1"},
|
||
{mjITEM_CHECKBYTE, "Geom 2", 2, sim->opt.geomgroup+2, " 2"},
|
||
{mjITEM_CHECKBYTE, "Geom 3", 2, sim->opt.geomgroup+3, " 3"},
|
||
{mjITEM_CHECKBYTE, "Geom 4", 2, sim->opt.geomgroup+4, " 4"},
|
||
{mjITEM_CHECKBYTE, "Geom 5", 2, sim->opt.geomgroup+5, " 5"},
|
||
{mjITEM_SEPARATOR, "Site groups", 1},
|
||
{mjITEM_CHECKBYTE, "Site 0", 2, sim->opt.sitegroup, "S0"},
|
||
{mjITEM_CHECKBYTE, "Site 1", 2, sim->opt.sitegroup+1, "S1"},
|
||
{mjITEM_CHECKBYTE, "Site 2", 2, sim->opt.sitegroup+2, "S2"},
|
||
{mjITEM_CHECKBYTE, "Site 3", 2, sim->opt.sitegroup+3, "S3"},
|
||
{mjITEM_CHECKBYTE, "Site 4", 2, sim->opt.sitegroup+4, "S4"},
|
||
{mjITEM_CHECKBYTE, "Site 5", 2, sim->opt.sitegroup+5, "S5"},
|
||
{mjITEM_SEPARATOR, "Joint groups", 1},
|
||
{mjITEM_CHECKBYTE, "Joint 0", 2, sim->opt.jointgroup, ""},
|
||
{mjITEM_CHECKBYTE, "Joint 1", 2, sim->opt.jointgroup+1, ""},
|
||
{mjITEM_CHECKBYTE, "Joint 2", 2, sim->opt.jointgroup+2, ""},
|
||
{mjITEM_CHECKBYTE, "Joint 3", 2, sim->opt.jointgroup+3, ""},
|
||
{mjITEM_CHECKBYTE, "Joint 4", 2, sim->opt.jointgroup+4, ""},
|
||
{mjITEM_CHECKBYTE, "Joint 5", 2, sim->opt.jointgroup+5, ""},
|
||
{mjITEM_SEPARATOR, "Tendon groups", 1},
|
||
{mjITEM_CHECKBYTE, "Tendon 0", 2, sim->opt.tendongroup, ""},
|
||
{mjITEM_CHECKBYTE, "Tendon 1", 2, sim->opt.tendongroup+1, ""},
|
||
{mjITEM_CHECKBYTE, "Tendon 2", 2, sim->opt.tendongroup+2, ""},
|
||
{mjITEM_CHECKBYTE, "Tendon 3", 2, sim->opt.tendongroup+3, ""},
|
||
{mjITEM_CHECKBYTE, "Tendon 4", 2, sim->opt.tendongroup+4, ""},
|
||
{mjITEM_CHECKBYTE, "Tendon 5", 2, sim->opt.tendongroup+5, ""},
|
||
{mjITEM_SEPARATOR, "Actuator groups", 1},
|
||
{mjITEM_CHECKBYTE, "Actuator 0", 2, sim->opt.actuatorgroup, ""},
|
||
{mjITEM_CHECKBYTE, "Actuator 1", 2, sim->opt.actuatorgroup+1, ""},
|
||
{mjITEM_CHECKBYTE, "Actuator 2", 2, sim->opt.actuatorgroup+2, ""},
|
||
{mjITEM_CHECKBYTE, "Actuator 3", 2, sim->opt.actuatorgroup+3, ""},
|
||
{mjITEM_CHECKBYTE, "Actuator 4", 2, sim->opt.actuatorgroup+4, ""},
|
||
{mjITEM_CHECKBYTE, "Actuator 5", 2, sim->opt.actuatorgroup+5, ""},
|
||
{mjITEM_SEPARATOR, "Flex groups", 1},
|
||
{mjITEM_CHECKBYTE, "Flex 0", 2, sim->opt.flexgroup, ""},
|
||
{mjITEM_CHECKBYTE, "Flex 1", 2, sim->opt.flexgroup+1, ""},
|
||
{mjITEM_CHECKBYTE, "Flex 2", 2, sim->opt.flexgroup+2, ""},
|
||
{mjITEM_CHECKBYTE, "Flex 3", 2, sim->opt.flexgroup+3, ""},
|
||
{mjITEM_CHECKBYTE, "Flex 4", 2, sim->opt.flexgroup+4, ""},
|
||
{mjITEM_CHECKBYTE, "Flex 5", 2, sim->opt.flexgroup+5, ""},
|
||
{mjITEM_SEPARATOR, "Skin groups", 1},
|
||
{mjITEM_CHECKBYTE, "Skin 0", 2, sim->opt.skingroup, ""},
|
||
{mjITEM_CHECKBYTE, "Skin 1", 2, sim->opt.skingroup+1, ""},
|
||
{mjITEM_CHECKBYTE, "Skin 2", 2, sim->opt.skingroup+2, ""},
|
||
{mjITEM_CHECKBYTE, "Skin 3", 2, sim->opt.skingroup+3, ""},
|
||
{mjITEM_CHECKBYTE, "Skin 4", 2, sim->opt.skingroup+4, ""},
|
||
{mjITEM_CHECKBYTE, "Skin 5", 2, sim->opt.skingroup+5, ""},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add section
|
||
mjui_add(&sim->ui0, defGroup);
|
||
}
|
||
|
||
// make joint section of UI
|
||
void MakeJointSection(mj::Simulate* sim) {
|
||
mjuiDef defJoint[] = {
|
||
{mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defSlider[] = {
|
||
{mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add section
|
||
mjui_add(&sim->ui1, defJoint);
|
||
defSlider[0].state = 4;
|
||
|
||
// add scalar joints, exit if UI limit reached
|
||
int itemcnt = 0;
|
||
for (int i=0; i < sim->jnt_type_.size() && itemcnt<mjMAXUIITEM; i++) {
|
||
if ((sim->jnt_type_[i]==mjJNT_HINGE || sim->jnt_type_[i]==mjJNT_SLIDE)) {
|
||
// skip if joint group is disabled
|
||
if (!sim->opt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->jnt_group_[i]))]) {
|
||
continue;
|
||
}
|
||
|
||
// set data and name
|
||
if (!sim->is_passive_) {
|
||
defSlider[0].pdata = &sim->d_->qpos[sim->m_->jnt_qposadr[i]];
|
||
} else {
|
||
defSlider[0].pdata = &sim->qpos_[sim->jnt_qposadr_[i]];
|
||
}
|
||
if (!sim->jnt_names_[i].empty()) {
|
||
mju::strcpy_arr(defSlider[0].name, sim->jnt_names_[i].c_str());
|
||
} else {
|
||
mju::sprintf_arr(defSlider[0].name, "joint %d", i);
|
||
}
|
||
|
||
// set range
|
||
if (sim->jnt_range_[i].has_value())
|
||
mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
|
||
sim->jnt_range_[i]->first, sim->jnt_range_[i]->second);
|
||
else if (sim->jnt_type_[i]==mjJNT_SLIDE) {
|
||
mju::strcpy_arr(defSlider[0].other, "-1 1");
|
||
} else {
|
||
mju::strcpy_arr(defSlider[0].other, "-3.1416 3.1416");
|
||
}
|
||
|
||
// add and count
|
||
mjui_add(&sim->ui1, defSlider);
|
||
itemcnt++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// make control section of UI
|
||
void MakeControlSection(mj::Simulate* sim) {
|
||
mjuiDef defControl[] = {
|
||
{mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"},
|
||
{mjITEM_BUTTON, "Clear all", 2},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defSlider[] = {
|
||
{mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add section
|
||
mjui_add(&sim->ui1, defControl);
|
||
|
||
// add controls, exit if UI limit reached (Clear button already added)
|
||
int itemcnt = 1;
|
||
for (int i=0; i < sim->actuator_ctrlrange_.size() && itemcnt<mjMAXUIITEM; i++) {
|
||
// skip if actuator vis group is disabled
|
||
int group = sim->actuator_group_[i];
|
||
if (!sim->opt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, group))]) {
|
||
continue;
|
||
}
|
||
// grey out if actuator group is disabled
|
||
if (group >= 0 && group <= 30 && sim->m_->opt.disableactuator & (1 << group)) {
|
||
defSlider[0].state = 0;
|
||
} else {
|
||
defSlider[0].state = 2;
|
||
}
|
||
|
||
// set data and name
|
||
if (!sim->is_passive_) {
|
||
defSlider[0].pdata = &sim->d_->ctrl[i];
|
||
} else {
|
||
defSlider[0].pdata = &sim->ctrl_[i];
|
||
}
|
||
if (!sim->actuator_names_[i].empty()) {
|
||
mju::strcpy_arr(defSlider[0].name, sim->actuator_names_[i].c_str());
|
||
} else {
|
||
mju::sprintf_arr(defSlider[0].name, "control %d", i);
|
||
}
|
||
|
||
// set range
|
||
if (sim->actuator_ctrlrange_[i].has_value())
|
||
mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
|
||
sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second);
|
||
else {
|
||
mju::strcpy_arr(defSlider[0].other, "-1 1");
|
||
}
|
||
|
||
// add and count
|
||
mjui_add(&sim->ui1, defSlider);
|
||
itemcnt++;
|
||
}
|
||
}
|
||
|
||
// make equality section of UI
|
||
void MakeEqualitySection(mj::Simulate* sim) {
|
||
mjuiDef defEquality[] = {
|
||
{mjITEM_SECTION, "Equality", mjPRESERVE, nullptr, "AE"},
|
||
{mjITEM_END}
|
||
};
|
||
mjuiDef defCheckBox[] = {
|
||
{mjITEM_CHECKBYTE, "", 2, nullptr, ""},
|
||
{mjITEM_END}
|
||
};
|
||
|
||
// add section
|
||
mjui_add(&sim->ui1, defEquality);
|
||
|
||
// add equalities, exit if UI limit reached
|
||
for (int i= 0; i < sim->m_->neq && i<mjMAXUIITEM; i++) {
|
||
// set data
|
||
defCheckBox[0].pdata = &sim->d_->eq_active[i];
|
||
|
||
// set name
|
||
if (!sim->equality_names_[i].empty()) {
|
||
mju::strcpy_arr(defCheckBox[0].name, sim->equality_names_[i].c_str());
|
||
} else {
|
||
mju::sprintf_arr(defCheckBox[0].name, "equality %d", i);
|
||
}
|
||
|
||
mjui_add(&sim->ui1, defCheckBox);
|
||
}
|
||
}
|
||
|
||
// make model-dependent UI sections
|
||
void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) {
|
||
// clear model-dependent sections of UI
|
||
sim->ui0.nsect = SECT_PHYSICS;
|
||
sim->ui1.nsect = 0;
|
||
|
||
// make
|
||
MakePhysicsSection(sim);
|
||
MakeRenderingSection(sim, m);
|
||
MakeVisualizationSection(sim, m);
|
||
MakeGroupSection(sim);
|
||
MakeJointSection(sim);
|
||
MakeControlSection(sim);
|
||
MakeEqualitySection(sim);
|
||
}
|
||
|
||
//---------------------------------- utility functions ---------------------------------------------
|
||
|
||
// align and scale view
|
||
void AlignAndScaleView(mj::Simulate* sim, const mjModel* m) {
|
||
// if the id is valid, use the initial fixed camera
|
||
if (m->vis.global.cameraid >= 0 && m->vis.global.cameraid < m->ncam) {
|
||
sim->cam.fixedcamid = m->vis.global.cameraid;
|
||
sim->cam.type = mjCAMERA_FIXED;
|
||
}
|
||
|
||
// otherwise use default free camera
|
||
else {
|
||
mjv_defaultFreeCamera(m, &sim->cam);
|
||
}
|
||
}
|
||
|
||
|
||
// copy state to clipboard as key
|
||
void CopyKey(mj::Simulate* sim, const mjModel* m, const mjData* d, bool fp) {
|
||
char clipboard[5000] = "<key\n";
|
||
char buf[200];
|
||
const char p_regular[] = "%g";
|
||
const char p_full[] = "%-22.16g";
|
||
const char* format = fp ? p_full : p_regular;
|
||
|
||
// time
|
||
mju::strcat_arr(clipboard, " time=\"");
|
||
mju::sprintf_arr(buf, format, d->time);
|
||
mju::strcat_arr(clipboard, buf);
|
||
|
||
// qpos
|
||
mju::strcat_arr(clipboard, "\"\n qpos=\"");
|
||
for (int i = 0; i < m->nq; i++) {
|
||
mju::sprintf_arr(buf, format, d->qpos[i]);
|
||
if (i < m->nq-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
|
||
// qvel
|
||
mju::strcat_arr(clipboard, "\"\n qvel=\"");
|
||
for (int i = 0; i < m->nv; i++) {
|
||
mju::sprintf_arr(buf, format, d->qvel[i]);
|
||
if (i < m->nv-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
|
||
// act
|
||
if (m->na > 0) {
|
||
mju::strcat_arr(clipboard, "\"\n act=\"");
|
||
for (int i = 0; i < m->na; i++) {
|
||
mju::sprintf_arr(buf, format, d->act[i]);
|
||
if (i < m->na-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
}
|
||
|
||
// ctrl
|
||
if (m->nu > 0) {
|
||
mju::strcat_arr(clipboard, "\"\n ctrl=\"");
|
||
for (int i = 0; i < m->nu; i++) {
|
||
mju::sprintf_arr(buf, format, d->ctrl[i]);
|
||
if (i < m->nu-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
}
|
||
|
||
if (m->nmocap > 0) {
|
||
// mocap_pos
|
||
mju::strcat_arr(clipboard, "\"\n mpos=\"");
|
||
for (int i = 0; i < 3*m->nmocap; i++) {
|
||
mju::sprintf_arr(buf, format, d->mocap_pos[i]);
|
||
if (i < 3*m->nmocap-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
|
||
// mocap_quat
|
||
mju::strcat_arr(clipboard, "\"\n mquat=\"");
|
||
for (int i = 0; i < 4*m->nmocap; i++) {
|
||
mju::sprintf_arr(buf, format, d->mocap_quat[i]);
|
||
if (i < 4*m->nmocap-1) mju::strcat_arr(buf, " ");
|
||
mju::strcat_arr(clipboard, buf);
|
||
}
|
||
}
|
||
|
||
mju::strcat_arr(clipboard, "\"\n/>");
|
||
|
||
// copy to clipboard
|
||
sim->platform_ui->SetClipboardString(clipboard);
|
||
}
|
||
|
||
// millisecond timer, for MuJoCo built-in profiler
|
||
mjtNum Timer() {
|
||
static auto start = mj::Simulate::Clock::now();
|
||
auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start);
|
||
return elapsed.count();
|
||
}
|
||
|
||
// clear all times
|
||
void ClearTimers(mjData* d) {
|
||
for (int i=0; i<mjNTIMER; i++) {
|
||
d->timer[i].duration = 0;
|
||
d->timer[i].number = 0;
|
||
}
|
||
}
|
||
|
||
// copy current camera to clipboard as MJCF specification
|
||
void CopyCamera(mj::Simulate* sim) {
|
||
mjvGLCamera* camera = sim->scn.camera;
|
||
|
||
char clipboard[500];
|
||
mjtNum cam_right[3];
|
||
mjtNum cam_forward[3];
|
||
mjtNum cam_up[3];
|
||
|
||
// get camera spec from the GLCamera
|
||
mju_f2n(cam_forward, camera[0].forward, 3);
|
||
mju_f2n(cam_up, camera[0].up, 3);
|
||
mju_cross(cam_right, cam_forward, cam_up);
|
||
|
||
// make MJCF camera spec
|
||
mju::sprintf_arr(clipboard,
|
||
"<camera pos=\"%.3f %.3f %.3f\" xyaxes=\"%.3f %.3f %.3f %.3f %.3f %.3f\"/>\n",
|
||
(camera[0].pos[0] + camera[1].pos[0]) / 2,
|
||
(camera[0].pos[1] + camera[1].pos[1]) / 2,
|
||
(camera[0].pos[2] + camera[1].pos[2]) / 2,
|
||
cam_right[0], cam_right[1], cam_right[2],
|
||
camera[0].up[0], camera[0].up[1], camera[0].up[2]);
|
||
|
||
// copy spec into clipboard
|
||
sim->platform_ui->SetClipboardString(clipboard);
|
||
}
|
||
|
||
// update UI 0 when MuJoCo structures change (except for joint sliders)
|
||
void UpdateSettings(mj::Simulate* sim, const mjModel* m) {
|
||
// physics flags
|
||
for (int i=0; i<mjNDISABLE; i++) {
|
||
int new_value = ((m->opt.disableflags & (1<<i)) != 0);
|
||
if (sim->disable[i] != new_value) {
|
||
sim->disable[i] = new_value;
|
||
sim->pending_.ui_update_physics = true;
|
||
}
|
||
}
|
||
for (int i=0; i<mjNENABLE; i++) {
|
||
int new_value = ((m->opt.enableflags & (1<<i)) != 0);
|
||
if (sim->enable[i] != new_value) {
|
||
sim->enable[i] = new_value;
|
||
sim->pending_.ui_update_physics = true;
|
||
}
|
||
}
|
||
for (int i=0; i<mjNGROUP; i++) {
|
||
int enabled = ((m->opt.disableactuator & (1<<i)) == 0);
|
||
if (sim->enableactuator[i] != enabled) {
|
||
sim->enableactuator[i] = enabled;
|
||
sim->pending_.ui_update_physics = true;
|
||
sim->pending_.ui_remake_ctrl = true;
|
||
}
|
||
}
|
||
|
||
// camera
|
||
int old_camera = sim->camera;
|
||
if (sim->cam.type==mjCAMERA_FIXED) {
|
||
sim->camera = 2 + sim->cam.fixedcamid;
|
||
} else if (sim->cam.type==mjCAMERA_TRACKING) {
|
||
sim->camera = 1;
|
||
} else {
|
||
sim->camera = 0;
|
||
}
|
||
if (old_camera != sim->camera) {
|
||
sim->pending_.ui_update_rendering = true;
|
||
}
|
||
}
|
||
|
||
// Compute suitable font scale.
|
||
int ComputeFontScale(const mj::PlatformUIAdapter& platform_ui) {
|
||
// compute framebuffer-to-window ratio
|
||
auto [buf_width, buf_height] = platform_ui.GetFramebufferSize();
|
||
auto [win_width, win_height] = platform_ui.GetWindowSize();
|
||
double b2w = static_cast<double>(buf_width) / win_width;
|
||
|
||
// compute PPI
|
||
double PPI = b2w * platform_ui.GetDisplayPixelsPerInch();
|
||
|
||
// estimate font scaling, guard against unrealistic PPI
|
||
int fs;
|
||
if (buf_width > win_width) {
|
||
fs = mju_round(b2w * 100);
|
||
} else if (PPI>50 && PPI<350) {
|
||
fs = mju_round(PPI);
|
||
} else {
|
||
fs = 150;
|
||
}
|
||
fs = mju_round(fs * 0.02) * 50;
|
||
fs = mjMIN(300, mjMAX(100, fs));
|
||
|
||
return fs;
|
||
}
|
||
|
||
|
||
//---------------------------------- UI handlers ---------------------------------------------------
|
||
|
||
// determine enable/disable item state given category
|
||
int UiPredicate(int category, void* userdata) {
|
||
mj::Simulate* sim = static_cast<mj::Simulate*>(userdata);
|
||
|
||
switch (category) {
|
||
case 2: // require model
|
||
return sim->m_ || sim->is_passive_;
|
||
|
||
case 3: // require model and nkey
|
||
return (sim->m_ || sim->is_passive_) && sim->nkey_;
|
||
|
||
case 4: // require model and paused
|
||
return sim->m_ && !sim->run;
|
||
|
||
case 5: // require model and fully managed mode
|
||
return !sim->is_passive_ && sim->m_;
|
||
|
||
default:
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
// set window layout
|
||
void UiLayout(mjuiState* state) {
|
||
mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
|
||
mjrRect* rect = state->rect;
|
||
|
||
// set number of rectangles
|
||
state->nrect = 4;
|
||
|
||
// rect 1: UI 0
|
||
rect[1].left = 0;
|
||
rect[1].width = sim->ui0_enable ? sim->ui0.width : 0;
|
||
rect[1].bottom = 0;
|
||
rect[1].height = rect[0].height;
|
||
|
||
// rect 2: UI 1
|
||
rect[2].width = sim->ui1_enable ? sim->ui1.width : 0;
|
||
rect[2].left = mjMAX(0, rect[0].width - rect[2].width);
|
||
rect[2].bottom = 0;
|
||
rect[2].height = rect[0].height;
|
||
|
||
// rect 3: 3D plot (everything else is an overlay)
|
||
rect[3].left = rect[1].width;
|
||
rect[3].width = mjMAX(0, rect[0].width - rect[1].width - rect[2].width);
|
||
rect[3].bottom = 0;
|
||
rect[3].height = rect[0].height;
|
||
}
|
||
|
||
// modify UI
|
||
void UiModify(mjUI* ui, mjuiState* state, mjrContext* con) {
|
||
mjui_resize(ui, con);
|
||
|
||
// remake aux buffer only if missing or different
|
||
int id = ui->auxid;
|
||
if (con->auxFBO[id] == 0 ||
|
||
con->auxFBO_r[id] == 0 ||
|
||
con->auxColor[id] == 0 ||
|
||
con->auxColor_r[id] == 0 ||
|
||
con->auxWidth[id] != ui->width ||
|
||
con->auxHeight[id] != ui->maxheight ||
|
||
con->auxSamples[id] != ui->spacing.samples) {
|
||
mjr_addAux(id, ui->width, ui->maxheight, ui->spacing.samples, con);
|
||
}
|
||
|
||
UiLayout(state);
|
||
mjui_update(-1, -1, ui, state, con);
|
||
}
|
||
|
||
// handle UI event
|
||
void UiEvent(mjuiState* state) {
|
||
mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
|
||
|
||
// call UI 0 if event is directed to it
|
||
if ((state->dragrect==sim->ui0.rectid) ||
|
||
(state->dragrect==0 && state->mouserect==sim->ui0.rectid) ||
|
||
state->type==mjEVENT_KEY) {
|
||
// process UI event
|
||
mjuiItem* it = mjui_event(&sim->ui0, state, &sim->platform_ui->mjr_context());
|
||
|
||
// file section
|
||
if (it && it->sectionid==SECT_FILE) {
|
||
switch (it->itemid) {
|
||
case 0: // Save xml
|
||
sim->pending_.save_xml = GetSavePath("mjmodel.xml");
|
||
break;
|
||
|
||
case 1: // Save mjb
|
||
sim->pending_.save_mjb = GetSavePath("mjmodel.mjb");
|
||
break;
|
||
|
||
case 2: // Print model
|
||
sim->pending_.print_model = GetSavePath("MJMODEL.TXT");
|
||
break;
|
||
|
||
case 3: // Print data
|
||
sim->pending_.print_data = GetSavePath("MJDATA.TXT");
|
||
break;
|
||
|
||
case 4: // Quit
|
||
sim->exitrequest.store(1);
|
||
break;
|
||
|
||
case 5: // Screenshot
|
||
sim->screenshotrequest.store(true);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// option section
|
||
else if (it && it->sectionid==SECT_OPTION) {
|
||
if (it->pdata == &sim->spacing) {
|
||
sim->ui0.spacing = mjui_themeSpacing(sim->spacing);
|
||
sim->ui1.spacing = mjui_themeSpacing(sim->spacing);
|
||
} else if (it->pdata == &sim->color) {
|
||
sim->ui0.color = mjui_themeColor(sim->color);
|
||
sim->ui1.color = mjui_themeColor(sim->color);
|
||
} else if (it->pdata == &sim->font) {
|
||
mjr_changeFont(50*(sim->font+1), &sim->platform_ui->mjr_context());
|
||
} else if (it->pdata == &sim->fullscreen) {
|
||
sim->platform_ui->ToggleFullscreen();
|
||
} else if (it->pdata == &sim->vsync) {
|
||
sim->platform_ui->SetVSync(sim->vsync);
|
||
}
|
||
|
||
// modify UI
|
||
UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context());
|
||
UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
// simulation section
|
||
else if (it && it->sectionid==SECT_SIMULATION) {
|
||
switch (it->itemid) {
|
||
case 1: // Reset
|
||
sim->pending_.reset = true;
|
||
break;
|
||
|
||
case 2: // Reload
|
||
sim->uiloadrequest.fetch_add(1);
|
||
break;
|
||
|
||
case 3: // Align
|
||
sim->pending_.align = true;
|
||
break;
|
||
|
||
case 4: // Copy key
|
||
sim->pending_.copy_key = true;
|
||
sim->pending_.copy_key_full_precision = sim->platform_ui->IsShiftKeyPressed();
|
||
break;
|
||
|
||
case 5: // Adjust key
|
||
case 6: // Load key
|
||
sim->pending_.load_key = true;
|
||
break;
|
||
|
||
case 7: // Save key
|
||
sim->pending_.save_key = true;
|
||
break;
|
||
|
||
case 11: // History scrubber
|
||
sim->run = 0;
|
||
sim->pending_.load_from_history = true;
|
||
mjui0_update_section(sim, SECT_SIMULATION);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// physics section
|
||
else if (it && it->sectionid==SECT_PHYSICS && sim->m_) {
|
||
mjOption* opt = sim->is_passive_ ? &sim->m_passive_->opt : &sim->m_->opt;
|
||
|
||
// update disable flags in mjOption
|
||
opt->disableflags = 0;
|
||
for (int i=0; i<mjNDISABLE; i++) {
|
||
if (sim->disable[i]) {
|
||
opt->disableflags |= (1<<i);
|
||
}
|
||
}
|
||
|
||
// update enable flags in mjOption
|
||
opt->enableflags = 0;
|
||
for (int i=0; i<mjNENABLE; i++) {
|
||
if (sim->enable[i]) {
|
||
opt->enableflags |= (1<<i);
|
||
}
|
||
}
|
||
|
||
// update disableactuator bitflag in mjOption
|
||
bool group_changed = false;
|
||
for (int i=0; i<mjNGROUP; i++) {
|
||
if ((!sim->enableactuator[i]) != (opt->disableactuator & (1<<i))) {
|
||
group_changed = true;
|
||
if (!sim->enableactuator[i]) {
|
||
// disable actuator group i
|
||
opt->disableactuator |= (1<<i);
|
||
} else {
|
||
// enable actuator group i
|
||
opt->disableactuator &= ~(1<<i);
|
||
}
|
||
}
|
||
}
|
||
|
||
// remake control section if actuator disable group changed
|
||
if (group_changed) {
|
||
sim->pending_.ui_remake_ctrl = true;
|
||
}
|
||
}
|
||
|
||
// rendering section
|
||
else if (it && it->sectionid==SECT_RENDERING) {
|
||
// set camera in mjvCamera
|
||
if (sim->camera==0) {
|
||
sim->cam.type = mjCAMERA_FREE;
|
||
} else if (sim->camera==1) {
|
||
if (sim->pert.select>0) {
|
||
sim->cam.type = mjCAMERA_TRACKING;
|
||
sim->cam.trackbodyid = sim->pert.select;
|
||
sim->cam.fixedcamid = -1;
|
||
} else {
|
||
sim->cam.type = mjCAMERA_FREE;
|
||
sim->camera = 0;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
}
|
||
} else {
|
||
sim->cam.type = mjCAMERA_FIXED;
|
||
sim->cam.fixedcamid = sim->camera - 2;
|
||
}
|
||
// copy camera spec to clipboard (as MJCF element)
|
||
if (it->itemid == 3) {
|
||
CopyCamera(sim);
|
||
}
|
||
}
|
||
|
||
// visualization section
|
||
else if (it && it->sectionid==SECT_VISUALIZATION) {
|
||
if (!mju::strcmp_arr(it->name, "Align")) {
|
||
sim->pending_.align = true;
|
||
}
|
||
}
|
||
|
||
// group section
|
||
else if (it && it->sectionid==SECT_GROUP) {
|
||
// remake joint section if joint group changed
|
||
if (it->name[0]=='J' && it->name[1]=='o') {
|
||
sim->ui1.nsect = SECT_JOINT;
|
||
MakeJointSection(sim);
|
||
sim->ui1.nsect = NSECT1;
|
||
UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
|
||
}
|
||
|
||
// remake control section if actuator group changed
|
||
if (it->name[0]=='A' && it->name[1]=='c') {
|
||
sim->pending_.ui_remake_ctrl = true;
|
||
}
|
||
}
|
||
|
||
// stop if UI processed event
|
||
if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) {
|
||
return;
|
||
}
|
||
}
|
||
|
||
// call UI 1 if event is directed to it
|
||
if ((state->dragrect==sim->ui1.rectid) ||
|
||
(state->dragrect==0 && state->mouserect==sim->ui1.rectid) ||
|
||
state->type==mjEVENT_KEY) {
|
||
// process UI event
|
||
mjuiItem* it = mjui_event(&sim->ui1, state, &sim->platform_ui->mjr_context());
|
||
|
||
// control section
|
||
if (it && it->sectionid==SECT_CONTROL) {
|
||
// clear controls
|
||
if (it->itemid==0) {
|
||
sim->pending_.zero_ctrl = true;
|
||
}
|
||
}
|
||
|
||
// stop if UI processed event
|
||
if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) {
|
||
return;
|
||
}
|
||
}
|
||
|
||
// shortcut not handled by UI
|
||
if (state->type==mjEVENT_KEY && state->key!=0) {
|
||
switch (state->key) {
|
||
case ' ': // Mode
|
||
if (!sim->is_passive_ && sim->m_) {
|
||
sim->run = 1 - sim->run;
|
||
sim->pert.active = 0;
|
||
|
||
if (sim->run) sim->scrub_index = 0; // reset scrubber
|
||
|
||
mjui0_update_section(sim, -1);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_RIGHT: // step forward
|
||
if (!sim->is_passive_ && sim->m_ && !sim->run) {
|
||
ClearTimers(sim->d_);
|
||
|
||
// currently in scrubber: increment scrub, load state, update slider UI
|
||
if (sim->scrub_index < 0) {
|
||
sim->scrub_index++;
|
||
sim->pending_.load_from_history = true;
|
||
mjui0_update_section(sim, SECT_SIMULATION);
|
||
}
|
||
|
||
// not in scrubber: step, add to history buffer
|
||
else {
|
||
mj_step(sim->m_, sim->d_);
|
||
sim->AddToHistory();
|
||
}
|
||
|
||
UpdateProfiler(sim, sim->m_, sim->d_);
|
||
UpdateSensor(sim, sim->m_, sim->d_);
|
||
UpdateSettings(sim, sim->m_);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_LEFT: // step backward
|
||
if (!sim->is_passive_ && sim->m_) {
|
||
sim->run = 0;
|
||
ClearTimers(sim->d_);
|
||
|
||
// decrement scrub, load state
|
||
sim->scrub_index = mjMAX(sim->scrub_index - 1, 1 - sim->nhistory_);
|
||
sim->pending_.load_from_history = true;
|
||
|
||
// update slider UI, profiler, sensor
|
||
mjui0_update_section(sim, SECT_SIMULATION);
|
||
UpdateProfiler(sim, sim->m_, sim->d_);
|
||
UpdateSensor(sim, sim->m_, sim->d_);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_PAGE_UP: // select parent body
|
||
if ((sim->m_ || sim->is_passive_) && sim->pert.select > 0) {
|
||
sim->pert.select = sim->body_parentid_[sim->pert.select];
|
||
sim->pert.flexselect = -1;
|
||
sim->pert.skinselect = -1;
|
||
|
||
// stop perturbation if world reached
|
||
if (sim->pert.select<=0) {
|
||
sim->pert.active = 0;
|
||
}
|
||
}
|
||
|
||
break;
|
||
|
||
case ']': // cycle up fixed cameras
|
||
if ((sim->m_ || !sim->is_passive_) && sim->ncam_) {
|
||
sim->cam.type = mjCAMERA_FIXED;
|
||
// camera = {0 or 1} are reserved for the free and tracking cameras
|
||
if (sim->camera < 2 || sim->camera == 2 + sim->ncam_ - 1) {
|
||
sim->camera = 2;
|
||
} else {
|
||
sim->camera += 1;
|
||
}
|
||
sim->cam.fixedcamid = sim->camera - 2;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
}
|
||
break;
|
||
|
||
case '[': // cycle down fixed cameras
|
||
if ((sim->m_ || sim->is_passive_) && sim->ncam_) {
|
||
sim->cam.type = mjCAMERA_FIXED;
|
||
// camera = {0 or 1} are reserved for the free and tracking cameras
|
||
if (sim->camera <= 2) {
|
||
sim->camera = 2 + sim->ncam_-1;
|
||
} else {
|
||
sim->camera -= 1;
|
||
}
|
||
sim->cam.fixedcamid = sim->camera - 2;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_F6: // cycle frame visualisation
|
||
if (sim->m_ || sim->is_passive_) {
|
||
sim->opt.frame = (sim->opt.frame + 1) % mjNFRAME;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_F7: // cycle label visualisation
|
||
if (sim->m_ || sim->is_passive_) {
|
||
sim->opt.label = (sim->opt.label + 1) % mjNLABEL;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
}
|
||
break;
|
||
|
||
case mjKEY_ESCAPE: // free camera
|
||
sim->cam.type = mjCAMERA_FREE;
|
||
sim->camera = 0;
|
||
mjui0_update_section(sim, SECT_RENDERING);
|
||
break;
|
||
|
||
case '-': // slow down
|
||
if (!sim->is_passive_) {
|
||
int numclicks = sizeof(sim->percentRealTime) / sizeof(sim->percentRealTime[0]);
|
||
if (sim->real_time_index < numclicks-1 && !state->shift) {
|
||
sim->real_time_index++;
|
||
sim->speed_changed = true;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case '=': // speed up
|
||
if (!sim->is_passive_ && sim->real_time_index > 0 && !state->shift) {
|
||
sim->real_time_index--;
|
||
sim->speed_changed = true;
|
||
}
|
||
break;
|
||
|
||
case mjKEY_TAB: // toggle left/right UI
|
||
if (!state->shift) {
|
||
// toggle left UI
|
||
sim->ui0_enable = !sim->ui0_enable;
|
||
UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context());
|
||
} else {
|
||
// toggle right UI
|
||
sim->ui1_enable = !sim->ui1_enable;
|
||
UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
|
||
}
|
||
break;
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
// local pointers used below
|
||
mjModel* model = sim->is_passive_ ? sim->m_passive_ : sim->m_;
|
||
mjData* data = sim->is_passive_ ? sim->d_passive_ : sim->d_;
|
||
|
||
// 3D scroll
|
||
if (state->type==mjEVENT_SCROLL && state->mouserect==3 && model) {
|
||
// emulate vertical mouse motion = 2% of window height
|
||
mjv_moveCamera(model, mjMOUSE_ZOOM, 0, -zoom_increment*state->sy, &sim->scn, &sim->cam);
|
||
return;
|
||
}
|
||
|
||
// 3D press
|
||
if (state->type==mjEVENT_PRESS && state->mouserect==3) {
|
||
// set perturbation
|
||
int newperturb = 0;
|
||
if (state->control && sim->pert.select>0 && (sim->m_ || sim->is_passive_)) {
|
||
// right: translate; left: rotate
|
||
if (state->right) {
|
||
newperturb = mjPERT_TRANSLATE;
|
||
} else if (state->left) {
|
||
newperturb = mjPERT_ROTATE;
|
||
}
|
||
if (newperturb && !sim->pert.active) {
|
||
sim->pending_.newperturb = newperturb;
|
||
}
|
||
}
|
||
|
||
// handle double-click
|
||
if (state->doubleclick && (sim->m_ || sim->is_passive_)) {
|
||
sim->pending_.select = true;
|
||
std::memcpy(&sim->pending_.select_state, state, sizeof(sim->pending_.select_state));
|
||
|
||
// stop perturbation on select
|
||
sim->pert.active = 0;
|
||
sim->pending_.newperturb = 0;
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
// 3D release
|
||
if (state->type==mjEVENT_RELEASE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
|
||
// stop perturbation
|
||
sim->pert.active = 0;
|
||
sim->pending_.newperturb = 0;
|
||
return;
|
||
}
|
||
|
||
// 3D move
|
||
if (state->type==mjEVENT_MOVE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
|
||
// determine action based on mouse button
|
||
mjtMouse action;
|
||
if (state->right) {
|
||
action = state->shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
|
||
} else if (state->left) {
|
||
action = state->shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
|
||
} else {
|
||
action = mjMOUSE_ZOOM;
|
||
}
|
||
|
||
// move perturb or camera
|
||
mjrRect r = state->rect[3];
|
||
if (sim->pert.active) {
|
||
mjv_movePerturb(model, data, action, state->dx / r.height, -state->dy / r.height,
|
||
&sim->scn, &sim->pert);
|
||
} else {
|
||
mjv_moveCamera(model, action, state->dx / r.height, -state->dy / r.height,
|
||
&sim->scn, &sim->cam);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// Dropped files
|
||
if (state->type == mjEVENT_FILESDROP && state->dropcount > 0 && !sim->is_passive_) {
|
||
while (sim->droploadrequest.load()) {}
|
||
mju::strcpy_arr(sim->dropfilename, state->droppaths[0]);
|
||
sim->droploadrequest.store(true);
|
||
return;
|
||
}
|
||
|
||
// Redraw
|
||
if (state->type == mjEVENT_REDRAW) {
|
||
sim->Render();
|
||
return;
|
||
}
|
||
}
|
||
} // namespace
|
||
|
||
namespace mujoco {
|
||
namespace mju = ::mujoco::sample_util;
|
||
|
||
Simulate::Simulate(std::unique_ptr<PlatformUIAdapter> platform_ui,
|
||
mjvCamera* cam, mjvOption* opt, mjvPerturb* pert,
|
||
bool is_passive)
|
||
: is_passive_(is_passive),
|
||
cam(*cam),
|
||
opt(*opt),
|
||
pert(*pert),
|
||
platform_ui(std::move(platform_ui)),
|
||
uistate(this->platform_ui->state()) {
|
||
mjv_defaultScene(&scn);
|
||
}
|
||
|
||
|
||
//------------------------- Synchronize render and physics threads ---------------------------------
|
||
|
||
// operations which require holding the mutex, prevents racing with physics thread
|
||
void Simulate::Sync(bool state_only) {
|
||
MutexLock lock(this->mtx);
|
||
|
||
if (!m_) {
|
||
return;
|
||
}
|
||
if (this->exitrequest.load()) {
|
||
return;
|
||
}
|
||
|
||
bool update_profiler = this->profiler;
|
||
bool update_sensor = this->sensor;
|
||
|
||
for (int i = 0; i < m_->njnt; ++i) {
|
||
std::optional<std::pair<mjtNum, mjtNum>> range;
|
||
if (m_->jnt_limited[i]) {
|
||
range.emplace(m_->jnt_range[2*i], m_->jnt_range[2*i + 1]);
|
||
}
|
||
if (jnt_range_[i] != range) {
|
||
pending_.ui_update_joint = true;
|
||
jnt_range_[i].swap(range);
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < m_->nu; ++i) {
|
||
std::optional<std::pair<mjtNum, mjtNum>> range;
|
||
if (m_->actuator_ctrllimited[i]) {
|
||
range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]);
|
||
}
|
||
if (actuator_ctrlrange_[i] != range) {
|
||
pending_.ui_remake_ctrl = true;
|
||
actuator_ctrlrange_[i].swap(range);
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < m_->nq; ++i) {
|
||
if (qpos_[i] != qpos_prev_[i]) {
|
||
d_->qpos[i] = qpos_[i];
|
||
} else {
|
||
qpos_[i] = d_->qpos[i];
|
||
}
|
||
if (qpos_prev_[i] != qpos_[i]) {
|
||
pending_.ui_update_joint = true;
|
||
qpos_prev_[i] = qpos_[i];
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < m_->nu; ++i) {
|
||
if (ctrl_[i] != ctrl_prev_[i]) {
|
||
d_->ctrl[i] = ctrl_[i];
|
||
} else {
|
||
ctrl_[i] = d_->ctrl[i];
|
||
}
|
||
if (ctrl_prev_[i] != ctrl_[i]) {
|
||
pending_.ui_update_ctrl = true;
|
||
ctrl_prev_[i] = ctrl_[i];
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < m_->neq; ++i) {
|
||
if (eq_active_[i] != eq_active_prev_[i]) {
|
||
d_->eq_active[i] = eq_active_[i];
|
||
} else {
|
||
eq_active_[i] = d_->eq_active[i];
|
||
}
|
||
if (eq_active_prev_[i] != eq_active_[i]) {
|
||
pending_.ui_update_equality = true;
|
||
eq_active_prev_[i] = eq_active_[i];
|
||
}
|
||
}
|
||
|
||
// in passive mode, synchronize user's mjModel with changes made via the UI
|
||
if (is_passive_) {
|
||
// synchronize mjModel.opt
|
||
if (std::memcmp(&m_passive_->opt, &mjopt_prev_, sizeof(mjOption))) {
|
||
pending_.ui_update_physics = true;
|
||
m_->opt = m_passive_->opt;
|
||
}
|
||
|
||
// synchronize mjModel.vis
|
||
if (std::memcmp(&m_passive_->vis, &mjvis_prev_, sizeof(mjVisual))) {
|
||
pending_.ui_update_visualization = true;
|
||
m_->vis = m_passive_->vis;
|
||
}
|
||
|
||
// synchronize mjModel.stat
|
||
if (std::memcmp(&m_passive_->stat, &mjstat_prev_, sizeof(mjStatistic))) {
|
||
pending_.ui_update_visualization = true;
|
||
m_->stat = m_passive_->stat;
|
||
}
|
||
|
||
// synchronize number of mjWARN_VGEOMFULL warnings
|
||
if (d_passive_->warning[mjWARN_VGEOMFULL].number > warn_vgeomfull_prev_) {
|
||
d_->warning[mjWARN_VGEOMFULL].number +=
|
||
d_passive_->warning[mjWARN_VGEOMFULL].number - warn_vgeomfull_prev_;
|
||
}
|
||
}
|
||
|
||
if (pending_.save_xml) {
|
||
char err[200];
|
||
if (!pending_.save_xml->empty() && !mj_saveLastXML(pending_.save_xml->c_str(), m_, err, 200)) {
|
||
std::printf("Save XML error: %s", err);
|
||
}
|
||
pending_.save_xml = std::nullopt;
|
||
}
|
||
|
||
if (pending_.save_mjb) {
|
||
if (!pending_.save_mjb->empty()) {
|
||
mj_saveModel(m_, pending_.save_mjb->c_str(), nullptr, 0);
|
||
}
|
||
pending_.save_mjb = std::nullopt;
|
||
}
|
||
|
||
if (pending_.print_model) {
|
||
if (!pending_.print_model->empty()) {
|
||
mj_printModel(m_, pending_.print_model->c_str());
|
||
}
|
||
pending_.print_model = std::nullopt;
|
||
}
|
||
|
||
if (pending_.print_data) {
|
||
if (!pending_.print_data->empty()) {
|
||
mj_printData(m_, d_, pending_.print_data->c_str());
|
||
}
|
||
pending_.print_data = std::nullopt;
|
||
}
|
||
|
||
if (pending_.reset) {
|
||
mj_resetData(m_, d_);
|
||
mj_forward(m_, d_);
|
||
load_error[0] = '\0';
|
||
update_profiler = true;
|
||
update_sensor = true;
|
||
scrub_index = 0;
|
||
pending_.ui_update_simulation = true;
|
||
pending_.reset = false;
|
||
}
|
||
|
||
if (pending_.align) {
|
||
AlignAndScaleView(this, m_);
|
||
pending_.align = false;
|
||
}
|
||
|
||
if (pending_.copy_key) {
|
||
CopyKey(this, m_, d_, pending_.copy_key_full_precision);
|
||
pending_.copy_key = false;
|
||
pending_.copy_key_full_precision = false;
|
||
}
|
||
|
||
if (pending_.load_from_history) {
|
||
LoadScrubState(this);
|
||
update_profiler = true;
|
||
update_sensor = true;
|
||
pending_.load_from_history = false;
|
||
}
|
||
|
||
if (pending_.load_key) {
|
||
mj_resetDataKeyframe(m_, d_, this->key);
|
||
mj_forward(m_, d_);
|
||
update_profiler = true;
|
||
update_sensor = true;
|
||
pending_.load_key = false;
|
||
}
|
||
|
||
if (pending_.save_key) {
|
||
mj_setKeyframe(m_, d_, this->key);
|
||
pending_.save_key = false;
|
||
}
|
||
|
||
if (pending_.zero_ctrl) {
|
||
mju_zero(d_->ctrl, m_->nu);
|
||
pending_.zero_ctrl = false;
|
||
}
|
||
|
||
// perturbation onset: reset reference
|
||
if (pending_.newperturb) {
|
||
mjv_initPerturb(m_, d_, &this->scn, &this->pert);
|
||
this->pert.active = pending_.newperturb;
|
||
pending_.newperturb = 0;
|
||
}
|
||
|
||
if (pending_.select) {
|
||
// determine selection mode
|
||
int selmode;
|
||
if (pending_.select_state.button==mjBUTTON_LEFT) {
|
||
selmode = 1;
|
||
} else if (pending_.select_state.control) {
|
||
selmode = 3;
|
||
} else {
|
||
selmode = 2;
|
||
}
|
||
|
||
// find geom and 3D click point, get corresponding body
|
||
mjrRect r = pending_.select_state.rect[3];
|
||
mjtNum selpnt[3];
|
||
int selgeom, selflex, selskin;
|
||
int selbody = mjv_select(m_, d_, &this->opt,
|
||
static_cast<mjtNum>(r.width) / r.height,
|
||
(pending_.select_state.x - r.left) / r.width,
|
||
(pending_.select_state.y - r.bottom) / r.height,
|
||
&this->scn, selpnt, &selgeom, &selflex, &selskin);
|
||
|
||
// set lookat point, start tracking is requested
|
||
if (selmode==2 || selmode==3) {
|
||
// copy selpnt if anything clicked
|
||
if (selbody>=0) {
|
||
mju_copy3(this->cam.lookat, selpnt);
|
||
}
|
||
|
||
// switch to tracking camera if dynamic body clicked
|
||
if (selmode==3 && selbody>0) {
|
||
// mujoco camera
|
||
this->cam.type = mjCAMERA_TRACKING;
|
||
this->cam.trackbodyid = selbody;
|
||
this->cam.fixedcamid = -1;
|
||
|
||
// UI camera
|
||
this->camera = 1;
|
||
pending_.ui_update_rendering = true;
|
||
}
|
||
}
|
||
|
||
// set body selection
|
||
else {
|
||
if (selbody>=0) {
|
||
// record selection
|
||
this->pert.select = selbody;
|
||
this->pert.flexselect = selflex;
|
||
this->pert.skinselect = selskin;
|
||
|
||
// compute localpos
|
||
mjtNum tmp[3];
|
||
mju_sub3(tmp, selpnt, d_->xpos + 3*this->pert.select);
|
||
mju_mulMatTVec(this->pert.localpos, d_->xmat + 9*this->pert.select, tmp, 3, 3);
|
||
} else {
|
||
this->pert.select = 0;
|
||
this->pert.flexselect = -1;
|
||
this->pert.skinselect = -1;
|
||
}
|
||
}
|
||
pending_.select = false;
|
||
}
|
||
|
||
// update scene or sync data from user in passive mode
|
||
if (!is_passive_) {
|
||
mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
|
||
} else {
|
||
if (state_only) {
|
||
int state_size = mj_stateSize(m_, mjSTATE_INTEGRATION);
|
||
mjtNum* state = new mjtNum[state_size];
|
||
mj_getState(m_, d_, state, mjSTATE_INTEGRATION);
|
||
mj_setState(m_passive_, d_passive_, state, mjSTATE_INTEGRATION);
|
||
mj_forward(m_passive_, d_passive_);
|
||
delete[] state;
|
||
} else {
|
||
mjv_copyModel(m_passive_, m_);
|
||
mjv_copyData(d_passive_, m_passive_, d_);
|
||
}
|
||
|
||
// append geoms from user_scn to scratch space
|
||
if (user_scn) {
|
||
user_scn_geoms_.clear();
|
||
user_scn_geoms_.reserve(user_scn->ngeom);
|
||
for (int i = 0; i < user_scn->ngeom; ++i) {
|
||
user_scn_geoms_.push_back(user_scn->geoms[i]);
|
||
}
|
||
}
|
||
|
||
// pick up rendering flags changed via user_scn
|
||
if (user_scn) {
|
||
for (int i = 0; i < mjNRNDFLAG; ++i) {
|
||
if (user_scn->flags[i] != user_scn_flags_prev_[i]) {
|
||
scn.flags[i] = user_scn->flags[i];
|
||
pending_.ui_update_rendering = true;
|
||
}
|
||
}
|
||
Copy(user_scn->flags, scn.flags);
|
||
Copy(user_scn_flags_prev_, user_scn->flags);
|
||
}
|
||
|
||
mjopt_prev_ = m_passive_->opt;
|
||
mjvis_prev_ = m_passive_->vis;
|
||
mjstat_prev_ = m_passive_->stat;
|
||
warn_vgeomfull_prev_ = d_passive_->warning[mjWARN_VGEOMFULL].number;
|
||
}
|
||
|
||
// update settings
|
||
UpdateSettings(this, m_);
|
||
|
||
// update watch
|
||
if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) {
|
||
UpdateWatch(this, m_, d_);
|
||
}
|
||
|
||
// update info text
|
||
if (this->info) {
|
||
UpdateInfoText(this, m_, d_, this->info_title, this->info_content);
|
||
}
|
||
if (update_profiler) { UpdateProfiler(this, m_, d_); }
|
||
if (update_sensor) { UpdateSensor(this, m_, d_); }
|
||
|
||
// clear timers once profiler info has been copied
|
||
ClearTimers(d_);
|
||
|
||
if (this->run || this->is_passive_) {
|
||
// clear old perturbations, apply new
|
||
mju_zero(d_->xfrc_applied, 6*m_->nbody);
|
||
mjv_applyPerturbPose(m_, d_, &this->pert, 0); // mocap bodies only
|
||
mjv_applyPerturbForce(m_, d_, &this->pert);
|
||
} else {
|
||
mjv_applyPerturbPose(m_, d_, &this->pert, 1); // mocap and dynamic bodies
|
||
}
|
||
}
|
||
|
||
//------------------------- Tell the render thread to load a file and wait -------------------------
|
||
void Simulate::LoadMessage(const char* displayed_filename) {
|
||
mju::strcpy_arr(this->filename, displayed_filename);
|
||
|
||
{
|
||
MutexLock lock(mtx);
|
||
this->loadrequest = 3;
|
||
}
|
||
}
|
||
|
||
void Simulate::Load(mjModel* m, mjData* d, const char* displayed_filename) {
|
||
this->mnew_ = m;
|
||
this->dnew_ = d;
|
||
mju::strcpy_arr(this->filename, displayed_filename);
|
||
|
||
{
|
||
MutexLock lock(mtx);
|
||
this->loadrequest = 2;
|
||
|
||
// Wait for the render thread to be done loading
|
||
// so that we know the old model and data's memory can
|
||
// be free'd by the other thread (sometimes python)
|
||
cond_loadrequest.wait(lock, [this]() { return this->loadrequest == 0; });
|
||
}
|
||
}
|
||
|
||
void Simulate::LoadMessageClear(void) {
|
||
{
|
||
MutexLock lock(mtx);
|
||
this->loadrequest = 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------------------------- load mjb or xml model --------------------------------------
|
||
void Simulate::LoadOnRenderThread() {
|
||
this->m_ = this->mnew_;
|
||
this->d_ = this->dnew_;
|
||
|
||
ncam_ = this->m_->ncam;
|
||
nkey_ = this->m_->nkey;
|
||
body_parentid_.resize(this->m_->nbody);
|
||
std::memcpy(body_parentid_.data(), this->m_->body_parentid,
|
||
sizeof(this->m_->body_parentid[0]) * this->m_->nbody);
|
||
|
||
jnt_type_.resize(this->m_->njnt);
|
||
std::memcpy(jnt_type_.data(), this->m_->jnt_type,
|
||
sizeof(this->m_->jnt_type[0]) * this->m_->njnt);
|
||
|
||
jnt_group_.resize(this->m_->njnt);
|
||
std::memcpy(jnt_group_.data(), this->m_->jnt_group,
|
||
sizeof(this->m_->jnt_group[0]) * this->m_->njnt);
|
||
|
||
jnt_qposadr_.resize(this->m_->njnt);
|
||
std::memcpy(jnt_qposadr_.data(), this->m_->jnt_qposadr,
|
||
sizeof(this->m_->jnt_qposadr[0]) * this->m_->njnt);
|
||
|
||
jnt_range_.clear();
|
||
jnt_range_.reserve(this->m_->njnt);
|
||
for (int i = 0; i < this->m_->njnt; ++i) {
|
||
if (this->m_->jnt_limited[i]) {
|
||
jnt_range_.push_back(
|
||
std::make_pair(this->m_->jnt_range[2 * i], this->m_->jnt_range[2 * i + 1]));
|
||
} else {
|
||
jnt_range_.push_back(std::nullopt);
|
||
}
|
||
}
|
||
|
||
jnt_names_.clear();
|
||
jnt_names_.reserve(this->m_->njnt);
|
||
for (int i = 0; i < this->m_->njnt; ++i) {
|
||
jnt_names_.emplace_back(this->m_->names + this->m_->name_jntadr[i]);
|
||
}
|
||
|
||
actuator_group_.resize(this->m_->nu);
|
||
std::memcpy(actuator_group_.data(), this->m_->actuator_group,
|
||
sizeof(this->m_->actuator_group[0]) * this->m_->nu);
|
||
|
||
actuator_ctrlrange_.clear();
|
||
actuator_ctrlrange_.reserve(this->m_->nu);
|
||
for (int i = 0; i < this->m_->nu; ++i) {
|
||
if (this->m_->actuator_ctrllimited[i]) {
|
||
actuator_ctrlrange_.push_back(std::make_pair(
|
||
this->m_->actuator_ctrlrange[2 * i], this->m_->actuator_ctrlrange[2 * i + 1]));
|
||
} else {
|
||
actuator_ctrlrange_.push_back(std::nullopt);
|
||
}
|
||
}
|
||
|
||
actuator_names_.clear();
|
||
actuator_names_.reserve(this->m_->nu);
|
||
for (int i = 0; i < this->m_->nu; ++i) {
|
||
actuator_names_.emplace_back(this->m_->names + this->m_->name_actuatoradr[i]);
|
||
}
|
||
|
||
equality_names_.clear();
|
||
equality_names_.reserve(this->m_->neq);
|
||
for (int i = 0; i < this->m_->neq; ++i) {
|
||
equality_names_.emplace_back(this->m_->names + this->m_->name_eqadr[i]);
|
||
}
|
||
|
||
qpos_.resize(this->m_->nq);
|
||
std::memcpy(qpos_.data(), this->d_->qpos, sizeof(this->d_->qpos[0]) * this->m_->nq);
|
||
qpos_prev_ = qpos_;
|
||
|
||
ctrl_.resize(this->m_->nu);
|
||
std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu);
|
||
ctrl_prev_ = ctrl_;
|
||
|
||
eq_active_.resize(this->m_->neq);
|
||
std::memcpy(eq_active_.data(), this->d_->eq_active, sizeof(this->d_->eq_active[0]) * this->m_->neq);
|
||
eq_active_prev_ = eq_active_;
|
||
|
||
// allocate history buffer: smaller of {2000 states, 100 MB}
|
||
if (!this->is_passive_) {
|
||
constexpr int kMaxHistoryBytes = 1e8;
|
||
|
||
// get state size, size of history buffer
|
||
state_size_ = mj_stateSize(this->m_, mjSTATE_INTEGRATION);
|
||
int state_bytes = state_size_ * sizeof(mjtNum);
|
||
int history_length = mjMIN(INT_MAX / state_bytes, 2000);
|
||
int history_bytes = mjMIN(state_bytes * history_length, kMaxHistoryBytes);
|
||
nhistory_ = history_bytes / state_bytes;
|
||
|
||
// allocate history buffer, reset cursor and UI slider
|
||
history_.clear();
|
||
history_.resize(nhistory_ * state_size_);
|
||
history_cursor_ = 0;
|
||
scrub_index = 0;
|
||
|
||
// fill buffer with initial state
|
||
mj_getState(this->m_, this->d_, history_.data(), mjSTATE_INTEGRATION);
|
||
for (int i = 1; i < nhistory_; ++i) {
|
||
mju_copy(&history_[i * state_size_], history_.data(), state_size_);
|
||
}
|
||
}
|
||
|
||
// re-create scene
|
||
mjv_makeScene(this->m_, &this->scn, kMaxGeom);
|
||
|
||
this->platform_ui->RefreshMjrContext(this->m_, 50*(this->font+1));
|
||
UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
|
||
if (!this->platform_ui->IsGPUAccelerated()) {
|
||
this->scn.flags[mjRND_SHADOW] = 0;
|
||
this->scn.flags[mjRND_REFLECTION] = 0;
|
||
}
|
||
|
||
if (this->user_scn) {
|
||
Copy(this->user_scn->flags, this->scn.flags);
|
||
Copy(this->user_scn_flags_prev_, this->scn.flags);
|
||
}
|
||
|
||
// clear perturbation state
|
||
this->pert.active = 0;
|
||
this->pert.select = 0;
|
||
this->pert.flexselect = -1;
|
||
this->pert.skinselect = -1;
|
||
|
||
// align and scale view unless reloading the same file
|
||
if (this->filename[0] &&
|
||
mju::strcmp_arr(this->filename, this->previous_filename)) {
|
||
AlignAndScaleView(this, this->m_);
|
||
mju::strcpy_arr(this->previous_filename, this->filename);
|
||
}
|
||
|
||
// update scene in managed mode, in passive mode copy data from user (update in RenderLoop)
|
||
if (!is_passive_) {
|
||
mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
|
||
} else {
|
||
mjopt_prev_ = m_->opt;
|
||
opt_prev_ = opt;
|
||
cam_prev_ = cam;
|
||
warn_vgeomfull_prev_ = d_->warning[mjWARN_VGEOMFULL].number;
|
||
|
||
// full copy on init
|
||
m_passive_ = mj_copyModel(nullptr, m_);
|
||
d_passive_ = mj_copyData(nullptr, m_passive_, d_);
|
||
}
|
||
|
||
// set window title to model name
|
||
if (this->m_->names) {
|
||
char title[200] = "MuJoCo : ";
|
||
mju::strcat_arr(title, this->m_->names);
|
||
platform_ui->SetWindowTitle(title);
|
||
}
|
||
|
||
// set keyframe range and divisions
|
||
this->ui0.sect[SECT_SIMULATION].item[5].slider.range[0] = 0;
|
||
this->ui0.sect[SECT_SIMULATION].item[5].slider.range[1] = mjMAX(0, this->m_->nkey - 1);
|
||
this->ui0.sect[SECT_SIMULATION].item[5].slider.divisions = mjMAX(1, this->m_->nkey - 1);
|
||
|
||
// set scrubber range and divisions
|
||
this->ui0.sect[SECT_SIMULATION].item[11].slider.range[0] = 1 - nhistory_;
|
||
this->ui0.sect[SECT_SIMULATION].item[11].slider.divisions = nhistory_;
|
||
|
||
// rebuild UI sections
|
||
MakeUiSections(this, this->m_, this->d_);
|
||
|
||
// full ui update
|
||
UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
UpdateSettings(this, this->m_);
|
||
|
||
// clear request
|
||
this->loadrequest = 0;
|
||
cond_loadrequest.notify_all();
|
||
|
||
// set real time index
|
||
int numclicks = sizeof(this->percentRealTime) / sizeof(this->percentRealTime[0]);
|
||
float min_error = 1e6;
|
||
float desired = mju_log(100*this->m_->vis.global.realtime);
|
||
for (int click=0; click<numclicks; click++) {
|
||
float error = mju_abs(mju_log(this->percentRealTime[click]) - desired);
|
||
if (error < min_error) {
|
||
min_error = error;
|
||
this->real_time_index = click;
|
||
}
|
||
}
|
||
|
||
this->mnew_ = nullptr;
|
||
this->dnew_ = nullptr;
|
||
}
|
||
|
||
|
||
//------------------------------------------- rendering --------------------------------------------
|
||
|
||
// render the ui to the window
|
||
void Simulate::Render() {
|
||
// update rendering context buffer size if required
|
||
if (this->platform_ui->EnsureContextSize()) {
|
||
UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// get 3D rectangle and reduced for profiler
|
||
mjrRect rect = this->uistate.rect[3];
|
||
mjrRect smallrect = rect;
|
||
if (this->profiler) {
|
||
smallrect.width = rect.width - rect.width/4;
|
||
}
|
||
|
||
// no model
|
||
if (!this->is_passive_ && !this->m_) {
|
||
// blank screen
|
||
mjr_rectangle(rect, 0.2f, 0.3f, 0.4f, 1);
|
||
|
||
// label
|
||
if (this->loadrequest) {
|
||
mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, "LOADING...", nullptr,
|
||
&this->platform_ui->mjr_context());
|
||
} else {
|
||
char intro_message[Simulate::kMaxFilenameLength];
|
||
mju::sprintf_arr(intro_message,
|
||
"MuJoCo version %s\nDrag-and-drop model file here", mj_versionString());
|
||
mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, intro_message, 0,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show last loading error
|
||
if (this->load_error[0]) {
|
||
mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// render uis
|
||
if (this->ui0_enable) {
|
||
mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
if (this->ui1_enable) {
|
||
mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// finalize
|
||
this->platform_ui->SwapBuffers();
|
||
|
||
return;
|
||
}
|
||
|
||
// update UI sections from last sync
|
||
if (pending_.ui_update_simulation) {
|
||
if (this->ui0_enable && this->ui0.sect[SECT_SIMULATION].state) {
|
||
mjui0_update_section(this, SECT_SIMULATION);
|
||
}
|
||
pending_.ui_update_simulation = false;
|
||
}
|
||
|
||
if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) {
|
||
mjui0_update_section(this, SECT_WATCH);
|
||
}
|
||
|
||
if (pending_.ui_update_physics) {
|
||
if (this->ui0_enable && this->ui0.sect[SECT_PHYSICS].state) {
|
||
mjui0_update_section(this, SECT_PHYSICS);
|
||
}
|
||
pending_.ui_update_physics = false;
|
||
}
|
||
|
||
if (pending_.ui_update_visualization) {
|
||
if (this->ui0_enable && this->ui0.sect[SECT_VISUALIZATION].state) {
|
||
mjui0_update_section(this, SECT_VISUALIZATION);
|
||
}
|
||
pending_.ui_update_visualization = false;
|
||
}
|
||
|
||
if (is_passive_) {
|
||
if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state &&
|
||
(cam_prev_.type != cam.type ||
|
||
cam_prev_.fixedcamid != cam.fixedcamid ||
|
||
cam_prev_.trackbodyid != cam.trackbodyid ||
|
||
opt_prev_.label != opt.label || opt_prev_.frame != opt.frame ||
|
||
IsDifferent(opt_prev_.flags, opt.flags))) {
|
||
pending_.ui_update_rendering = true;
|
||
}
|
||
|
||
if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state &&
|
||
(IsDifferent(opt_prev_.geomgroup, opt.geomgroup) ||
|
||
IsDifferent(opt_prev_.sitegroup, opt.sitegroup) ||
|
||
IsDifferent(opt_prev_.jointgroup, opt.jointgroup) ||
|
||
IsDifferent(opt_prev_.tendongroup, opt.tendongroup) ||
|
||
IsDifferent(opt_prev_.actuatorgroup, opt.actuatorgroup) ||
|
||
IsDifferent(opt_prev_.flexgroup, opt.flexgroup) ||
|
||
IsDifferent(opt_prev_.skingroup, opt.skingroup))) {
|
||
mjui0_update_section(this, SECT_GROUP);
|
||
}
|
||
|
||
opt_prev_ = opt;
|
||
cam_prev_ = cam;
|
||
}
|
||
|
||
if (pending_.ui_update_rendering) {
|
||
if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state) {
|
||
mjui0_update_section(this, SECT_RENDERING);
|
||
}
|
||
pending_.ui_update_rendering = false;
|
||
}
|
||
|
||
if (pending_.ui_update_joint) {
|
||
if (this->ui1_enable && this->ui1.sect[SECT_JOINT].state) {
|
||
mjui_update(SECT_JOINT, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
pending_.ui_update_joint = false;
|
||
}
|
||
|
||
if (pending_.ui_remake_ctrl) {
|
||
if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) {
|
||
this->ui1.nsect = SECT_CONTROL;
|
||
MakeControlSection(this);
|
||
this->ui1.nsect = NSECT1;
|
||
UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
pending_.ui_remake_ctrl = false;
|
||
}
|
||
|
||
if (pending_.ui_update_ctrl) {
|
||
if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) {
|
||
mjui_update(SECT_CONTROL, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
pending_.ui_update_ctrl = false;
|
||
}
|
||
|
||
if (pending_.ui_update_equality) {
|
||
if (this->ui1_enable && this->ui1.sect[SECT_EQUALITY].state) {
|
||
mjui_update(SECT_EQUALITY, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
pending_.ui_update_equality = false;
|
||
}
|
||
|
||
// render scene
|
||
mjr_render(rect, &this->scn, &this->platform_ui->mjr_context());
|
||
|
||
// show last loading error
|
||
if (this->load_error[0]) {
|
||
mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show pause/loading label
|
||
if (!this->run || this->loadrequest) {
|
||
char label[30] = {'\0'};
|
||
if (this->loadrequest) {
|
||
std::snprintf(label, sizeof(label), "LOADING...");
|
||
} else if (this->scrub_index == 0) {
|
||
std::snprintf(label, sizeof(label), "PAUSE");
|
||
} else {
|
||
std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index);
|
||
}
|
||
mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, label, nullptr,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// get desired and actual percent-of-real-time
|
||
float desiredRealtime = this->percentRealTime[this->real_time_index];
|
||
float actualRealtime = 100 / this->measured_slowdown;
|
||
|
||
// if running, check for misalignment of more than 10%
|
||
float realtime_offset = mju_abs(actualRealtime - desiredRealtime);
|
||
bool misaligned = this->run && realtime_offset > 0.1 * desiredRealtime;
|
||
|
||
// make realtime overlay label
|
||
char rtlabel[30] = {'\0'};
|
||
if (desiredRealtime != 100.0 || misaligned) {
|
||
// print desired realtime
|
||
int labelsize = std::snprintf(rtlabel, sizeof(rtlabel), "%g%%", desiredRealtime);
|
||
|
||
// if misaligned, append to label
|
||
if (misaligned) {
|
||
std::snprintf(rtlabel+labelsize, sizeof(rtlabel)-labelsize, " (%-4.1f%%)", actualRealtime);
|
||
}
|
||
}
|
||
|
||
// show real-time overlay
|
||
if (rtlabel[0]) {
|
||
mjr_overlay(mjFONT_BIG, mjGRID_TOPLEFT, smallrect, rtlabel, nullptr,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show ui 0
|
||
if (this->ui0_enable) {
|
||
mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show ui 1
|
||
if (this->ui1_enable) {
|
||
mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show help
|
||
if (this->help) {
|
||
mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, help_title, help_content,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show info
|
||
if (this->info) {
|
||
mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->info_title, this->info_content,
|
||
&this->platform_ui->mjr_context());
|
||
}
|
||
|
||
// show profiler
|
||
if (this->profiler) {
|
||
ShowProfiler(this, rect);
|
||
}
|
||
|
||
// show sensor
|
||
if (this->sensor) {
|
||
ShowSensor(this, smallrect);
|
||
}
|
||
|
||
// take screenshot, save to file
|
||
if (this->screenshotrequest.exchange(false)) {
|
||
const unsigned int h = uistate.rect[0].height;
|
||
const unsigned int w = uistate.rect[0].width;
|
||
std::unique_ptr<unsigned char[]> rgb(new unsigned char[3*w*h]);
|
||
if (!rgb) {
|
||
mju_error("could not allocate buffer for screenshot");
|
||
}
|
||
mjr_readPixels(rgb.get(), nullptr, uistate.rect[0], &this->platform_ui->mjr_context());
|
||
|
||
// flip up-down
|
||
for (int r = 0; r < h/2; ++r) {
|
||
unsigned char* top_row = &rgb[3*w*r];
|
||
unsigned char* bottom_row = &rgb[3*w*(h-1-r)];
|
||
std::swap_ranges(top_row, top_row+3*w, bottom_row);
|
||
}
|
||
|
||
// save as PNG
|
||
// TODO(b/241577466): Parse the stem of the filename and use a .PNG extension.
|
||
// Unfortunately, if we just yank ".xml"/".mjb" from the filename and append .PNG, the macOS
|
||
// file dialog does not automatically open that location. Thus, we defer to a default
|
||
// "screenshot.png" for now.
|
||
const std::string path = GetSavePath("screenshot.png");
|
||
if (!path.empty()) {
|
||
if (lodepng::encode(path, rgb.get(), w, h, LCT_RGB)) {
|
||
mju_error("could not save screenshot");
|
||
} else {
|
||
std::printf("saved screenshot: %s\n", path.c_str());
|
||
}
|
||
}
|
||
}
|
||
|
||
// user figures
|
||
if (this->newfigurerequest.load() == 1) {
|
||
this->user_figures_.clear();
|
||
std::swap(this->user_figures_, this->user_figures_new_);
|
||
int value = 1;
|
||
this->newfigurerequest.compare_exchange_strong(value, 0);
|
||
}
|
||
for (auto& [viewport, figure] : this->user_figures_) {
|
||
ShowFigure(this, viewport, &figure);
|
||
}
|
||
|
||
// overlay text
|
||
if (this->newtextrequest.load() == 1) {
|
||
this->user_texts_.clear();
|
||
std::swap(this->user_texts_, this->user_texts_new_);
|
||
int value = 1;
|
||
this->newtextrequest.compare_exchange_strong(value, 0);
|
||
}
|
||
for (auto& [font, gridpos, text1, text2] : this->user_texts_) {
|
||
ShowOverlayText(this, rect, font, gridpos, text1, text2);
|
||
}
|
||
|
||
// user images
|
||
if (this->newimagerequest.load() == 1) {
|
||
this->user_images_.clear();
|
||
std::swap(this->user_images_, this->user_images_new_);
|
||
int value = 1;
|
||
this->newimagerequest.compare_exchange_strong(value, 0);
|
||
}
|
||
for (auto& [viewport, image] : this->user_images_) {
|
||
ShowImage(this, viewport, image.get());
|
||
}
|
||
|
||
// finalize
|
||
this->platform_ui->SwapBuffers();
|
||
}
|
||
|
||
|
||
|
||
void Simulate::RenderLoop() {
|
||
// Set timer callback (milliseconds)
|
||
mjcb_time = Timer;
|
||
|
||
// init abstract visualization
|
||
mjv_defaultCamera(&this->cam);
|
||
mjv_defaultOption(&this->opt);
|
||
InitializeProfiler(this);
|
||
InitializeSensor(this);
|
||
|
||
// make empty scene
|
||
if (!is_passive_) {
|
||
mjv_defaultScene(&this->scn);
|
||
mjv_makeScene(nullptr, &this->scn, kMaxGeom);
|
||
}
|
||
|
||
if (!this->platform_ui->IsGPUAccelerated()) {
|
||
this->scn.flags[mjRND_SHADOW] = 0;
|
||
this->scn.flags[mjRND_REFLECTION] = 0;
|
||
}
|
||
|
||
// select default font
|
||
int fontscale = ComputeFontScale(*this->platform_ui);
|
||
this->font = fontscale/50 - 1;
|
||
|
||
// make empty context
|
||
this->platform_ui->RefreshMjrContext(nullptr, fontscale);
|
||
|
||
// init state and uis
|
||
std::memset(&this->uistate, 0, sizeof(mjuiState));
|
||
std::memset(&this->ui0, 0, sizeof(mjUI));
|
||
std::memset(&this->ui1, 0, sizeof(mjUI));
|
||
|
||
auto [buf_width, buf_height] = this->platform_ui->GetFramebufferSize();
|
||
this->uistate.nrect = 1;
|
||
this->uistate.rect[0].width = buf_width;
|
||
this->uistate.rect[0].height = buf_height;
|
||
|
||
this->ui0.spacing = mjui_themeSpacing(this->spacing);
|
||
this->ui0.color = mjui_themeColor(this->color);
|
||
this->ui0.predicate = UiPredicate;
|
||
this->ui0.rectid = 1;
|
||
this->ui0.auxid = 0;
|
||
|
||
this->ui1.spacing = mjui_themeSpacing(this->spacing);
|
||
this->ui1.color = mjui_themeColor(this->color);
|
||
this->ui1.predicate = UiPredicate;
|
||
this->ui1.rectid = 2;
|
||
this->ui1.auxid = 1;
|
||
|
||
// set GUI adapter callbacks
|
||
this->uistate.userdata = this;
|
||
this->platform_ui->SetEventCallback(UiEvent);
|
||
this->platform_ui->SetLayoutCallback(UiLayout);
|
||
|
||
// populate uis with standard sections, open some sections initially
|
||
this->ui0.userdata = this;
|
||
this->ui1.userdata = this;
|
||
mjui_add(&this->ui0, defFile);
|
||
mjui_add(&this->ui0, this->def_option);
|
||
mjui_add(&this->ui0, this->def_simulation);
|
||
this->ui0.sect[0].state = 1;
|
||
this->ui0.sect[1].state = 1;
|
||
this->ui0.sect[2].state = 1;
|
||
mjui_add(&this->ui0, this->def_watch);
|
||
UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
|
||
UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
|
||
|
||
// set VSync to initial value
|
||
this->platform_ui->SetVSync(this->vsync);
|
||
|
||
frames_ = 0;
|
||
last_fps_update_ = mj::Simulate::Clock::now();
|
||
|
||
// run event loop
|
||
while (!this->platform_ui->ShouldCloseWindow() && !this->exitrequest.load()) {
|
||
{
|
||
const MutexLock lock(this->mtx);
|
||
|
||
// load model (not on first pass, to show "loading" label)
|
||
if (this->loadrequest==1) {
|
||
this->LoadOnRenderThread();
|
||
} else if (this->loadrequest == 2) {
|
||
this->loadrequest = 1;
|
||
}
|
||
|
||
// poll and handle events
|
||
this->platform_ui->PollEvents();
|
||
|
||
// upload assets if requested
|
||
bool upload_notify = false;
|
||
if (hfield_upload_ != -1) {
|
||
mjr_uploadHField(m_, &platform_ui->mjr_context(), hfield_upload_);
|
||
hfield_upload_ = -1;
|
||
upload_notify = true;
|
||
}
|
||
if (mesh_upload_ != -1) {
|
||
mjr_uploadMesh(m_, &platform_ui->mjr_context(), mesh_upload_);
|
||
mesh_upload_ = -1;
|
||
upload_notify = true;
|
||
}
|
||
if (texture_upload_ != -1) {
|
||
mjr_uploadTexture(m_, &platform_ui->mjr_context(), texture_upload_);
|
||
texture_upload_ = -1;
|
||
upload_notify = true;
|
||
}
|
||
if (upload_notify) {
|
||
cond_upload_.notify_all();
|
||
}
|
||
|
||
// update scene, doing a full sync if in fully managed mode
|
||
if (!is_passive_) {
|
||
Sync();
|
||
} else if (m_passive_ && d_passive_) {
|
||
// the user has called Sync() in their code
|
||
mjv_updateScene(m_passive_, d_passive_,
|
||
&this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
|
||
|
||
// add user geoms to scene
|
||
int nusergeom = user_scn_geoms_.size();
|
||
int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom);
|
||
if (ngeom < nusergeom) {
|
||
mj_warning(d_passive_, mjWARN_VGEOMFULL, this->scn.maxgeom);
|
||
}
|
||
std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(),
|
||
ngeom * sizeof(mjvGeom));
|
||
this->scn.ngeom += ngeom;
|
||
}
|
||
} // MutexLock (unblocks simulation thread)
|
||
|
||
// render while simulation is running
|
||
this->Render();
|
||
|
||
// update FPS stat, at most 5 times per second
|
||
auto now = mj::Simulate::Clock::now();
|
||
double interval = Seconds(now - last_fps_update_).count();
|
||
++frames_;
|
||
if (interval > 0.2) {
|
||
last_fps_update_ = now;
|
||
fps_ = frames_ / interval;
|
||
frames_ = 0;
|
||
}
|
||
}
|
||
|
||
const MutexLock lock(this->mtx);
|
||
mjv_freeScene(&this->scn);
|
||
if (is_passive_) {
|
||
mj_deleteData(d_passive_);
|
||
mj_deleteModel(m_passive_);
|
||
}
|
||
|
||
this->exitrequest.store(2);
|
||
}
|
||
|
||
// add state to history buffer
|
||
void Simulate::AddToHistory() {
|
||
if (history_.empty()) {
|
||
return;
|
||
}
|
||
|
||
// circular increment of cursor
|
||
history_cursor_ = (history_cursor_ + 1) % nhistory_;
|
||
|
||
// add state at cursor
|
||
mjtNum* state = &history_[state_size_ * history_cursor_];
|
||
mj_getState(m_, d_, state, mjSTATE_INTEGRATION);
|
||
}
|
||
|
||
// inject Brownian noise
|
||
void Simulate::InjectNoise(int key) {
|
||
// no noise, return
|
||
if (ctrl_noise_std <= 0) {
|
||
return;
|
||
}
|
||
|
||
// 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);
|
||
|
||
for (int i=0; i<m_->nu; i++) {
|
||
mjtNum bottom = 0, top = 0, midpoint = 0, halfrange = 1;
|
||
if (m_->actuator_ctrllimited[i]) {
|
||
bottom = m_->actuator_ctrlrange[2*i];
|
||
top = m_->actuator_ctrlrange[2*i+1];
|
||
midpoint = 0.5 * (top + bottom); // target of exponential decay
|
||
halfrange = 0.5 * (top - bottom); // scales noise
|
||
}
|
||
|
||
// overwrite midpoint with keyframe, if given
|
||
if (key >= 0) {
|
||
midpoint = m_->key_ctrl[key*m_->nu+i];
|
||
}
|
||
|
||
// exponential convergence to midpoint at ctrl_noise_rate
|
||
d_->ctrl[i] = rate * d_->ctrl[i] + (1-rate) * midpoint;
|
||
|
||
// add noise
|
||
d_->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr);
|
||
|
||
// clip to range if limited
|
||
if (m_->actuator_ctrllimited[i]) {
|
||
d_->ctrl[i] = mju_clip(d_->ctrl[i], bottom, top);
|
||
}
|
||
}
|
||
}
|
||
|
||
void Simulate::UpdateHField(int hfieldid) {
|
||
MutexLock lock(this->mtx);
|
||
if (!m_ || hfieldid < 0 || hfieldid >= m_->nhfield) {
|
||
return;
|
||
}
|
||
hfield_upload_ = hfieldid;
|
||
cond_upload_.wait(lock, [this]() { return hfield_upload_ == -1; });
|
||
}
|
||
|
||
void Simulate::UpdateMesh(int meshid) {
|
||
MutexLock lock(this->mtx);
|
||
if (!m_ || meshid < 0 || meshid >= m_->nmesh) {
|
||
return;
|
||
}
|
||
mesh_upload_ = meshid;
|
||
cond_upload_.wait(lock, [this]() { return mesh_upload_ == -1; });
|
||
}
|
||
|
||
void Simulate::UpdateTexture(int texid) {
|
||
MutexLock lock(this->mtx);
|
||
if (!m_ || texid < 0 || texid >= m_->ntex) {
|
||
return;
|
||
}
|
||
texture_upload_ = texid;
|
||
cond_upload_.wait(lock, [this]() { return texture_upload_ == -1; });
|
||
}
|
||
} // namespace mujoco
|