背景
这个例子官方给了代码，sample/simulate.cc,用到了uitools.h,uitools.h
编译出来的结果就是、bin/simulate
可以用来学习整个simulation的框架，以及UI信息的显示
后面有时间会回来注解代码，以及加入更多的UI
编译 makefile
注意，将mujoco的uitools.h,uitools.c复制到跟simulate.cc同一目录
#MAC
#COMMON=-O2 -I../../include -L../../bin -mavx -pthread
#LIBS = -w -lmujoco200 -lglfw.3
#CC = gcc
#LINUX
COMMON=-O2 -I../include -L../bin -mavx -pthread -Wl,-rpath,'$$ORIGIN'
LIBS = ../bin/libmujoco.so -lGL -lm ../bin/libglew.so ../bin/libglfw.so.3
CC = g++
#WINDOWS
#COMMON=/O2 /MT /EHsc /arch:AVX /I../../include /Fe../../bin/
#LIBS = ../../bin/glfw3.lib ../../bin/mujoco200.lib
#CC = cl
ROOT = simulate
all:
$(CC) $(COMMON) simulate.cc uitools.c $(LIBS) -o ../bin/$(ROOT)
main.o:
$(CC) $(COMMON) -c main.c
clean:
rm *.o ../../bin/$(ROOT)
#代码
// 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 <chrono>
#include <cstdio>
#include <cstring>
#include <mutex>
#include <string>
#include <thread>
#include "mjxmacro.h"
#include "uitools.h"
#include "array_safety.h"
namespace mju = ::mujoco::sample_util;
//-------------------------------- global -----------------------------------------------
static constexpr int kBufSize = 1000;
// constants
const int maxgeom = 5000; // preallocated geom array in mjvScene
const double syncmisalign = 0.1; // maximum time mis-alignment before re-sync
const double refreshfactor = 0.7; // fraction of refresh available for simulation
const int max_slow_down = 128; // maximum slow-down quotient
// model and data
mjModel* m = NULL;
mjData* d = NULL;
// filename strings
char filename[kBufSize] = "";
char previous_filename[kBufSize] = "";
// control noise variables
mjtNum* ctrlnoise = nullptr;
// abstract visualization
mjvScene scn;
mjvCamera cam;
mjvOption vopt;
mjvPerturb pert;
mjvFigure figconstraint;
mjvFigure figcost;
mjvFigure figtimer;
mjvFigure figsize;
mjvFigure figsensor;
// OpenGL rendering and UI
GLFWvidmode vmode;
int windowpos[2];
int windowsize[2];
mjrContext con;
GLFWwindow* window = NULL;
mjuiState uistate;
mjUI ui0, ui1;
// UI settings not contained in MuJoCo structures
struct {
// file
int exitrequest = 0;
// option
int spacing = 0;
int color = 0;
int font = 0;
int ui0 = 1;
int ui1 = 1;
int help = 0;
int info = 0;
int profiler = 0;
int sensor = 0;
int fullscreen = 0;
int vsync = 1;
int busywait = 0;
// simulation
int run = 1;
int key = 0;
int loadrequest = 0;
int slow_down = 1;
bool speed_changed = true;
double ctrlnoisestd = 0.0;
double ctrlnoiserate = 0.0;
// watch
char field[mjMAXUITEXT] = "qpos";
int index = 0;
// physics: need sync
int disable[mjNDISABLE];
int enable[mjNENABLE];
// rendering: need sync
int camera = 0;
} settings;
// section ids
enum {
// left ui
SECT_FILE = 0,
SECT_OPTION,
SECT_SIMULATION,
SECT_WATCH,
SECT_PHYSICS,
SECT_RENDERING,
SECT_GROUP,
NSECT0,
// right ui
SECT_JOINT = 0,
SECT_CONTROL,
NSECT1
};
// file section of UI
const mjuiDef defFile[] = {
{mjITEM_SECTION, "File", 1, NULL, "AF"},
{mjITEM_BUTTON, "Save xml", 2, NULL, ""},
{mjITEM_BUTTON, "Save mjb", 2, NULL, ""},
{mjITEM_BUTTON, "Print model", 2, NULL, "CM"},
{mjITEM_BUTTON, "Print data", 2, NULL, "CD"},
{mjITEM_BUTTON, "Quit", 1, NULL, "CQ"},
{mjITEM_END}
};
// option section of UI
const mjuiDef defOption[] = {
{mjITEM_SECTION, "Option", 1, NULL, "AO"},
{mjITEM_SELECT, "Spacing", 1, &settings.spacing, "Tight\nWide"},
{mjITEM_SELECT, "Color", 1, &settings.color, "Default\nOrange\nWhite\nBlack"},
{mjITEM_SELECT, "Font", 1, &settings.font, "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
{mjITEM_CHECKINT, "Left UI (Tab)", 1, &settings.ui0, " #258"},
{mjITEM_CHECKINT, "Right UI", 1, &settings.ui1, "S#258"},
{mjITEM_CHECKINT, "Help", 2, &settings.help, " #290"},
{mjITEM_CHECKINT, "Info", 2, &settings.info, " #291"},
{mjITEM_CHECKINT, "Profiler", 2, &settings.profiler, " #292"},
{mjITEM_CHECKINT, "Sensor", 2, &settings.sensor, " #293"},
#ifdef __APPLE__
{mjITEM_CHECKINT, "Fullscreen", 0, &settings.fullscreen, " #294"},
#else
{mjITEM_CHECKINT, "Fullscreen", 1, &settings.fullscreen, " #294"},
#endif
{mjITEM_CHECKINT, "Vertical Sync", 1, &settings.vsync, " #295"},
{mjITEM_CHECKINT, "Busy Wait", 1, &settings.busywait, " #296"},
{mjITEM_END}
};
// simulation section of UI
const mjuiDef defSimulation[] = {
{mjITEM_SECTION, "Simulation", 1, NULL, "AS"},
{mjITEM_RADIO, "", 2, &settings.run, "Pause\nRun"},
{mjITEM_BUTTON, "Reset", 2, NULL, " #259"},
{mjITEM_BUTTON, "Reload", 2, NULL, "CL"},
{mjITEM_BUTTON, "Align", 2, NULL, "CA"},
{mjITEM_BUTTON, "Copy pose", 2, NULL, "CC"},
{mjITEM_SLIDERINT, "Key", 3, &settings.key, "0 0"},
{mjITEM_BUTTON, "Reset to key", 3},
{mjITEM_BUTTON, "Set key", 3},
{mjITEM_SLIDERNUM, "Noise scale", 2, &settings.ctrlnoisestd, "0 2"},
{mjITEM_SLIDERNUM, "Noise rate", 2, &settings.ctrlnoiserate, "0 2"},
{mjITEM_END}
};
// watch section of UI
const mjuiDef defWatch[] = {
{mjITEM_SECTION, "Watch", 0, NULL, "AW"},
{mjITEM_EDITTXT, "Field", 2, settings.field, "qpos"},
{mjITEM_EDITINT, "Index", 2, &settings.index, "1"},
{mjITEM_STATIC, "Value", 2, NULL, " "},
{mjITEM_END}
};
// help strings
const char help_content[] =
"Alt mouse button\n"
"UI right hold\n"
"UI title double-click\n"
"Space\n"
"Esc\n"
"Right arrow\n"
"Left arrow\n"
"Down arrow\n"
"Up arrow\n"
"Page Up\n"
"Double-click\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";
const char help_title[] =
"Swap left-right\n"
"Show UI shortcuts\n"
"Expand/collapse all \n"
"Pause\n"
"Free camera\n"
"Step forward\n"
"Step back\n"
"Step forward 100\n"
"Step back 100\n"
"Select parent\n"
"Select\n"
"Center\n"
"Track camera\n"
"Zoom\n"
"View rotate\n"
"View translate\n"
"Object rotate\n"
"Object translate";
// info strings
char info_title[kBufSize];
char info_content[kBufSize];
//-------------------------------- profiler, sensor, info, watch -----------------------------------
// init profiler figures
void profilerinit(void) {
int i, n;
// set figures to default
mjv_defaultFigure(&figconstraint);
mjv_defaultFigure(&figcost);
mjv_defaultFigure(&figtimer);
mjv_defaultFigure(&figsize);
// titles
mju::strcpy_arr(figconstraint.title, "Counts");
mju::strcpy_arr(figcost.title, "Convergence (log 10)");
mju::strcpy_arr(figsize.title, "Dimensions");
mju::strcpy_arr(figtimer.title, "CPU time (msec)");
// x-labels
mju::strcpy_arr(figconstraint.xlabel, "Solver iteration");
mju::strcpy_arr(figcost.xlabel, "Solver iteration");
mju::strcpy_arr(figsize.xlabel, "Video frame");
mju::strcpy_arr(figtimer.xlabel, "Video frame");
// y-tick nubmer formats
mju::strcpy_arr(figconstraint.yformat, "%.0f");
mju::strcpy_arr(figcost.yformat, "%.1f");
mju::strcpy_arr(figsize.yformat, "%.0f");
mju::strcpy_arr(figtimer.yformat, "%.2f");
// colors
figconstraint.figurergba[0] = 0.1f;
figcost.figurergba[2] = 0.2f;
figsize.figurergba[0] = 0.1f;
figtimer.figurergba[2] = 0.2f;
figconstraint.figurergba[3] = 0.5f;
figcost.figurergba[3] = 0.5f;
figsize.figurergba[3] = 0.5f;
figtimer.figurergba[3] = 0.5f;
// legends
mju::strcpy_arr(figconstraint.linename[0], "total");
mju::strcpy_arr(figconstraint.linename[1], "active");
mju::strcpy_arr(figconstraint.linename[2], "changed");
mju::strcpy_arr(figconstraint.linename[3], "evals");
mju::strcpy_arr(figconstraint.linename[4], "updates");
mju::strcpy_arr(figcost.linename[0], "improvement");
mju::strcpy_arr(figcost.linename[1], "gradient");
mju::strcpy_arr(figcost.linename[2], "lineslope");
mju::strcpy_arr(figsize.linename[0], "dof");
mju::strcpy_arr(figsize.linename[1], "body");
mju::strcpy_arr(figsize.linename[2], "constraint");
mju::strcpy_arr(figsize.linename[3], "sqrt(nnz)");
mju::strcpy_arr(figsize.linename[4], "contact");
mju::strcpy_arr(figsize.linename[5], "iteration");
mju::strcpy_arr(figtimer.linename[0], "total");
mju::strcpy_arr(figtimer.linename[1], "collision");
mju::strcpy_arr(figtimer.linename[2], "prepare");
mju::strcpy_arr(figtimer.linename[3], "solve");
mju::strcpy_arr(figtimer.linename[4], "other");
// grid sizes
figconstraint.gridsize[0] = 5;
figconstraint.gridsize[1] = 5;
figcost.gridsize[0] = 5;
figcost.gridsize[1] = 5;
figsize.gridsize[0] = 3;
figsize.gridsize[1] = 5;
figtimer.gridsize[0] = 3;
figtimer.gridsize[1] = 5;
// minimum ranges
figconstraint.range[0][0] = 0;
figconstraint.range[0][1] = 20;
figconstraint.range[1][0] = 0;
figconstraint.range[1][1] = 80;
figcost.range[0][0] = 0;
figcost.range[0][1] = 20;
figcost.range[1][0] = -15;
figcost.range[1][1] = 5;
figsize.range[0][0] = -200;
figsize.range[0][1] = 0;
figsize.range[1][0] = 0;
figsize.range[1][1] = 100;
figtimer.range[0][0] = -200;
figtimer.range[0][1] = 0;
figtimer.range[1][0] = 0;
figtimer.range[1][1] = 0.4f;
// init x axis on history figures (do not show yet)
for (n=0; n<6; n++)
for (i=0; i<mjMAXLINEPNT; i++) {
figtimer.linedata[n][2*i] = (float)-i;
figsize.linedata[n][2*i] = (float)-i;
}
}
// update profiler figures
void profilerupdate(void) {
int i, n;
// update constraint figure
figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (i=1; i<5; i++) {
figconstraint.linepnt[i] = figconstraint.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
figconstraint.linepnt[3] = 0;
figconstraint.linepnt[4] = 0;
}
if (m->opt.solver==mjSOL_CG) {
figconstraint.linepnt[4] = 0;
}
for (i=0; i<figconstraint.linepnt[0]; i++) {
// x
figconstraint.linedata[0][2*i] = (float)i;
figconstraint.linedata[1][2*i] = (float)i;
figconstraint.linedata[2][2*i] = (float)i;
figconstraint.linedata[3][2*i] = (float)i;
figconstraint.linedata[4][2*i] = (float)i;
// y
figconstraint.linedata[0][2*i+1] = (float)d->nefc;
figconstraint.linedata[1][2*i+1] = (float)d->solver[i].nactive;
figconstraint.linedata[2][2*i+1] = (float)d->solver[i].nchange;
figconstraint.linedata[3][2*i+1] = (float)d->solver[i].neval;
figconstraint.linedata[4][2*i+1] = (float)d->solver[i].nupdate;
}
// update cost figure
figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (i=1; i<3; i++) {
figcost.linepnt[i] = figcost.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
figcost.linepnt[1] = 0;
figcost.linepnt[2] = 0;
}
for (i=0; i<figcost.linepnt[0]; i++) {
// x
figcost.linedata[0][2*i] = (float)i;
figcost.linedata[1][2*i] = (float)i;
figcost.linedata[2][2*i] = (float)i;
// y
figcost.linedata[0][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].improvement));
figcost.linedata[1][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].gradient));
figcost.linedata[2][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].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;
}
number = mjMAX(1, number);
float tdata[5] = {
(float)(total/number),
(float)(d->timer[mjTIMER_POS_COLLISION].duration/number),
(float)(d->timer[mjTIMER_POS_MAKE].duration/number) +
(float)(d->timer[mjTIMER_POS_PROJECT].duration/number),
(float)(d->timer[mjTIMER_CONSTRAINT].duration/number),
0
};
tdata[4] = tdata[0] - tdata[1] - tdata[2] - tdata[3];
// update figtimer
int pnt = mjMIN(201, figtimer.linepnt[0]+1);
for (n=0; n<5; n++) {
// shift data
for (i=pnt-1; i>0; i--) {
figtimer.linedata[n][2*i+1] = figtimer.linedata[n][2*i-1];
}
// assign new
figtimer.linepnt[n] = pnt;
figtimer.linedata[n][1] = tdata[n];
}
// get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
float sdata[6] = {
(float)m->nv,
(float)m->nbody,
(float)d->nefc,
(float)mju_sqrt((mjtNum)d->solver_nnz),
(float)d->ncon,
(float)d->solver_iter
};
// update figsize
pnt = mjMIN(201, figsize.linepnt[0]+1);
for (n=0; n<6; n++) {
// shift data
for (i=pnt-1; i>0; i--) {
figsize.linedata[n][2*i+1] = figsize.linedata[n][2*i-1];
}
// assign new
figsize.linepnt[n] = pnt;
figsize.linedata[n][1] = sdata[n];
}
}
// show profiler figures
void profilershow(mjrRect rect) {
mjrRect viewport = {
rect.left + rect.width - rect.width/4,
rect.bottom,
rect.width/4,
rect.height/4
};
mjr_figure(viewport, &figtimer, &con);
viewport.bottom += rect.height/4;
mjr_figure(viewport, &figsize, &con);
viewport.bottom += rect.height/4;
mjr_figure(viewport, &figcost, &con);
viewport.bottom += rect.height/4;
mjr_figure(viewport, &figconstraint, &con);
}
// init sensor figure
void sensorinit(void) {
// 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 nubmer format
mju::strcpy_arr(figsensor.yformat, "%.0f");
// 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 sensorupdate(void) {
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] = (float)(adr+i);
figsensor.linedata[lineid][2*p+4*i+2] = (float)(adr+i);
// y
figsensor.linedata[lineid][2*p+4*i+1] = 0;
figsensor.linedata[lineid][2*p+4*i+3] = (float)(d->sensordata[adr+i]/cutoff);
}
// update linepnt
figsensor.linepnt[lineid] = mjMIN(mjMAXLINEPNT-1,
figsensor.linepnt[lineid]+2*dim);
}
}
// show sensor figure
void sensorshow(mjrRect rect) {
// constant width with and without profiler
int width = settings.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, &figsensor, &con);
}
// prepare info text
void infotext(char (&title)[kBufSize], char (&content)[kBufSize], double interval) {
char tmp[20];
// compute solver error
mjtNum solerr = 0;
if (d->solver_iter) {
int ind = mjMIN(d->solver_iter-1, mjNSOLVER-1);
solerr = mju_min(d->solver[ind].improvement, d->solver[ind].gradient);
if (solerr==0) {
solerr = mju_max(d->solver[ind].improvement, d->solver[ind].gradient);
}
}
solerr = mju_log10(mju_max(mjMINVAL, solerr));
// prepare info text
const std::string realtime_nominator = settings.slow_down == 1 ? "" : "1/";
mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nstack\nconbuf\nefcbuf");
mju::sprintf_arr(content,
"%-9.3f %s%d x\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.3f\n%.3f\n%.3f",
d->time, realtime_nominator.c_str(), settings.slow_down,
d->nefc, d->ncon,
settings.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, d->solver_iter,
1/interval,
d->maxuse_stack/(double)d->nstack,
d->maxuse_con/(double)m->nconmax,
d->maxuse_efc/(double)m->njmax);
// 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");
}
}
// sprintf forwarding, to avoid compiler warning in x-macro
void printfield(char (&str)[mjMAXUINAME], void* ptr) {
mju::sprintf_arr(str, "%g", *(mjtNum*)ptr);
}
// update watch
void watch(void) {
// clear
ui0.sect[SECT_WATCH].item[2].multi.nelem = 1;
mju::strcpy_arr(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, settings.field) && \
!mju::strcmp_arr(#TYPE, "mjtNum")) { \
if (settings.index>=0 && settings.index<m->NR*NC) { \
printfield(ui0.sect[SECT_WATCH].item[2].multi.name[0], d->NAME + settings.index); \
} else { \
mju::strcpy_arr(ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid index"); \
} \
return; \
}
MJDATA_POINTERS
#undef X
}
//---------------------------------- UI construction -----------------------------------------------
// make physics section of UI
void makephysics(int oldstate) {
int i;
mjuiDef defPhysics[] = {
{mjITEM_SECTION, "Physics", oldstate, NULL, "AP"},
{mjITEM_SELECT, "Integrator", 2, &(m->opt.integrator), "Euler\nRK4"},
{mjITEM_SELECT, "Collision", 2, &(m->opt.collision), "All\nPair\nDynamic"},
{mjITEM_SELECT, "Cone", 2, &(m->opt.cone), "Pyramidal\nElliptic"},
{mjITEM_SELECT, "Jacobian", 2, &(m->opt.jacobian), "Dense\nSparse\nAuto"},
{mjITEM_SELECT, "Solver", 2, &(m->opt.solver), "PGS\nCG\nNewton"},
{mjITEM_SEPARATOR, "Algorithmic Parameters", 1},
{mjITEM_EDITNUM, "Timestep", 2, &(m->opt.timestep), "1 0 1"},
{mjITEM_EDITINT, "Iterations", 2, &(m->opt.iterations), "1 0 1000"},
{mjITEM_EDITNUM, "Tolerance", 2, &(m->opt.tolerance), "1 0 1"},
{mjITEM_EDITINT, "Noslip Iter", 2, &(m->opt.noslip_iterations), "1 0 1000"},
{mjITEM_EDITNUM, "Noslip Tol", 2, &(m->opt.noslip_tolerance), "1 0 1"},
{mjITEM_EDITINT, "MRR Iter", 2, &(m->opt.mpr_iterations), "1 0 1000"},
{mjITEM_EDITNUM, "MPR Tol", 2, &(m->opt.mpr_tolerance), "1 0 1"},
{mjITEM_EDITNUM, "API Rate", 2, &(m->opt.apirate), "1 0 1000"},
{mjITEM_SEPARATOR, "Physical Parameters", 1},
{mjITEM_EDITNUM, "Gravity", 2, m->opt.gravity, "3"},
{mjITEM_EDITNUM, "Wind", 2, m->opt.wind, "3"},
{mjITEM_EDITNUM, "Magnetic", 2, m->opt.magnetic, "3"},
{mjITEM_EDITNUM, "Density", 2, &(m->opt.density), "1"},
{mjITEM_EDITNUM, "Viscosity", 2, &(m->opt.viscosity), "1"},
{mjITEM_EDITNUM, "Imp Ratio", 2, &(m->opt.impratio), "1"},
{mjITEM_SEPARATOR, "Disable Flags", 1},
{mjITEM_END}
};
mjuiDef defEnableFlags[] = {
{mjITEM_SEPARATOR, "Enable Flags", 1},
{mjITEM_END}
};
mjuiDef defOverride[] = {
{mjITEM_SEPARATOR, "Contact Override", 1},
{mjITEM_EDITNUM, "Margin", 2, &(m->opt.o_margin), "1"},
{mjITEM_EDITNUM, "Sol Imp", 2, &(m->opt.o_solimp), "5"},
{mjITEM_EDITNUM, "Sol Ref", 2, &(m->opt.o_solref), "2"},
{mjITEM_END}
};
// add physics
mjui_add(&ui0, defPhysics);
// add flags programmatically
mjuiDef defFlag[] = {
{mjITEM_CHECKINT, "", 2, NULL, ""},
{mjITEM_END}
};
for (i=0; i<mjNDISABLE; i++) {
mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
defFlag[0].pdata = settings.disable + i;
mjui_add(&ui0, defFlag);
}
mjui_add(&ui0, defEnableFlags);
for (i=0; i<mjNENABLE; i++) {
mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
defFlag[0].pdata = settings.enable + i;
mjui_add(&ui0, defFlag);
}
// add contact override
mjui_add(&ui0, defOverride);
}
// make rendering section of UI
void makerendering(int oldstate) {
int i, j;
mjuiDef defRendering[] = {
{
mjITEM_SECTION,
"Rendering",
oldstate,
NULL,
"AR"
},
{
mjITEM_SELECT,
"Camera",
2,
&(settings.camera),
"Free\nTracking"
},
{
mjITEM_SELECT,
"Label",
2,
&(vopt.label),
"None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
"Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nForce"
},
{
mjITEM_SELECT,
"Frame",
2,
&(vopt.frame),
"None\nBody\nGeom\nSite\nCamera\nLight\nWorld"
},
{
mjITEM_BUTTON,
"Print camera",
2,
NULL,
""
},
{
mjITEM_SEPARATOR,
"Model Elements",
1
},
{
mjITEM_END
}
};
mjuiDef defOpenGL[] = {
{mjITEM_SEPARATOR, "OpenGL Effects", 1},
{mjITEM_END}
};
// add model cameras, up to UI limit
for (i=0; i<mjMIN(m->ncam, mjMAXUIMULTI-2); i++) {
// prepare name
char camname[mjMAXUITEXT] = "\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(&ui0, defRendering);
// add flags programmatically
mjuiDef defFlag[] = {
{mjITEM_CHECKBYTE, "", 2, NULL, ""},
{mjITEM_END}
};
for (i=0; i<mjNVISFLAG; i++) {
// set name, remove "&"
mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
for (j=0; j<strlen(mjVISSTRING[i][0]); j++)
if (mjVISSTRING[i][0][j]=='&') {
mju_strncpy(
defFlag[0].name+j, mjVISSTRING[i][0]+j+1, mju::sizeof_arr(defFlag[0].name)-j);
break;
}
// set shortcut and data
mju::sprintf_arr(defFlag[0].other, " %s", mjVISSTRING[i][2]);
defFlag[0].pdata = vopt.flags + i;
mjui_add(&ui0, defFlag);
}
mjui_add(&ui0, defOpenGL);
for (i=0; i<mjNRNDFLAG; i++) {
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
defFlag[0].pdata = scn.flags + i;
mjui_add(&ui0, defFlag);
}
}
// make group section of UI
void makegroup(int oldstate) {
mjuiDef defGroup[] = {
{mjITEM_SECTION, "Group enable", oldstate, NULL, "AG"},
{mjITEM_SEPARATOR, "Geom groups", 1},
{mjITEM_CHECKBYTE, "Geom 0", 2, vopt.geomgroup, " 0"},
{mjITEM_CHECKBYTE, "Geom 1", 2, vopt.geomgroup+1, " 1"},
{mjITEM_CHECKBYTE, "Geom 2", 2, vopt.geomgroup+2, " 2"},
{mjITEM_CHECKBYTE, "Geom 3", 2, vopt.geomgroup+3, " 3"},
{mjITEM_CHECKBYTE, "Geom 4", 2, vopt.geomgroup+4, " 4"},
{mjITEM_CHECKBYTE, "Geom 5", 2, vopt.geomgroup+5, " 5"},
{mjITEM_SEPARATOR, "Site groups", 1},
{mjITEM_CHECKBYTE, "Site 0", 2, vopt.sitegroup, "S0"},
{mjITEM_CHECKBYTE, "Site 1", 2, vopt.sitegroup+1, "S1"},
{mjITEM_CHECKBYTE, "Site 2", 2, vopt.sitegroup+2, "S2"},
{mjITEM_CHECKBYTE, "Site 3", 2, vopt.sitegroup+3, "S3"},
{mjITEM_CHECKBYTE, "Site 4", 2, vopt.sitegroup+4, "S4"},
{mjITEM_CHECKBYTE, "Site 5", 2, vopt.sitegroup+5, "S5"},
{mjITEM_SEPARATOR, "Joint groups", 1},
{mjITEM_CHECKBYTE, "Joint 0", 2, vopt.jointgroup, ""},
{mjITEM_CHECKBYTE, "Joint 1", 2, vopt.jointgroup+1, ""},
{mjITEM_CHECKBYTE, "Joint 2", 2, vopt.jointgroup+2, ""},
{mjITEM_CHECKBYTE, "Joint 3", 2, vopt.jointgroup+3, ""},
{mjITEM_CHECKBYTE, "Joint 4", 2, vopt.jointgroup+4, ""},
{mjITEM_CHECKBYTE, "Joint 5", 2, vopt.jointgroup+5, ""},
{mjITEM_SEPARATOR, "Tendon groups", 1},
{mjITEM_CHECKBYTE, "Tendon 0", 2, vopt.tendongroup, ""},
{mjITEM_CHECKBYTE, "Tendon 1", 2, vopt.tendongroup+1, ""},
{mjITEM_CHECKBYTE, "Tendon 2", 2, vopt.tendongroup+2, ""},
{mjITEM_CHECKBYTE, "Tendon 3", 2, vopt.tendongroup+3, ""},
{mjITEM_CHECKBYTE, "Tendon 4", 2, vopt.tendongroup+4, ""},
{mjITEM_CHECKBYTE, "Tendon 5", 2, vopt.tendongroup+5, ""},
{mjITEM_SEPARATOR, "Actuator groups", 1},
{mjITEM_CHECKBYTE, "Actuator 0", 2, vopt.actuatorgroup, ""},
{mjITEM_CHECKBYTE, "Actuator 1", 2, vopt.actuatorgroup+1, ""},
{mjITEM_CHECKBYTE, "Actuator 2", 2, vopt.actuatorgroup+2, ""},
{mjITEM_CHECKBYTE, "Actuator 3", 2, vopt.actuatorgroup+3, ""},
{mjITEM_CHECKBYTE, "Actuator 4", 2, vopt.actuatorgroup+4, ""},
{mjITEM_CHECKBYTE, "Actuator 5", 2, vopt.actuatorgroup+5, ""},
{mjITEM_END}
};
// add section
mjui_add(&ui0, defGroup);
}
// make joint section of UI
void makejoint(int oldstate) {
int i;
mjuiDef defJoint[] = {
{mjITEM_SECTION, "Joint", oldstate, NULL, "AJ"},
{mjITEM_END}
};
mjuiDef defSlider[] = {
{mjITEM_SLIDERNUM, "", 2, NULL, "0 1"},
{mjITEM_END}
};
// add section
mjui_add(&ui1, defJoint);
defSlider[0].state = 4;
// add scalar joints, exit if UI limit reached
int itemcnt = 0;
for (i=0; i<m->njnt && itemcnt<mjMAXUIITEM; i++)
if ((m->jnt_type[i]==mjJNT_HINGE || m->jnt_type[i]==mjJNT_SLIDE)) {
// skip if joint group is disabled
if (!vopt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->jnt_group[i]))]) {
continue;
}
// set data and name
defSlider[0].pdata = d->qpos + m->jnt_qposadr[i];
if (m->names[m->name_jntadr[i]]) {
mju::strcpy_arr(defSlider[0].name, m->names+m->name_jntadr[i]);
} else {
mju::sprintf_arr(defSlider[0].name, "joint %d", i);
}
// set range
if (m->jnt_limited[i])
mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
m->jnt_range[2*i], m->jnt_range[2*i+1]);
else if (m->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(&ui1, defSlider);
itemcnt++;
}
}
// make control section of UI
void makecontrol(int oldstate) {
int i;
mjuiDef defControl[] = {
{mjITEM_SECTION, "Control", oldstate, NULL, "AC"},
{mjITEM_BUTTON, "Clear all", 2},
{mjITEM_END}
};
mjuiDef defSlider[] = {
{mjITEM_SLIDERNUM, "", 2, NULL, "0 1"},
{mjITEM_END}
};
// add section
mjui_add(&ui1, defControl);
defSlider[0].state = 2;
// add controls, exit if UI limit reached (Clear button already added)
int itemcnt = 1;
for (i=0; i<m->nu && itemcnt<mjMAXUIITEM; i++) {
// skip if actuator group is disabled
if (!vopt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) {
continue;
}
// set data and name
defSlider[0].pdata = d->ctrl + i;
if (m->names[m->name_actuatoradr[i]]) {
mju::strcpy_arr(defSlider[0].name, m->names+m->name_actuatoradr[i]);
} else {
mju::sprintf_arr(defSlider[0].name, "control %d", i);
}
// set range
if (m->actuator_ctrllimited[i])
mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
m->actuator_ctrlrange[2*i], m->actuator_ctrlrange[2*i+1]);
else {
mju::strcpy_arr(defSlider[0].other, "-1 1");
}
// add and count
mjui_add(&ui1, defSlider);
itemcnt++;
}
}
// make model-dependent UI sections
void makesections(void) {
int i;
// get section open-close state, UI 0
int oldstate0[NSECT0];
for (i=0; i<NSECT0; i++) {
oldstate0[i] = 0;
if (ui0.nsect>i) {
oldstate0[i] = ui0.sect[i].state;
}
}
// get section open-close state, UI 1
int oldstate1[NSECT1];
for (i=0; i<NSECT1; i++) {
oldstate1[i] = 0;
if (ui1.nsect>i) {
oldstate1[i] = ui1.sect[i].state;
}
}
// clear model-dependent sections of UI
ui0.nsect = SECT_PHYSICS;
ui1.nsect = 0;
// make
makephysics(oldstate0[SECT_PHYSICS]);
makerendering(oldstate0[SECT_RENDERING]);
makegroup(oldstate0[SECT_GROUP]);
makejoint(oldstate1[SECT_JOINT]);
makecontrol(oldstate1[SECT_CONTROL]);
}
//---------------------------------- utility functions ---------------------------------------------
// align and scale view
void alignscale(void) {
// autoscale
cam.lookat[0] = m->stat.center[0];
cam.lookat[1] = m->stat.center[1];
cam.lookat[2] = m->stat.center[2];
cam.distance = 1.5 * m->stat.extent;
// set to free camera
cam.type = mjCAMERA_FREE;
}
// copy qpos to clipboard as key
void copykey(void) {
char clipboard[5000] = "<key qpos='";
char buf[200];
// prepare string
for (int i=0; i<m->nq; i++) {
mju::sprintf_arr(buf, i==m->nq-1 ? "%g" : "%g ", d->qpos[i]);
mju::strcat_arr(clipboard, buf);
}
mju::strcat_arr(clipboard, "'/>");
// copy to clipboard
glfwSetClipboardString(window, clipboard);
}
// millisecond timer, for MuJoCo built-in profiler
mjtNum timer(void) {
return (mjtNum)(1000*glfwGetTime());
}
// clear all times
void cleartimers(void) {
for (int i=0; i<mjNTIMER; i++) {
d->timer[i].duration = 0;
d->timer[i].number = 0;
}
}
// print current camera as MJCF specification
void printcamera(mjvGLCamera* camera) {
mjtNum cam_right[3];
mjtNum cam_forward[3];
mjtNum cam_up[3];
mju_f2n(cam_forward, camera[0].forward, 3);
mju_f2n(cam_up, camera[0].up, 3);
mju_cross(cam_right, cam_forward, cam_up);
std::printf("<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]);
}
// update UI 0 when MuJoCo structures change (except for joint sliders)
void updatesettings(void) {
int i;
// physics flags
for (i=0; i<mjNDISABLE; i++) {
settings.disable[i] = ((m->opt.disableflags & (1<<i)) !=0);
}
for (i=0; i<mjNENABLE; i++) {
settings.enable[i] = ((m->opt.enableflags & (1<<i)) !=0);
}
// camera
if (cam.type==mjCAMERA_FIXED) {
settings.camera = 2 + cam.fixedcamid;
} else if (cam.type==mjCAMERA_TRACKING) {
settings.camera = 1;
} else {
settings.camera = 0;
}
// update UI
mjui_update(-1, -1, &ui0, &uistate, &con);
}
// drop file callback
void drop(GLFWwindow* window, int count, const char** paths) {
// make sure list is non-empty
if (count>0) {
mju::strcpy_arr(filename, paths[0]);
settings.loadrequest = 1;
}
}
// load mjb or xml model
void loadmodel(void) {
// clear request
settings.loadrequest = 0;
// make sure filename is not empty
if (!filename[0]) {
return;
}
// load and compile
char error[500] = "";
mjModel* mnew = 0;
if (mju::strlen_arr(filename)>4 &&
!std::strncmp(filename+mju::strlen_arr(filename)-4, ".mjb",
mju::sizeof_arr(filename)-mju::strlen_arr(filename)+4)) {
mnew = mj_loadModel(filename, NULL);
if (!mnew) {
mju::strcpy_arr(error, "could not load binary model");
}
} else {
mnew = mj_loadXML(filename, NULL, error, 500);
}
if (!mnew) {
std::printf("%s\n", error);
return;
}
// compiler warning: print and pause
if (error[0]) {
// mj_forward() below will print the warning message
std::printf("Model compiled, but simulation warning (paused):\n %s\n\n",
error);
settings.run = 0;
}
// delete old model, assign new
mj_deleteData(d);
mj_deleteModel(m);
m = mnew;
d = mj_makeData(m);
mj_forward(m, d);
// allocate ctrlnoise
free(ctrlnoise);
ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
mju_zero(ctrlnoise, m->nu);
// re-create scene and context
mjv_makeScene(m, &scn, maxgeom);
mjr_makeContext(m, &con, 50*(settings.font+1));
// clear perturbation state
pert.active = 0;
pert.select = 0;
pert.skinselect = -1;
// align and scale view unless reloading the same file
if (mju::strcmp_arr(filename, previous_filename)) {
alignscale();
mju::strcpy_arr(previous_filename, filename);
}
// update scene
mjv_updateScene(m, d, &vopt, &pert, &cam, mjCAT_ALL, &scn);
// set window title to model name
if (window && m->names) {
char title[200] = "Simulate : ";
mju::strcat_arr(title, m->names);
glfwSetWindowTitle(window, title);
}
// set keyframe range and divisions
ui0.sect[SECT_SIMULATION].item[5].slider.range[0] = 0;
ui0.sect[SECT_SIMULATION].item[5].slider.range[1] = mjMAX(0, m->nkey - 1);
ui0.sect[SECT_SIMULATION].item[5].slider.divisions = mjMAX(1, m->nkey - 1);
// rebuild UI sections
makesections();
// full ui update
uiModify(window, &ui0, &uistate, &con);
uiModify(window, &ui1, &uistate, &con);
updatesettings();
}
//---------------------------------- UI hooks (for uitools.c) --------------------------------------
// determine enable/disable item state given category
int uiPredicate(int category, void* userdata) {
switch (category) {
case 2: // require model
return (m!=NULL);
case 3: // require model and nkey
return (m && m->nkey);
case 4: // require model and paused
return (m && !settings.run);
default:
return 1;
}
}
// set window layout
void uiLayout(mjuiState* state) {
mjrRect* rect = state->rect;
// set number of rectangles
state->nrect = 4;
// rect 0: entire framebuffer
rect[0].left = 0;
rect[0].bottom = 0;
glfwGetFramebufferSize(window, &rect[0].width, &rect[0].height);
// rect 1: UI 0
rect[1].left = 0;
rect[1].width = settings.ui0 ? ui0.width : 0;
rect[1].bottom = 0;
rect[1].height = rect[0].height;
// rect 2: UI 1
rect[2].width = settings.ui1 ? 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;
}
// handle UI event
void uiEvent(mjuiState* state) {
int i;
char err[200];
// call UI 0 if event is directed to it
if ((state->dragrect==ui0.rectid) ||
(state->dragrect==0 && state->mouserect==ui0.rectid) ||
state->type==mjEVENT_KEY) {
// process UI event
mjuiItem* it = mjui_event(&ui0, state, &con);
// file section
if (it && it->sectionid==SECT_FILE) {
switch (it->itemid) {
case 0: // Save xml
if (!mj_saveLastXML("mjmodel.xml", m, err, 200)) {
std::printf("Save XML error: %s", err);
}
break;
case 1: // Save mjb
mj_saveModel(m, "mjmodel.mjb", NULL, 0);
break;
case 2: // Print model
mj_printModel(m, "MJMODEL.TXT");
break;
case 3: // Print data
mj_printData(m, d, "MJDATA.TXT");
break;
case 4: // Quit
settings.exitrequest = 1;
break;
}
}
// option section
else if (it && it->sectionid==SECT_OPTION) {
switch (it->itemid) {
case 0: // Spacing
ui0.spacing = mjui_themeSpacing(settings.spacing);
ui1.spacing = mjui_themeSpacing(settings.spacing);
break;
case 1: // Color
ui0.color = mjui_themeColor(settings.color);
ui1.color = mjui_themeColor(settings.color);
break;
case 2: // Font
mjr_changeFont(50*(settings.font+1), &con);
break;
case 9: // Full screen
if (glfwGetWindowMonitor(window)) {
// restore window from saved data
glfwSetWindowMonitor(window, NULL, windowpos[0], windowpos[1],
windowsize[0], windowsize[1], 0);
}
// currently windowed: switch to full screen
else {
// save window data
glfwGetWindowPos(window, windowpos, windowpos+1);
glfwGetWindowSize(window, windowsize, windowsize+1);
// switch
glfwSetWindowMonitor(window, glfwGetPrimaryMonitor(), 0, 0,
vmode.width, vmode.height, vmode.refreshRate);
}
// reinstante vsync, just in case
glfwSwapInterval(settings.vsync);
break;
case 10: // Vertical sync
glfwSwapInterval(settings.vsync);
break;
}
// modify UI
uiModify(window, &ui0, state, &con);
uiModify(window, &ui1, state, &con);
}
// simulation section
else if (it && it->sectionid==SECT_SIMULATION) {
switch (it->itemid) {
case 1: // Reset
if (m) {
mj_resetData(m, d);
mj_forward(m, d);
profilerupdate();
sensorupdate();
updatesettings();
}
break;
case 2: // Reload
settings.loadrequest = 1;
break;
case 3: // Align
alignscale();
updatesettings();
break;
case 4: // Copy pose
copykey();
break;
case 5: // Adjust key
case 6: // Reset to key
i = settings.key;
d->time = m->key_time[i];
mju_copy(d->qpos, m->key_qpos+i*m->nq, m->nq);
mju_copy(d->qvel, m->key_qvel+i*m->nv, m->nv);
mju_copy(d->act, m->key_act+i*m->na, m->na);
mju_copy(d->mocap_pos, m->key_mpos+i*3*m->nmocap, 3*m->nmocap);
mju_copy(d->mocap_quat, m->key_mquat+i*4*m->nmocap, 4*m->nmocap);
mj_forward(m, d);
profilerupdate();
sensorupdate();
updatesettings();
break;
case 7: // Set key
i = settings.key;
m->key_time[i] = d->time;
mju_copy(m->key_qpos+i*m->nq, d->qpos, m->nq);
mju_copy(m->key_qvel+i*m->nv, d->qvel, m->nv);
mju_copy(m->key_act+i*m->na, d->act, m->na);
mju_copy(m->key_mpos+i*3*m->nmocap, d->mocap_pos, 3*m->nmocap);
mju_copy(m->key_mquat+i*4*m->nmocap, d->mocap_quat, 4*m->nmocap);
break;
}
}
// physics section
else if (it && it->sectionid==SECT_PHYSICS) {
// update disable flags in mjOption
m->opt.disableflags = 0;
for (i=0; i<mjNDISABLE; i++)
if (settings.disable[i]) {
m->opt.disableflags |= (1<<i);
}
// update enable flags in mjOption
m->opt.enableflags = 0;
for (i=0; i<mjNENABLE; i++)
if (settings.enable[i]) {
m->opt.enableflags |= (1<<i);
}
}
// rendering section
else if (it && it->sectionid==SECT_RENDERING) {
// set camera in mjvCamera
if (settings.camera==0) {
cam.type = mjCAMERA_FREE;
} else if (settings.camera==1) {
if (pert.select>0) {
cam.type = mjCAMERA_TRACKING;
cam.trackbodyid = pert.select;
cam.fixedcamid = -1;
} else {
cam.type = mjCAMERA_FREE;
settings.camera = 0;
mjui_update(SECT_RENDERING, -1, &ui0, &uistate, &con);
}
} else {
cam.type = mjCAMERA_FIXED;
cam.fixedcamid = settings.camera - 2;
}
// print floating camera as MJCF element
if (it->itemid == 3) {
printcamera(scn.camera);
}
}
// 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') {
ui1.nsect = SECT_JOINT;
makejoint(ui1.sect[SECT_JOINT].state);
ui1.nsect = NSECT1;
uiModify(window, &ui1, state, &con);
}
// remake control section if actuator group changed
if (it->name[0]=='A' && it->name[1]=='c') {
ui1.nsect = SECT_CONTROL;
makecontrol(ui1.sect[SECT_CONTROL].state);
ui1.nsect = NSECT1;
uiModify(window, &ui1, state, &con);
}
}
// stop if UI processed event
if (it!=NULL || (state->type==mjEVENT_KEY && state->key==0)) {
return;
}
}
// call UI 1 if event is directed to it
if ((state->dragrect==ui1.rectid) ||
(state->dragrect==0 && state->mouserect==ui1.rectid) ||
state->type==mjEVENT_KEY) {
// process UI event
mjuiItem* it = mjui_event(&ui1, state, &con);
// control section
if (it && it->sectionid==SECT_CONTROL) {
// clear controls
if (it->itemid==0) {
mju_zero(d->ctrl, m->nu);
mjui_update(SECT_CONTROL, -1, &ui1, &uistate, &con);
}
}
// stop if UI processed event
if (it!=NULL || (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 (m) {
settings.run = 1 - settings.run;
pert.active = 0;
mjui_update(-1, -1, &ui0, state, &con);
}
break;
case mjKEY_RIGHT: // step forward
if (m && !settings.run) {
cleartimers();
mj_step(m, d);
profilerupdate();
sensorupdate();
updatesettings();
}
break;
case mjKEY_LEFT: // step back
if (m && !settings.run) {
m->opt.timestep = -m->opt.timestep;
cleartimers();
mj_step(m, d);
m->opt.timestep = -m->opt.timestep;
profilerupdate();
sensorupdate();
updatesettings();
}
break;
case mjKEY_DOWN: // step forward 100
if (m && !settings.run) {
cleartimers();
for (i=0; i<100; i++) {
mj_step(m, d);
}
profilerupdate();
sensorupdate();
updatesettings();
}
break;
case mjKEY_UP: // step back 100
if (m && !settings.run) {
m->opt.timestep = -m->opt.timestep;
cleartimers();
for (i=0; i<100; i++) {
mj_step(m, d);
}
m->opt.timestep = -m->opt.timestep;
profilerupdate();
sensorupdate();
updatesettings();
}
break;
case mjKEY_PAGE_UP: // select parent body
if (m && pert.select>0) {
pert.select = m->body_parentid[pert.select];
pert.skinselect = -1;
// stop perturbation if world reached
if (pert.select<=0) {
pert.active = 0;
}
}
break;
case mjKEY_ESCAPE: // free camera
cam.type = mjCAMERA_FREE;
settings.camera = 0;
mjui_update(SECT_RENDERING, -1, &ui0, &uistate, &con);
break;
case '-': // slow down
if (settings.slow_down < max_slow_down && !state->shift) {
settings.slow_down *= 2;
settings.speed_changed = true;
}
break;
case '=': // speed up
if (settings.slow_down > 1 && !state->shift) {
settings.slow_down /= 2;
settings.speed_changed = true;
}
break;
}
return;
}
// 3D scroll
if (state->type==mjEVENT_SCROLL && state->mouserect==3 && m) {
// emulate vertical mouse motion = 5% of window height
mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*state->sy, &scn, &cam);
return;
}
// 3D press
if (state->type==mjEVENT_PRESS && state->mouserect==3 && m) {
// set perturbation
int newperturb = 0;
if (state->control && pert.select>0) {
// right: translate; left: rotate
if (state->right) {
newperturb = mjPERT_TRANSLATE;
} else if (state->left) {
newperturb = mjPERT_ROTATE;
}
// perturbation onset: reset reference
if (newperturb && !pert.active) {
mjv_initPerturb(m, d, &scn, &pert);
}
}
pert.active = newperturb;
// handle double-click
if (state->doubleclick) {
// determine selection mode
int selmode;
if (state->button==mjBUTTON_LEFT) {
selmode = 1;
} else if (state->control) {
selmode = 3;
} else {
selmode = 2;
}
// find geom and 3D click point, get corresponding body
mjrRect r = state->rect[3];
mjtNum selpnt[3];
int selgeom, selskin;
int selbody = mjv_select(m, d, &vopt,
(mjtNum)r.width/(mjtNum)r.height,
(mjtNum)(state->x-r.left)/(mjtNum)r.width,
(mjtNum)(state->y-r.bottom)/(mjtNum)r.height,
&scn, selpnt, &selgeom, &selskin);
// set lookat point, start tracking is requested
if (selmode==2 || selmode==3) {
// copy selpnt if anything clicked
if (selbody>=0) {
mju_copy3(cam.lookat, selpnt);
}
// switch to tracking camera if dynamic body clicked
if (selmode==3 && selbody>0) {
// mujoco camera
cam.type = mjCAMERA_TRACKING;
cam.trackbodyid = selbody;
cam.fixedcamid = -1;
// UI camera
settings.camera = 1;
mjui_update(SECT_RENDERING, -1, &ui0, &uistate, &con);
}
}
// set body selection
else {
if (selbody>=0) {
// record selection
pert.select = selbody;
pert.skinselect = selskin;
// compute localpos
mjtNum tmp[3];
mju_sub3(tmp, selpnt, d->xpos+3*pert.select);
mju_mulMatTVec(pert.localpos, d->xmat+9*pert.select, tmp, 3, 3);
} else {
pert.select = 0;
pert.skinselect = -1;
}
}
// stop perturbation on select
pert.active = 0;
}
return;
}
// 3D release
if (state->type==mjEVENT_RELEASE && state->dragrect==3 && m) {
// stop perturbation
pert.active = 0;
return;
}
// 3D move
if (state->type==mjEVENT_MOVE && state->dragrect==3 && m) {
// 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 (pert.active)
mjv_movePerturb(m, d, action, state->dx/r.height, -state->dy/r.height,
&scn, &pert);
else
mjv_moveCamera(m, action, state->dx/r.height, -state->dy/r.height,
&scn, &cam);
return;
}
}
//---------------------------------- rendering and simulation --------------------------------------
// sim thread synchronization
std::mutex mtx;
// prepare to render
void prepare(void) {
// data for FPS calculation
static double lastupdatetm = 0;
// update interval, save update time
double tmnow = glfwGetTime();
double interval = tmnow - lastupdatetm;
interval = mjMIN(1, mjMAX(0.0001, interval));
lastupdatetm = tmnow;
// no model: nothing to do
if (!m) {
return;
}
// update scene
mjv_updateScene(m, d, &vopt, &pert, &cam, mjCAT_ALL, &scn);
// update watch
if (settings.ui0 && ui0.sect[SECT_WATCH].state) {
watch();
mjui_update(SECT_WATCH, -1, &ui0, &uistate, &con);
}
// update joint
if (settings.ui1 && ui1.sect[SECT_JOINT].state) {
mjui_update(SECT_JOINT, -1, &ui1, &uistate, &con);
}
// update info text
if (settings.info) {
infotext(info_title, info_content, interval);
}
// update control
if( settings.ui1 && ui1.sect[SECT_CONTROL].state ) {
mjui_update(SECT_CONTROL, -1, &ui1, &uistate, &con);
}
// update profiler
if (settings.profiler && settings.run) {
profilerupdate();
}
// update sensor
if (settings.sensor && settings.run) {
sensorupdate();
}
// clear timers once profiler info has been copied
cleartimers();
}
// render im main thread (while simulating in background thread)
void render(GLFWwindow* window) {
// get 3D rectangle and reduced for profiler
mjrRect rect = uistate.rect[3];
mjrRect smallrect = rect;
if (settings.profiler) {
smallrect.width = rect.width - rect.width/4;
}
// no model
if (!m) {
// blank screen
mjr_rectangle(rect, 0.2f, 0.3f, 0.4f, 1);
// label
if (settings.loadrequest)
mjr_overlay(mjFONT_BIG, mjGRID_TOPRIGHT, smallrect,
"loading", NULL, &con);
else
mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect,
"Drag-and-drop model file here", 0, &con);
// render uis
if (settings.ui0) {
mjui_render(&ui0, &uistate, &con);
}
if (settings.ui1) {
mjui_render(&ui1, &uistate, &con);
}
// finalize
glfwSwapBuffers(window);
return;
}
// render scene
mjr_render(rect, &scn, &con);
// show pause/loading label
if (!settings.run || settings.loadrequest)
mjr_overlay(mjFONT_BIG, mjGRID_TOPRIGHT, smallrect,
settings.loadrequest ? "loading" : "pause", NULL, &con);
// show realtime label
if (settings.run && settings.slow_down != 1) {
std::string realtime_label = "1/" + std::to_string(settings.slow_down) + " x";
mjr_overlay(mjFONT_BIG, mjGRID_TOPRIGHT, smallrect, realtime_label.c_str(), NULL, &con);
}
// show ui 0
if (settings.ui0) {
mjui_render(&ui0, &uistate, &con);
}
// show ui 1
if (settings.ui1) {
mjui_render(&ui1, &uistate, &con);
}
// show help
if (settings.help) {
mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, help_title, help_content, &con);
}
// show info
if (settings.info) {
mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, info_title, info_content, &con);
}
// show profiler
if (settings.profiler) {
profilershow(rect);
}
// show sensor
if (settings.sensor) {
sensorshow(smallrect);
}
// finalize
glfwSwapBuffers(window);
}
// simulate in background thread (while rendering in main thread)
void simulate(void) {
// cpu-sim syncronization point
double cpusync = 0;
mjtNum simsync = 0;
// run until asked to exit
while (!settings.exitrequest) {
// sleep for 1 ms or yield, to let main thread run
// yield results in busy wait - which has better timing but kills battery life
if (settings.run && settings.busywait) {
std::this_thread::yield();
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
// start exclusive access
mtx.lock();
// run only if model is present
if (m) {
// running
if (settings.run) {
// record cpu time at start of iteration
double tmstart = glfwGetTime();
// inject noise
if (settings.ctrlnoisestd) {
// convert rate and scale to discrete time given current timestep
mjtNum rate = mju_exp(-m->opt.timestep / settings.ctrlnoiserate);
mjtNum scale = settings.ctrlnoisestd * mju_sqrt(1-rate*rate);
for (int i=0; i<m->nu; i++) {
// update noise
ctrlnoise[i] = rate * ctrlnoise[i] + scale * mju_standardNormal(nullptr);
// apply noise
d->ctrl[i] = ctrlnoise[i];
}
}
// out-of-sync (for any reason)
mjtNum offset = mju_abs((d->time*settings.slow_down-simsync)-(tmstart-cpusync));
if( d->time*settings.slow_down<simsync || tmstart<cpusync || cpusync==0 ||
offset > syncmisalign*settings.slow_down || settings.speed_changed) {
// re-sync
cpusync = tmstart;
simsync = d->time*settings.slow_down;
settings.speed_changed = false;
// clear old perturbations, apply new
mju_zero(d->xfrc_applied, 6*m->nbody);
mjv_applyPerturbPose(m, d, &pert, 0); // move mocap bodies only
mjv_applyPerturbForce(m, d, &pert);
// run single step, let next iteration deal with timing
mj_step(m, d);
}
// in-sync
else {
// step while simtime lags behind cputime, and within safefactor
while ((d->time*settings.slow_down-simsync) < (glfwGetTime()-cpusync) &&
(glfwGetTime()-tmstart) < refreshfactor/vmode.refreshRate) {
// clear old perturbations, apply new
mju_zero(d->xfrc_applied, 6*m->nbody);
mjv_applyPerturbPose(m, d, &pert, 0); // move mocap bodies only
mjv_applyPerturbForce(m, d, &pert);
// run mj_step
mjtNum prevtm = d->time*settings.slow_down;
mj_step(m, d);
// break on reset
if (d->time*settings.slow_down<prevtm) {
break;
}
}
}
}
// paused
else {
// apply pose perturbation
mjv_applyPerturbPose(m, d, &pert, 1); // move mocap and dynamic bodies
// run mj_forward, to update rendering and joint sliders
mj_forward(m, d);
}
}
// end exclusive access
mtx.unlock();
}
}
//---------------------------------- init and main -------------------------------------------------
// initalize everything
void init(void) {
// print version, check compatibility
std::printf("MuJoCo version %s\n", mj_versionString());
if (mjVERSION_HEADER!=mj_version()) {
mju_error("Headers and library have different versions");
}
// init GLFW, set timer callback (milliseconds)
if (!glfwInit()) {
mju_error("could not initialize GLFW");
}
mjcb_time = timer;
// multisampling
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_VISIBLE, 1);
// get videomode and save
vmode = *glfwGetVideoMode(glfwGetPrimaryMonitor());
// create window
window = glfwCreateWindow((2*vmode.width)/3, (2*vmode.height)/3,
"Simulate", NULL, NULL);
if (!window) {
glfwTerminate();
mju_error("could not create window");
}
// save window position and size
glfwGetWindowPos(window, windowpos, windowpos+1);
glfwGetWindowSize(window, windowsize, windowsize+1);
// make context current, set v-sync
glfwMakeContextCurrent(window);
glfwSwapInterval(settings.vsync);
// init abstract visualization
mjv_defaultCamera(&cam);
mjv_defaultOption(&vopt);
profilerinit();
sensorinit();
// make empty scene
mjv_defaultScene(&scn);
mjv_makeScene(NULL, &scn, maxgeom);
// select default font
int fontscale = uiFontScale(window);
settings.font = fontscale/50 - 1;
// make empty context
mjr_defaultContext(&con);
mjr_makeContext(NULL, &con, fontscale);
// set GLFW callbacks
uiSetCallback(window, &uistate, uiEvent, uiLayout);
glfwSetWindowRefreshCallback(window, render);
glfwSetDropCallback(window, drop);
// init state and uis
std::memset(&uistate, 0, sizeof(mjuiState));
std::memset(&ui0, 0, sizeof(mjUI));
std::memset(&ui1, 0, sizeof(mjUI));
ui0.spacing = mjui_themeSpacing(settings.spacing);
ui0.color = mjui_themeColor(settings.color);
ui0.predicate = uiPredicate;
ui0.rectid = 1;
ui0.auxid = 0;
ui1.spacing = mjui_themeSpacing(settings.spacing);
ui1.color = mjui_themeColor(settings.color);
ui1.predicate = uiPredicate;
ui1.rectid = 2;
ui1.auxid = 1;
// populate uis with standard sections
mjui_add(&ui0, defFile);
mjui_add(&ui0, defOption);
mjui_add(&ui0, defSimulation);
mjui_add(&ui0, defWatch);
uiModify(window, &ui0, &uistate, &con);
uiModify(window, &ui1, &uistate, &con);
}
// run event loop
int main(int argc, const char** argv) {
// initialize everything
init();
// request loadmodel if file given (otherwise drag-and-drop)
if (argc>1) {
mju::strcpy_arr(filename, argv[1]);
settings.loadrequest = 2;
}
// start simulation thread
std::thread simthread(simulate);
// event loop
while (!glfwWindowShouldClose(window) && !settings.exitrequest) {
// start exclusive access (block simulation thread)
mtx.lock();
// load model (not on first pass, to show "loading" label)
if (settings.loadrequest==1) {
loadmodel();
} else if (settings.loadrequest>1) {
settings.loadrequest = 1;
}
// handle events (calls all callbacks)
glfwPollEvents();
// prepare to render
prepare();
// end exclusive access (allow simulation thread to run)
mtx.unlock();
// render while simulation is running
render(window);
}
// stop simulation thread
settings.exitrequest = 1;
simthread.join();
// delete everything we allocated
uiClearCallback(window);
free(ctrlnoise);
mj_deleteData(d);
mj_deleteModel(m);
mjv_freeScene(&scn);
mjr_freeContext(&con);
// terminate GLFW (crashes with Linux NVidia drivers)
#if defined(__APPLE__) || defined(_WIN32)
glfwTerminate();
#endif
return 0;
}
276
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
