Mujoco Simulate例子笔记

背景

这个例子官方给了代码，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