Version 2.1: documentation, public API headers, and sample programs.
PiperOrigin-RevId: 403900419
This commit is contained in:
Saran Tunyasuvunakool
@@ -0,0 +1,12 @@
|
||||
COMMON=-O2 -I../include -L../bin -std=c++11 -mavx -pthread -Wl,-rpath,'$$ORIGIN'
|
||||
|
||||
all:
|
||||
g++ $(COMMON) testxml.cc -lmujoco210nogl -o ../bin/testxml
|
||||
g++ $(COMMON) testspeed.cc -lmujoco210nogl -o ../bin/testspeed
|
||||
g++ $(COMMON) compile.cc -lmujoco210nogl -o ../bin/compile
|
||||
g++ $(COMMON) derivative.cc -lmujoco210nogl -fopenmp -o ../bin/derivative
|
||||
g++ $(COMMON) basic.cc -lmujoco210 -lGL -lglew ../bin/libglfw.so.3 -o ../bin/basic
|
||||
g++ $(COMMON) record.cc -lmujoco210 -lGL -lglew ../bin/libglfw.so.3 -o ../bin/record
|
||||
gcc -c -O2 -mavx -I../include ../include/uitools.c
|
||||
g++ $(COMMON) uitools.o simulate.cc -lmujoco210 -lGL -lglew ../bin/libglfw.so.3 -o ../bin/simulate
|
||||
rm *.o
|
||||
@@ -0,0 +1,12 @@
|
||||
COMMON=-O2 -I../include -L../bin -std=c++11 -stdlib=libc++ -mavx -pthread
|
||||
|
||||
all:
|
||||
clang++ $(COMMON) testxml.cc -lmujoco210nogl -o ../bin/testxml
|
||||
clang++ $(COMMON) testspeed.cc -lmujoco210nogl -o ../bin/testspeed
|
||||
clang++ $(COMMON) compile.cc -lmujoco210nogl -o ../bin/compile
|
||||
clang++ $(COMMON) derivative.cc -lmujoco210nogl -o ../bin/derivative
|
||||
clang++ $(COMMON) basic.cc -lmujoco210 -lglfw.3 -o ../bin/basic
|
||||
clang++ $(COMMON) record.cc -lmujoco210 -lglfw.3 -o ../bin/record
|
||||
clang -c -O2 -mavx -I../include ../include/uitools.c
|
||||
clang++ $(COMMON) uitools.o simulate.cc -lmujoco210 -lglfw.3 -o ../bin/simulate
|
||||
rm *.o
|
||||
@@ -0,0 +1,11 @@
|
||||
COMMON=/O2 /MT /EHsc /arch:AVX /I../include /Fe../bin/
|
||||
|
||||
all:
|
||||
cl $(COMMON) testxml.cc ../bin/mujoco210nogl.lib
|
||||
cl $(COMMON) testspeed.cc ../bin/mujoco210nogl.lib
|
||||
cl $(COMMON) compile.cc ../bin/mujoco210nogl.lib
|
||||
cl $(COMMON) derivative.cc /openmp ../bin/mujoco210nogl.lib
|
||||
cl $(COMMON) basic.cc ../bin/glfw3.lib ../bin/mujoco210.lib
|
||||
cl $(COMMON) record.cc ../bin/glfw3.lib ../bin/mujoco210.lib
|
||||
cl $(COMMON) simulate.cc ../include/uitools.c ../bin/glfw3.lib ../bin/mujoco210.lib
|
||||
del *.obj
|
||||
+193
@@ -0,0 +1,193 @@
|
||||
// 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 "mujoco.h"
|
||||
#include "glfw3.h"
|
||||
#include "stdio.h"
|
||||
#include "stdlib.h"
|
||||
#include "string.h"
|
||||
|
||||
|
||||
// MuJoCo data structures
|
||||
mjModel* m = NULL; // MuJoCo model
|
||||
mjData* d = NULL; // MuJoCo data
|
||||
mjvCamera cam; // abstract camera
|
||||
mjvOption opt; // visualization options
|
||||
mjvScene scn; // abstract scene
|
||||
mjrContext con; // custom GPU context
|
||||
|
||||
// mouse interaction
|
||||
bool button_left = false;
|
||||
bool button_middle = false;
|
||||
bool button_right = false;
|
||||
double lastx = 0;
|
||||
double lasty = 0;
|
||||
|
||||
|
||||
// keyboard callback
|
||||
void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods)
|
||||
{
|
||||
// backspace: reset simulation
|
||||
if( act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE )
|
||||
{
|
||||
mj_resetData(m, d);
|
||||
mj_forward(m, d);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// mouse button callback
|
||||
void mouse_button(GLFWwindow* window, int button, int act, int mods)
|
||||
{
|
||||
// update button state
|
||||
button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);
|
||||
button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);
|
||||
button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);
|
||||
|
||||
// update mouse position
|
||||
glfwGetCursorPos(window, &lastx, &lasty);
|
||||
}
|
||||
|
||||
|
||||
// mouse move callback
|
||||
void mouse_move(GLFWwindow* window, double xpos, double ypos)
|
||||
{
|
||||
// no buttons down: nothing to do
|
||||
if( !button_left && !button_middle && !button_right )
|
||||
return;
|
||||
|
||||
// compute mouse displacement, save
|
||||
double dx = xpos - lastx;
|
||||
double dy = ypos - lasty;
|
||||
lastx = xpos;
|
||||
lasty = ypos;
|
||||
|
||||
// get current window size
|
||||
int width, height;
|
||||
glfwGetWindowSize(window, &width, &height);
|
||||
|
||||
// get shift key state
|
||||
bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||
|
||||
glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);
|
||||
|
||||
// determine action based on mouse button
|
||||
mjtMouse action;
|
||||
if( button_right )
|
||||
action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
|
||||
else if( button_left )
|
||||
action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
|
||||
else
|
||||
action = mjMOUSE_ZOOM;
|
||||
|
||||
// move camera
|
||||
mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
|
||||
}
|
||||
|
||||
|
||||
// scroll callback
|
||||
void scroll(GLFWwindow* window, double xoffset, double yoffset)
|
||||
{
|
||||
// emulate vertical mouse motion = 5% of window height
|
||||
mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam);
|
||||
}
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
// check command-line arguments
|
||||
if( argc!=2 )
|
||||
{
|
||||
printf(" USAGE: basic modelfile\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// load and compile model
|
||||
char error[1000] = "Could not load binary model";
|
||||
if( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )
|
||||
m = mj_loadModel(argv[1], 0);
|
||||
else
|
||||
m = mj_loadXML(argv[1], 0, error, 1000);
|
||||
if( !m )
|
||||
mju_error_s("Load model error: %s", error);
|
||||
|
||||
// make data
|
||||
d = mj_makeData(m);
|
||||
|
||||
// init GLFW
|
||||
if( !glfwInit() )
|
||||
mju_error("Could not initialize GLFW");
|
||||
|
||||
// create window, make OpenGL context current, request v-sync
|
||||
GLFWwindow* window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL);
|
||||
glfwMakeContextCurrent(window);
|
||||
glfwSwapInterval(1);
|
||||
|
||||
// initialize visualization data structures
|
||||
mjv_defaultCamera(&cam);
|
||||
mjv_defaultOption(&opt);
|
||||
mjv_defaultScene(&scn);
|
||||
mjr_defaultContext(&con);
|
||||
|
||||
// create scene and context
|
||||
mjv_makeScene(m, &scn, 2000);
|
||||
mjr_makeContext(m, &con, mjFONTSCALE_150);
|
||||
|
||||
// install GLFW mouse and keyboard callbacks
|
||||
glfwSetKeyCallback(window, keyboard);
|
||||
glfwSetCursorPosCallback(window, mouse_move);
|
||||
glfwSetMouseButtonCallback(window, mouse_button);
|
||||
glfwSetScrollCallback(window, scroll);
|
||||
|
||||
// run main loop, target real-time simulation and 60 fps rendering
|
||||
while( !glfwWindowShouldClose(window) )
|
||||
{
|
||||
// advance interactive simulation for 1/60 sec
|
||||
// Assuming MuJoCo can simulate faster than real-time, which it usually can,
|
||||
// this loop will finish on time for the next frame to be rendered at 60 fps.
|
||||
// Otherwise add a cpu timer and exit this loop when it is time to render.
|
||||
mjtNum simstart = d->time;
|
||||
while( d->time - simstart < 1.0/60.0 )
|
||||
mj_step(m, d);
|
||||
|
||||
// get framebuffer viewport
|
||||
mjrRect viewport = {0, 0, 0, 0};
|
||||
glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
|
||||
|
||||
// update scene and render
|
||||
mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
|
||||
mjr_render(viewport, &scn, &con);
|
||||
|
||||
// swap OpenGL buffers (blocking call due to v-sync)
|
||||
glfwSwapBuffers(window);
|
||||
|
||||
// process pending GUI events, call GLFW callbacks
|
||||
glfwPollEvents();
|
||||
}
|
||||
|
||||
//free visualization storage
|
||||
mjv_freeScene(&scn);
|
||||
mjr_freeContext(&con);
|
||||
|
||||
// free MuJoCo model and data
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
|
||||
// terminate GLFW (crashes with Linux NVidia drivers)
|
||||
#if defined(__APPLE__) || defined(_WIN32)
|
||||
glfwTerminate();
|
||||
#endif
|
||||
|
||||
return 1;
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
// 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 "mujoco.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
|
||||
|
||||
// help
|
||||
const char helpstring[] =
|
||||
"\n Usage: compile infile outfile\n"
|
||||
" infile can be in mjcf, urdf, mjb format\n"
|
||||
" outfile can be in mjcf, mjb, txt format\n\n"
|
||||
" Example: compile model.xml model.mjb\n";
|
||||
|
||||
|
||||
// deallocate and print message
|
||||
int finish(const char* msg = 0, mjModel* m = 0)
|
||||
{
|
||||
// deallocated everything
|
||||
if( m )
|
||||
mj_deleteModel(m);
|
||||
|
||||
// print message
|
||||
if( msg )
|
||||
printf("%s\n", msg);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// possible file types
|
||||
enum
|
||||
{
|
||||
typeUNKNOWN = 0,
|
||||
typeXML,
|
||||
typeMJB,
|
||||
typeTXT
|
||||
};
|
||||
|
||||
|
||||
// determine file type
|
||||
int filetype(const char* filename)
|
||||
{
|
||||
// convert to lower case for string comparison
|
||||
char lower[1000];
|
||||
size_t i=0;
|
||||
while( i<strlen(filename) && i<999 )
|
||||
{
|
||||
lower[i] = (char)tolower(filename[i]);
|
||||
i++;
|
||||
}
|
||||
lower[i] = 0;
|
||||
|
||||
// find last dot
|
||||
int dot = (int)strlen(lower);
|
||||
while( dot>=0 && lower[dot]!='.' )
|
||||
dot--;
|
||||
|
||||
// no dot found
|
||||
if( dot<0 )
|
||||
return typeUNKNOWN;
|
||||
|
||||
// check extension
|
||||
if( !strcmp(lower+dot, ".xml") || !strcmp(lower+dot, ".urdf") )
|
||||
return typeXML;
|
||||
else if( !strcmp(lower+dot, ".mjb") )
|
||||
return typeMJB;
|
||||
else if( !strcmp(lower+dot, ".txt") )
|
||||
return typeTXT;
|
||||
else
|
||||
return typeUNKNOWN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
// model and error
|
||||
mjModel* m = 0;
|
||||
char error[1000];
|
||||
|
||||
// print help if arguments are missing
|
||||
if( argc!=3 )
|
||||
return finish(helpstring);
|
||||
|
||||
// determine file types
|
||||
int type1 = filetype(argv[1]);
|
||||
int type2 = filetype(argv[2]);
|
||||
|
||||
// check types
|
||||
if( type1==typeUNKNOWN || type1==typeTXT ||
|
||||
type2==typeUNKNOWN || (type1==typeMJB && type2==typeXML) )
|
||||
return finish("Illegal combination of file formats");
|
||||
|
||||
// make sure output file does not exist
|
||||
FILE* fp = fopen(argv[2], "r");
|
||||
if( fp )
|
||||
{
|
||||
fclose(fp);
|
||||
return finish("Output file already exists");
|
||||
}
|
||||
|
||||
// load model
|
||||
if( type1==typeXML )
|
||||
m = mj_loadXML(argv[1], 0, error, 1000);
|
||||
else
|
||||
m = mj_loadModel(argv[1], 0);
|
||||
|
||||
// check error
|
||||
if( !m )
|
||||
{
|
||||
if( type1==typeXML )
|
||||
return finish(error, 0);
|
||||
else
|
||||
return finish("Could not load model", 0);
|
||||
}
|
||||
|
||||
// save model
|
||||
if( type2==typeXML )
|
||||
{
|
||||
if( mj_saveLastXML(argv[2], m, error, 1000) )
|
||||
return finish(error, m);
|
||||
}
|
||||
else if( type2==typeMJB )
|
||||
mj_saveModel(m, argv[2], 0, 0);
|
||||
else
|
||||
mj_printModel(m, argv[2]);
|
||||
|
||||
// finalize
|
||||
return finish("Done", m);
|
||||
}
|
||||
@@ -0,0 +1,443 @@
|
||||
// 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 "mujoco.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
// enable compilation with and without OpenMP support
|
||||
#if defined(_OPENMP)
|
||||
#include <omp.h>
|
||||
#else
|
||||
// omp timer replacement
|
||||
#include <chrono>
|
||||
double omp_get_wtime(void)
|
||||
{
|
||||
static std::chrono::system_clock::time_point _start = std::chrono::system_clock::now();
|
||||
std::chrono::duration<double> elapsed = std::chrono::system_clock::now() - _start;
|
||||
return elapsed.count();
|
||||
}
|
||||
|
||||
// omp functions used below
|
||||
void omp_set_dynamic(int) {}
|
||||
void omp_set_num_threads(int) {}
|
||||
int omp_get_num_procs(void) {return 1;}
|
||||
#endif
|
||||
|
||||
|
||||
// gloval variables: internal
|
||||
const int MAXTHREAD = 64; // maximum number of threads allowed
|
||||
const int MAXEPOCH = 100; // maximum number of epochs
|
||||
int isforward = 0; // dynamics mode: forward or inverse
|
||||
mjtNum* deriv = 0; // dynamics derivatives (6*nv*nv):
|
||||
// dinv/dpos, dinv/dvel, dinv/dacc, dacc/dpos, dacc/dvel, dacc/dfrc
|
||||
|
||||
|
||||
// global variables: user-defined, with defaults
|
||||
int nthread = 0; // number of parallel threads (default set later)
|
||||
int niter = 30; // fixed number of solver iterations for finite-differencing
|
||||
int nwarmup = 3; // center point repetitions to improve warmstart
|
||||
int nepoch = 20; // number of timing epochs
|
||||
int nstep = 500; // number of simulation steps per epoch
|
||||
double eps = 1e-6; // finite-difference epsilon
|
||||
|
||||
|
||||
// worker function for parallel finite-difference computation of derivatives
|
||||
void worker(const mjModel* m, const mjData* dmain, mjData* d, int id)
|
||||
{
|
||||
int nv = m->nv;
|
||||
|
||||
// allocate stack space for result at center
|
||||
mjMARKSTACK
|
||||
mjtNum* center = mj_stackAlloc(d, nv);
|
||||
mjtNum* warmstart = mj_stackAlloc(d, nv);
|
||||
|
||||
// prepare static schedule: range of derivative columns to be computed by this thread
|
||||
int chunk = (m->nv + nthread-1) / nthread;
|
||||
int istart = id * chunk;
|
||||
int iend = mjMIN(istart + chunk, m->nv);
|
||||
|
||||
// copy state and control from dmain to thread-specific d
|
||||
d->time = dmain->time;
|
||||
mju_copy(d->qpos, dmain->qpos, m->nq);
|
||||
mju_copy(d->qvel, dmain->qvel, m->nv);
|
||||
mju_copy(d->qacc, dmain->qacc, m->nv);
|
||||
mju_copy(d->qacc_warmstart, dmain->qacc_warmstart, m->nv);
|
||||
mju_copy(d->qfrc_applied, dmain->qfrc_applied, m->nv);
|
||||
mju_copy(d->xfrc_applied, dmain->xfrc_applied, 6*m->nbody);
|
||||
mju_copy(d->ctrl, dmain->ctrl, m->nu);
|
||||
|
||||
// run full computation at center point (usually faster than copying dmain)
|
||||
if( isforward )
|
||||
{
|
||||
mj_forward(m, d);
|
||||
|
||||
// extra solver iterations to improve warmstart (qacc) at center point
|
||||
for( int rep=1; rep<nwarmup; rep++ )
|
||||
mj_forwardSkip(m, d, mjSTAGE_VEL, 1);
|
||||
}
|
||||
else
|
||||
mj_inverse(m, d);
|
||||
|
||||
// select output from forward or inverse dynamics
|
||||
mjtNum* output = (isforward ? d->qacc : d->qfrc_inverse);
|
||||
|
||||
// save output for center point and warmstart (needed in forward only)
|
||||
mju_copy(center, output, nv);
|
||||
mju_copy(warmstart, d->qacc_warmstart, nv);
|
||||
|
||||
// select target vector and original vector for force or acceleration derivative
|
||||
mjtNum* target = (isforward ? d->qfrc_applied : d->qacc);
|
||||
const mjtNum* original = (isforward ? dmain->qfrc_applied : dmain->qacc);
|
||||
|
||||
// finite-difference over force or acceleration: skip = mjSTAGE_VEL
|
||||
for( int i=istart; i<iend; i++ )
|
||||
{
|
||||
// perturb selected target
|
||||
target[i] += eps;
|
||||
|
||||
// evaluate dynamics, with center warmstart
|
||||
if( isforward )
|
||||
{
|
||||
mju_copy(d->qacc_warmstart, warmstart, m->nv);
|
||||
mj_forwardSkip(m, d, mjSTAGE_VEL, 1);
|
||||
}
|
||||
else
|
||||
mj_inverseSkip(m, d, mjSTAGE_VEL, 1);
|
||||
|
||||
// undo perturbation
|
||||
target[i] = original[i];
|
||||
|
||||
// compute column i of derivative 2
|
||||
for( int j=0; j<nv; j++ )
|
||||
deriv[(3*isforward+2)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
|
||||
}
|
||||
|
||||
// finite-difference over velocity: skip = mjSTAGE_POS
|
||||
for( int i=istart; i<iend; i++ )
|
||||
{
|
||||
// perturb velocity
|
||||
d->qvel[i] += eps;
|
||||
|
||||
// evaluate dynamics, with center warmstart
|
||||
if( isforward )
|
||||
{
|
||||
mju_copy(d->qacc_warmstart, warmstart, m->nv);
|
||||
mj_forwardSkip(m, d, mjSTAGE_POS, 1);
|
||||
}
|
||||
else
|
||||
mj_inverseSkip(m, d, mjSTAGE_POS, 1);
|
||||
|
||||
// undo perturbation
|
||||
d->qvel[i] = dmain->qvel[i];
|
||||
|
||||
// compute column i of derivative 1
|
||||
for( int j=0; j<nv; j++ )
|
||||
deriv[(3*isforward+1)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
|
||||
}
|
||||
|
||||
// finite-difference over position: skip = mjSTAGE_NONE
|
||||
for( int i=istart; i<iend; i++ )
|
||||
{
|
||||
// get joint id for this dof
|
||||
int jid = m->dof_jntid[i];
|
||||
|
||||
// get quaternion address and dof position within quaternion (-1: not in quaternion)
|
||||
int quatadr = -1, dofpos = 0;
|
||||
if( m->jnt_type[jid]==mjJNT_BALL )
|
||||
{
|
||||
quatadr = m->jnt_qposadr[jid];
|
||||
dofpos = i - m->jnt_dofadr[jid];
|
||||
}
|
||||
else if( m->jnt_type[jid]==mjJNT_FREE && i>=m->jnt_dofadr[jid]+3 )
|
||||
{
|
||||
quatadr = m->jnt_qposadr[jid] + 3;
|
||||
dofpos = i - m->jnt_dofadr[jid] - 3;
|
||||
}
|
||||
|
||||
// apply quaternion or simple perturbation
|
||||
if( quatadr>=0 )
|
||||
{
|
||||
mjtNum angvel[3] = {0,0,0};
|
||||
angvel[dofpos] = eps;
|
||||
mju_quatIntegrate(d->qpos+quatadr, angvel, 1);
|
||||
}
|
||||
else
|
||||
d->qpos[m->jnt_qposadr[jid] + i - m->jnt_dofadr[jid]] += eps;
|
||||
|
||||
// evaluate dynamics, with center warmstart
|
||||
if( isforward )
|
||||
{
|
||||
mju_copy(d->qacc_warmstart, warmstart, m->nv);
|
||||
mj_forwardSkip(m, d, mjSTAGE_NONE, 1);
|
||||
}
|
||||
else
|
||||
mj_inverseSkip(m, d, mjSTAGE_NONE, 1);
|
||||
|
||||
// undo perturbation
|
||||
mju_copy(d->qpos, dmain->qpos, m->nq);
|
||||
|
||||
// compute column i of derivative 0
|
||||
for( int j=0; j<nv; j++ )
|
||||
deriv[(3*isforward+0)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
|
||||
}
|
||||
|
||||
mjFREESTACK
|
||||
}
|
||||
|
||||
|
||||
// compute relative L1 norm of residual
|
||||
double relnorm(mjtNum* residual, mjtNum* base, int n)
|
||||
{
|
||||
mjtNum L1res = 0, L1base = 0;
|
||||
for( int i=0; i<n; i++ )
|
||||
{
|
||||
L1res += mju_abs(residual[i]);
|
||||
L1base += mju_abs(base[i]);
|
||||
}
|
||||
|
||||
return (double) mju_log10(mju_max(mjMINVAL,L1res/mju_max(mjMINVAL,L1base)));
|
||||
}
|
||||
|
||||
|
||||
// names of residuals for accuracy check
|
||||
const char* accuracy[8] = {
|
||||
"G2*F2 - I ",
|
||||
"G2 - G2' ",
|
||||
"G1 - G1' ",
|
||||
"F2 - F2' ",
|
||||
"G1 + G2*F1",
|
||||
"G0 + G2*F0",
|
||||
"F1 + F2*G1",
|
||||
"F0 + F2*G0"
|
||||
};
|
||||
|
||||
|
||||
// check accuracy of derivatives using known mathematical identities
|
||||
void checkderiv(const mjModel* m, mjData* d, mjtNum error[7])
|
||||
{
|
||||
int nv = m->nv;
|
||||
|
||||
// allocate space
|
||||
mjMARKSTACK
|
||||
mjtNum* mat = mj_stackAlloc(d, nv*nv);
|
||||
|
||||
// get pointers to derivative matrices
|
||||
mjtNum* G0 = deriv; // dinv/dpos
|
||||
mjtNum* G1 = deriv + nv*nv; // dinv/dvel
|
||||
mjtNum* G2 = deriv + 2*nv*nv; // dinv/dacc
|
||||
mjtNum* F0 = deriv + 3*nv*nv; // dacc/dpos
|
||||
mjtNum* F1 = deriv + 4*nv*nv; // dacc/dvel
|
||||
mjtNum* F2 = deriv + 5*nv*nv; // dacc/dfrc
|
||||
|
||||
// G2*F2 - I
|
||||
mju_mulMatMat(mat, G2, F2, nv, nv, nv);
|
||||
for( int i=0; i<nv; i++ )
|
||||
mat[i*(nv+1)] -= 1;
|
||||
error[0] = relnorm(mat, G2, nv*nv);
|
||||
|
||||
// G2 - G2'
|
||||
mju_transpose(mat, G2, nv, nv);
|
||||
mju_sub(mat, mat, G2, nv*nv);
|
||||
error[1] = relnorm(mat, G2, nv*nv);
|
||||
|
||||
// G1 - G1'
|
||||
mju_transpose(mat, G1, nv, nv);
|
||||
mju_sub(mat, mat, G1, nv*nv);
|
||||
error[2] = relnorm(mat, G1, nv*nv);
|
||||
|
||||
// F2 - F2'
|
||||
mju_transpose(mat, F2, nv, nv);
|
||||
mju_sub(mat, mat, F2, nv*nv);
|
||||
error[3] = relnorm(mat, F2, nv*nv);
|
||||
|
||||
// G1 + G2*F1
|
||||
mju_mulMatMat(mat, G2, F1, nv, nv, nv);
|
||||
mju_addTo(mat, G1, nv*nv);
|
||||
error[4] = relnorm(mat, G1, nv*nv);
|
||||
|
||||
// G0 + G2*F0
|
||||
mju_mulMatMat(mat, G2, F0, nv, nv, nv);
|
||||
mju_addTo(mat, G0, nv*nv);
|
||||
error[5] = relnorm(mat, G0, nv*nv);
|
||||
|
||||
// F1 + F2*G1
|
||||
mju_mulMatMat(mat, F2, G1, nv, nv, nv);
|
||||
mju_addTo(mat, F1, nv*nv);
|
||||
error[6] = relnorm(mat, F1, nv*nv);
|
||||
|
||||
// F0 + F2*G0
|
||||
mju_mulMatMat(mat, F2, G0, nv, nv, nv);
|
||||
mju_addTo(mat, F0, nv*nv);
|
||||
error[7] = relnorm(mat, F0, nv*nv);
|
||||
|
||||
mjFREESTACK
|
||||
}
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// print help if not enough arguments
|
||||
if( argc<2 )
|
||||
{
|
||||
printf("\n Arguments: modelfile [nthread niter nwarmup nepoch nstep eps]\n\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// default nthread = number of logical cores (usually optimal)
|
||||
nthread = omp_get_num_procs();
|
||||
|
||||
// get numeric command-line arguments
|
||||
if( argc>2 )
|
||||
sscanf(argv[2], "%d", &nthread);
|
||||
if( argc>3 )
|
||||
sscanf(argv[3], "%d", &niter);
|
||||
if( argc>4 )
|
||||
sscanf(argv[4], "%d", &nwarmup);
|
||||
if( argc>5 )
|
||||
sscanf(argv[5], "%d", &nepoch);
|
||||
if( argc>6 )
|
||||
sscanf(argv[6], "%d", &nstep);
|
||||
if( argc>7 )
|
||||
sscanf(argv[7], "%lf", &eps);
|
||||
|
||||
// check number of threads
|
||||
if( nthread<1 || nthread>MAXTHREAD )
|
||||
{
|
||||
printf("nthread must be between 1 and %d\n", MAXTHREAD);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// check number of epochs
|
||||
if( nepoch<1 || nepoch>MAXEPOCH )
|
||||
{
|
||||
printf("nepoch must be between 1 and %d\n", MAXEPOCH);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// load model
|
||||
mjModel* m = 0;
|
||||
if( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )
|
||||
m = mj_loadModel(argv[1], NULL);
|
||||
else
|
||||
m = mj_loadXML(argv[1], NULL, NULL, 0);
|
||||
if( !m )
|
||||
{
|
||||
printf("Could not load modelfile '%s'\n", argv[1]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// print arguments
|
||||
#if defined(_OPENMP)
|
||||
printf("\nnthread : %d (OpenMP)\n", nthread);
|
||||
#else
|
||||
printf("\nnthread : %d (serial)\n", nthread);
|
||||
#endif
|
||||
printf("niter : %d\n", niter);
|
||||
printf("nwarmup : %d\n", nwarmup);
|
||||
printf("nepoch : %d\n", nepoch);
|
||||
printf("nstep : %d\n", nstep);
|
||||
printf("eps : %g\n\n", eps);
|
||||
|
||||
// make mjData: main, per-thread
|
||||
mjData* dmain = mj_makeData(m);
|
||||
mjData* d[MAXTHREAD];
|
||||
for( int n=0; n<nthread; n++ )
|
||||
d[n] = mj_makeData(m);
|
||||
|
||||
// allocate derivatives
|
||||
deriv = (mjtNum*) mju_malloc(6*sizeof(mjtNum)*m->nv*m->nv);
|
||||
|
||||
// set up OpenMP (if not enabled, this does nothing)
|
||||
omp_set_dynamic(0);
|
||||
omp_set_num_threads(nthread);
|
||||
|
||||
// save solver options
|
||||
int save_iterations = m->opt.iterations;
|
||||
mjtNum save_tolerance = m->opt.tolerance;
|
||||
|
||||
// allocate statistics
|
||||
int nefc = 0;
|
||||
double cputm[MAXEPOCH][2];
|
||||
mjtNum error[MAXEPOCH][8];
|
||||
|
||||
// run epochs, collect statistics
|
||||
for( int epoch=0; epoch<nepoch; epoch++ )
|
||||
{
|
||||
// set solver options for main simulation
|
||||
m->opt.iterations = save_iterations;
|
||||
m->opt.tolerance = save_tolerance;
|
||||
|
||||
// advance main simulation for nstep
|
||||
for( int i=0; i<nstep; i++ )
|
||||
mj_step(m, dmain);
|
||||
|
||||
// count number of active constraints
|
||||
nefc += dmain->nefc;
|
||||
|
||||
// set solver options for finite differences
|
||||
m->opt.iterations = niter;
|
||||
m->opt.tolerance = 0;
|
||||
|
||||
// test forward and inverse
|
||||
for( isforward=0; isforward<2; isforward++ )
|
||||
{
|
||||
// start timer
|
||||
double starttm = omp_get_wtime();
|
||||
|
||||
// run worker threads in parallel if OpenMP is enabled
|
||||
#pragma omp parallel for schedule(static)
|
||||
for( int n=0; n<nthread; n++ )
|
||||
worker(m, dmain, d[n], n);
|
||||
|
||||
// record duration in ms
|
||||
cputm[epoch][isforward] = 1000*(omp_get_wtime() - starttm);
|
||||
}
|
||||
|
||||
// check derivatives
|
||||
checkderiv(m, d[0], error[epoch]);
|
||||
}
|
||||
|
||||
// compute statistics
|
||||
double mcputm[2] = {0,0}, merror[8] = {0,0,0,0,0,0,0,0};
|
||||
for( int epoch=0; epoch<nepoch; epoch++ )
|
||||
{
|
||||
mcputm[0] += cputm[epoch][0];
|
||||
mcputm[1] += cputm[epoch][1];
|
||||
|
||||
for( int ie=0; ie<8; ie++ )
|
||||
merror[ie] += error[epoch][ie];
|
||||
}
|
||||
|
||||
// print sizes, timing, accuracy
|
||||
printf("sizes : nv %d, nefc %d\n\n", m->nv, nefc/nepoch);
|
||||
printf("inverse : %.2f ms\n", mcputm[0]/nepoch);
|
||||
printf("forward : %.2f ms\n\n", mcputm[1]/nepoch);
|
||||
printf("accuracy: log10(residual L1 relnorm)\n");
|
||||
printf("------------------------------------\n");
|
||||
for( int ie=0; ie<8; ie++ )
|
||||
printf(" %s : %.2g\n", accuracy[ie], merror[ie]/nepoch);
|
||||
printf("\n");
|
||||
|
||||
// shut down
|
||||
mju_free(deriv);
|
||||
mj_deleteData(dmain);
|
||||
for( int n=0; n<nthread; n++ )
|
||||
mj_deleteData(d[n]);
|
||||
mj_deleteModel(m);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,311 @@
|
||||
// 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 "mujoco.h"
|
||||
#include "stdio.h"
|
||||
#include "stdlib.h"
|
||||
#include "string.h"
|
||||
|
||||
|
||||
// select EGL, OSMESA or GLFW
|
||||
#if defined(MJ_EGL)
|
||||
#include <EGL/egl.h>
|
||||
#elif defined(MJ_OSMESA)
|
||||
#include <GL/osmesa.h>
|
||||
OSMesaContext ctx;
|
||||
unsigned char buffer[10000000];
|
||||
#else
|
||||
#include "glfw3.h"
|
||||
#endif
|
||||
|
||||
|
||||
//-------------------------------- global data ------------------------------------------
|
||||
|
||||
// MuJoCo model and data
|
||||
mjModel* m = 0;
|
||||
mjData* d = 0;
|
||||
|
||||
// MuJoCo visualization
|
||||
mjvScene scn;
|
||||
mjvCamera cam;
|
||||
mjvOption opt;
|
||||
mjrContext con;
|
||||
|
||||
|
||||
//-------------------------------- utility functions ------------------------------------
|
||||
|
||||
// load model, init simulation and rendering
|
||||
void initMuJoCo(const char* filename)
|
||||
{
|
||||
// load and compile
|
||||
char error[1000] = "Could not load binary model";
|
||||
if( strlen(filename)>4 && !strcmp(filename+strlen(filename)-4, ".mjb") )
|
||||
m = mj_loadModel(filename, 0);
|
||||
else
|
||||
m = mj_loadXML(filename, 0, error, 1000);
|
||||
if( !m )
|
||||
mju_error_s("Load model error: %s", error);
|
||||
|
||||
// make data, run one computation to initialize all fields
|
||||
d = mj_makeData(m);
|
||||
mj_forward(m, d);
|
||||
|
||||
// initialize visualization data structures
|
||||
mjv_defaultCamera(&cam);
|
||||
mjv_defaultOption(&opt);
|
||||
mjv_defaultScene(&scn);
|
||||
mjr_defaultContext(&con);
|
||||
|
||||
// create scene and context
|
||||
mjv_makeScene(m, &scn, 2000);
|
||||
mjr_makeContext(m, &con, 200);
|
||||
|
||||
// center and scale view
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
// deallocate everything
|
||||
void closeMuJoCo(void)
|
||||
{
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
mjr_freeContext(&con);
|
||||
mjv_freeScene(&scn);
|
||||
}
|
||||
|
||||
|
||||
// create OpenGL context/window
|
||||
void initOpenGL(void)
|
||||
{
|
||||
//------------------------ EGL
|
||||
#if defined(MJ_EGL)
|
||||
// desired config
|
||||
const EGLint configAttribs[] ={
|
||||
EGL_RED_SIZE, 8,
|
||||
EGL_GREEN_SIZE, 8,
|
||||
EGL_BLUE_SIZE, 8,
|
||||
EGL_ALPHA_SIZE, 8,
|
||||
EGL_DEPTH_SIZE, 24,
|
||||
EGL_STENCIL_SIZE, 8,
|
||||
EGL_COLOR_BUFFER_TYPE, EGL_RGB_BUFFER,
|
||||
EGL_SURFACE_TYPE, EGL_PBUFFER_BIT,
|
||||
EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT,
|
||||
EGL_NONE
|
||||
};
|
||||
|
||||
// get default display
|
||||
EGLDisplay eglDpy = eglGetDisplay(EGL_DEFAULT_DISPLAY);
|
||||
if( eglDpy==EGL_NO_DISPLAY )
|
||||
mju_error_i("Could not get EGL display, error 0x%x\n", eglGetError());
|
||||
|
||||
// initialize
|
||||
EGLint major, minor;
|
||||
if( eglInitialize(eglDpy, &major, &minor)!=EGL_TRUE )
|
||||
mju_error_i("Could not initialize EGL, error 0x%x\n", eglGetError());
|
||||
|
||||
// choose config
|
||||
EGLint numConfigs;
|
||||
EGLConfig eglCfg;
|
||||
if( eglChooseConfig(eglDpy, configAttribs, &eglCfg, 1, &numConfigs)!=EGL_TRUE )
|
||||
mju_error_i("Could not choose EGL config, error 0x%x\n", eglGetError());
|
||||
|
||||
// bind OpenGL API
|
||||
if( eglBindAPI(EGL_OPENGL_API)!=EGL_TRUE )
|
||||
mju_error_i("Could not bind EGL OpenGL API, error 0x%x\n", eglGetError());
|
||||
|
||||
// create context
|
||||
EGLContext eglCtx = eglCreateContext(eglDpy, eglCfg, EGL_NO_CONTEXT, NULL);
|
||||
if( eglCtx==EGL_NO_CONTEXT )
|
||||
mju_error_i("Could not create EGL context, error 0x%x\n", eglGetError());
|
||||
|
||||
// make context current, no surface (let OpenGL handle FBO)
|
||||
if( eglMakeCurrent(eglDpy, EGL_NO_SURFACE, EGL_NO_SURFACE, eglCtx)!=EGL_TRUE )
|
||||
mju_error_i("Could not make EGL context current, error 0x%x\n", eglGetError());
|
||||
|
||||
//------------------------ OSMESA
|
||||
#elif defined(MJ_OSMESA)
|
||||
// create context
|
||||
ctx = OSMesaCreateContextExt(GL_RGBA, 24, 8, 8, 0);
|
||||
if( !ctx )
|
||||
mju_error("OSMesa context creation failed");
|
||||
|
||||
// make current
|
||||
if( !OSMesaMakeCurrent(ctx, buffer, GL_UNSIGNED_BYTE, 800, 800) )
|
||||
mju_error("OSMesa make current failed");
|
||||
|
||||
//------------------------ GLFW
|
||||
#else
|
||||
// init GLFW
|
||||
if( !glfwInit() )
|
||||
mju_error("Could not initialize GLFW");
|
||||
|
||||
// create invisible window, single-buffered
|
||||
glfwWindowHint(GLFW_VISIBLE, 0);
|
||||
glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_FALSE);
|
||||
GLFWwindow* window = glfwCreateWindow(800, 800, "Invisible window", NULL, NULL);
|
||||
if( !window )
|
||||
mju_error("Could not create GLFW window");
|
||||
|
||||
// make context current
|
||||
glfwMakeContextCurrent(window);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// close OpenGL context/window
|
||||
void closeOpenGL(void)
|
||||
{
|
||||
//------------------------ EGL
|
||||
#if defined(MJ_EGL)
|
||||
// get current display
|
||||
EGLDisplay eglDpy = eglGetCurrentDisplay();
|
||||
if( eglDpy==EGL_NO_DISPLAY )
|
||||
return;
|
||||
|
||||
// get current context
|
||||
EGLContext eglCtx = eglGetCurrentContext();
|
||||
|
||||
// release context
|
||||
eglMakeCurrent(eglDpy, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT);
|
||||
|
||||
// destroy context if valid
|
||||
if( eglCtx!=EGL_NO_CONTEXT )
|
||||
eglDestroyContext(eglDpy, eglCtx);
|
||||
|
||||
// terminate display
|
||||
eglTerminate(eglDpy);
|
||||
|
||||
//------------------------ OSMESA
|
||||
#elif defined(MJ_OSMESA)
|
||||
OSMesaDestroyContext(ctx);
|
||||
|
||||
//------------------------ GLFW
|
||||
#else
|
||||
// terminate GLFW (crashes with Linux NVidia drivers)
|
||||
#if defined(__APPLE__) || defined(_WIN32)
|
||||
glfwTerminate();
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//-------------------------------- main function ----------------------------------------
|
||||
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
// check command-line arguments
|
||||
if( argc!=5 )
|
||||
{
|
||||
printf(" USAGE: record modelfile duration fps rgbfile\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// parse numeric arguments
|
||||
double duration = 10, fps = 30;
|
||||
sscanf(argv[2], "%lf", &duration);
|
||||
sscanf(argv[3], "%lf", &fps);
|
||||
|
||||
// initialize OpenGL and MuJoCo
|
||||
initOpenGL();
|
||||
initMuJoCo(argv[1]);
|
||||
|
||||
// set rendering to offscreen buffer
|
||||
mjr_setBuffer(mjFB_OFFSCREEN, &con);
|
||||
if( con.currentBuffer!=mjFB_OFFSCREEN )
|
||||
printf("Warning: offscreen rendering not supported, using default/window framebuffer\n");
|
||||
|
||||
// get size of active renderbuffer
|
||||
mjrRect viewport = mjr_maxViewport(&con);
|
||||
int W = viewport.width;
|
||||
int H = viewport.height;
|
||||
|
||||
// allocate rgb and depth buffers
|
||||
unsigned char* rgb = (unsigned char*)malloc(3*W*H);
|
||||
float* depth = (float*)malloc(sizeof(float)*W*H);
|
||||
if( !rgb || !depth )
|
||||
mju_error("Could not allocate buffers");
|
||||
|
||||
// create output rgb file
|
||||
FILE* fp = fopen(argv[4], "wb");
|
||||
if( !fp )
|
||||
mju_error("Could not open rgbfile for writing");
|
||||
|
||||
// main loop
|
||||
double frametime = 0;
|
||||
int framecount = 0;
|
||||
while( d->time<duration )
|
||||
{
|
||||
// render new frame if it is time (or first frame)
|
||||
if( (d->time-frametime)>1/fps || frametime==0 )
|
||||
{
|
||||
// update abstract scene
|
||||
mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
|
||||
|
||||
// render scene in offscreen buffer
|
||||
mjr_render(viewport, &scn, &con);
|
||||
|
||||
// add time stamp in upper-left corner
|
||||
char stamp[50];
|
||||
sprintf(stamp, "Time = %.3f", d->time);
|
||||
mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, viewport, stamp, NULL, &con);
|
||||
|
||||
// read rgb and depth buffers
|
||||
mjr_readPixels(rgb, depth, viewport, &con);
|
||||
|
||||
// insert subsampled depth image in lower-left corner of rgb image
|
||||
const int NS = 3; // depth image sub-sampling
|
||||
for( int r=0; r<H; r+=NS )
|
||||
for( int c=0; c<W; c+=NS )
|
||||
{
|
||||
int adr = (r/NS)*W + c/NS;
|
||||
rgb[3*adr] = rgb[3*adr+1] = rgb[3*adr+2] =
|
||||
(unsigned char)((1.0f-depth[r*W+c])*255.0f);
|
||||
}
|
||||
|
||||
// write rgb image to file
|
||||
fwrite(rgb, 3, W*H, fp);
|
||||
|
||||
// print every 10 frames: '.' if ok, 'x' if OpenGL error
|
||||
if( ((framecount++)%10)==0 )
|
||||
{
|
||||
if( mjr_getError() )
|
||||
printf("x");
|
||||
else
|
||||
printf(".");
|
||||
}
|
||||
|
||||
// save simulation time
|
||||
frametime = d->time;
|
||||
}
|
||||
|
||||
// advance simulation
|
||||
mj_step(m, d);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
// close file, free buffers
|
||||
fclose(fp);
|
||||
free(rgb);
|
||||
free(depth);
|
||||
|
||||
// close MuJoCo and OpenGL
|
||||
closeMuJoCo();
|
||||
closeOpenGL();
|
||||
|
||||
return 1;
|
||||
}
|
||||
+2066
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,195 @@
|
||||
// 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 "mujoco.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
// model and per-thread data
|
||||
mjModel* m = NULL;
|
||||
mjData* d[64];
|
||||
|
||||
|
||||
// per-thread statistics
|
||||
int contacts[64];
|
||||
int constraints[64];
|
||||
double simtime[64];
|
||||
|
||||
|
||||
// timer
|
||||
chrono::system_clock::time_point tm_start;
|
||||
mjtNum gettm(void)
|
||||
{
|
||||
chrono::duration<double> elapsed = chrono::system_clock::now() - tm_start;
|
||||
return elapsed.count();
|
||||
}
|
||||
|
||||
|
||||
// deallocate and print message
|
||||
int finish(const char* msg = NULL, mjModel* m = NULL)
|
||||
{
|
||||
// deallocate model
|
||||
if( m )
|
||||
mj_deleteModel(m);
|
||||
|
||||
// print message
|
||||
if( msg )
|
||||
printf("%s\n", msg);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// thread function
|
||||
void simulate(int id, int nstep)
|
||||
{
|
||||
// clear statistics
|
||||
contacts[id] = 0;
|
||||
constraints[id] = 0;
|
||||
|
||||
// run and time
|
||||
double start = gettm();
|
||||
for( int i=0; i<nstep; i++ )
|
||||
{
|
||||
// advance simulation
|
||||
mj_step(m, d[id]);
|
||||
|
||||
// accumulate statistics
|
||||
contacts[id] += d[id]->ncon;
|
||||
constraints[id] += d[id]->nefc;
|
||||
}
|
||||
simtime[id] = gettm() - start;
|
||||
}
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
// print help if arguments are missing
|
||||
if( argc<3 || argc>5 )
|
||||
return finish("\n Usage: testspeed modelfile nstep [nthread [profile]]\n");
|
||||
|
||||
// read nstep and nthread
|
||||
int nstep = 0, nthread = 0, profile = 0;
|
||||
if( sscanf(argv[2], "%d", &nstep)!=1 || nstep<=0 )
|
||||
return finish("Invalid nstep argument");
|
||||
if( argc>3 )
|
||||
if( sscanf(argv[3], "%d", &nthread)!=1 )
|
||||
return finish("Invalid nthread argument");
|
||||
if( argc>4 )
|
||||
if( sscanf(argv[4], "%d", &profile)!=1 )
|
||||
return finish("Invalid profile argument");
|
||||
|
||||
// clamp nthread to [1, 64]
|
||||
nthread = mjMAX(1, mjMIN(64, nthread));
|
||||
|
||||
// get filename, determine file type
|
||||
std::string filename(argv[1]);
|
||||
bool binary = (filename.find(".mjb")!=std::string::npos);
|
||||
|
||||
// load model
|
||||
char error[1000] = "Could not load binary model";
|
||||
if( binary )
|
||||
m = mj_loadModel(argv[1], 0);
|
||||
else
|
||||
m = mj_loadXML(argv[1], 0, error, 1000);
|
||||
if( !m )
|
||||
return finish(error);
|
||||
|
||||
// make per-thread data
|
||||
int testkey = mj_name2id(m, mjOBJ_KEY, "test");
|
||||
for( int id=0; id<nthread; id++ )
|
||||
{
|
||||
d[id] = mj_makeData(m);
|
||||
if( !d[id] )
|
||||
return finish("Could not allocate mjData", m);
|
||||
|
||||
// init to keyframe "test" if present
|
||||
if( testkey>=0 )
|
||||
{
|
||||
mju_copy(d[id]->qpos, m->key_qpos + testkey*m->nq, m->nq);
|
||||
mju_copy(d[id]->qvel, m->key_qvel + testkey*m->nv, m->nv);
|
||||
mju_copy(d[id]->act, m->key_act + testkey*m->na, m->na);
|
||||
}
|
||||
}
|
||||
|
||||
// install timer callback for profiling if requested
|
||||
tm_start = chrono::system_clock::now();
|
||||
if( profile )
|
||||
mjcb_time = gettm;
|
||||
|
||||
// print start
|
||||
if( nthread>1 )
|
||||
printf("\nRunning %d steps per thread at dt = %g ...\n\n", nstep, m->opt.timestep);
|
||||
else
|
||||
printf("\nRunning %d steps at dt = %g ...\n\n", nstep, m->opt.timestep);
|
||||
|
||||
// run simulation, record total time
|
||||
thread th[64];
|
||||
double starttime = gettm();
|
||||
for( int id=0; id<nthread; id++ )
|
||||
th[id] = thread(simulate, id, nstep);
|
||||
for( int id=0; id<nthread; id++ )
|
||||
th[id].join();
|
||||
double tottime = gettm() - starttime;
|
||||
|
||||
// all-thread summary
|
||||
if( nthread>1 )
|
||||
{
|
||||
printf("Summary for all %d threads\n\n", nthread);
|
||||
printf(" Total simulation time : %.2f s\n", tottime);
|
||||
printf(" Total steps per second : %.0f\n", nthread*nstep/tottime);
|
||||
printf(" Total realtime factor : %.2f x\n", nthread*nstep*m->opt.timestep/tottime);
|
||||
printf(" Total time per step : %.4f ms\n\n", 1000*tottime/(nthread*nstep));
|
||||
|
||||
printf("Details for thread 0\n\n");
|
||||
}
|
||||
|
||||
// details for thread 0
|
||||
printf(" Simulation time : %.2f s\n", simtime[0]);
|
||||
printf(" Steps per second : %.0f\n", nstep/simtime[0]);
|
||||
printf(" Realtime factor : %.2f x\n", nstep*m->opt.timestep/simtime[0]);
|
||||
printf(" Time per step : %.4f ms\n\n", 1000*simtime[0]/nstep);
|
||||
printf(" Contacts per step : %d\n", contacts[0]/nstep);
|
||||
printf(" Constraints per step : %d\n", constraints[0]/nstep);
|
||||
printf(" Degrees of freedom : %d\n\n", m->nv);
|
||||
|
||||
// profiler results for thread 0
|
||||
if( profile )
|
||||
{
|
||||
printf(" Profiler phase (ms per step)\n");
|
||||
mjtNum tstep = d[0]->timer[mjTIMER_STEP].duration/d[0]->timer[mjTIMER_STEP].number;
|
||||
for( int i=0; i<mjNTIMER; i++ )
|
||||
if( d[0]->timer[i].number>0 )
|
||||
{
|
||||
mjtNum istep = d[0]->timer[i].duration/d[0]->timer[i].number;
|
||||
printf(" %16s : %.5f (%6.2f %%)\n", mjTIMERSTRING[i],
|
||||
1000*istep, 100*istep/tstep);
|
||||
}
|
||||
}
|
||||
|
||||
// free per-thread data
|
||||
for( int id=0; id<nthread; id++ )
|
||||
mj_deleteData(d[id]);
|
||||
|
||||
// finalize
|
||||
return finish();
|
||||
}
|
||||
@@ -0,0 +1,174 @@
|
||||
// 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 "mujoco.h"
|
||||
#include "mjxmacro.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
// help
|
||||
const char helpstring[] = "\n Usage: testxml modelfile.xml\n";
|
||||
|
||||
|
||||
// deallocate and print message
|
||||
int finish(const char* msg = 0, mjModel* m = 0, mjData* d = 0)
|
||||
{
|
||||
// deallocated everything
|
||||
if( d )
|
||||
mj_deleteData(d);
|
||||
if( m )
|
||||
mj_deleteModel(m);
|
||||
|
||||
// print message
|
||||
if( msg )
|
||||
printf("%s\n", msg);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// return absolute difference if it is below 1, relative difference otherwise
|
||||
static mjtNum _compare(mjtNum val1, mjtNum val2)
|
||||
{
|
||||
mjtNum magnitude = mju_max(mju_abs(val1), mju_abs(val2));
|
||||
|
||||
if( magnitude>1.0 )
|
||||
return mju_abs(val1-val2) / magnitude;
|
||||
else
|
||||
return mju_abs(val1-val2);
|
||||
}
|
||||
|
||||
|
||||
// compare two models, return largest difference and field name
|
||||
mjtNum compareModel(const mjModel* m1, const mjModel* m2, char* field)
|
||||
{
|
||||
int r, c;
|
||||
mjtNum dif, maxdif = 0.0;
|
||||
|
||||
// define symbols corresponding to number of columns (needed in MJMODEL_POINTERS)
|
||||
int nq = m1->nq;
|
||||
int nv = m1->nv;
|
||||
int na = m1->na;
|
||||
int nmocap3 = 3*m1->nmocap;
|
||||
int nmocap4 = 4*m1->nmocap;
|
||||
int nuser_body = m1->nuser_body;
|
||||
int nuser_jnt = m1->nuser_jnt;
|
||||
int nuser_geom = m1->nuser_geom;
|
||||
int nuser_site = m1->nuser_site;
|
||||
int nuser_cam = m1->nuser_cam;
|
||||
int nuser_tendon = m1->nuser_tendon;
|
||||
int nuser_actuator = m1->nuser_actuator;
|
||||
int nuser_sensor = m1->nuser_sensor;
|
||||
|
||||
// compare ints
|
||||
#define X(name) if(m1->name!=m2->name) {strcpy(field, #name); return 1.0;}
|
||||
|
||||
MJMODEL_INTS
|
||||
#undef X
|
||||
|
||||
// compare arrays
|
||||
#define X(type, name, nr, nc) \
|
||||
for( r=0; r<m1->nr; r++ ) \
|
||||
for( c=0; c<nc; c++ ) { \
|
||||
dif = _compare(m1->name[r*nc+c], m2->name[r*nc+c]); \
|
||||
if(dif>maxdif) {maxdif=dif; strcpy(field, #name);} }
|
||||
|
||||
MJMODEL_POINTERS
|
||||
#undef X
|
||||
|
||||
// compare scalars in mjOption
|
||||
#define X(type, name) \
|
||||
dif = _compare(m1->opt.name, m2->opt.name); \
|
||||
if(dif>maxdif) {maxdif=dif; strcpy(field, #name);}
|
||||
|
||||
MJOPTION_SCALARS
|
||||
#undef X
|
||||
|
||||
// compare arrays in mjOption
|
||||
#define X(name, n) \
|
||||
for( c=0; c<n; c++ ) { \
|
||||
dif = _compare(m1->opt.name[c], m2->opt.name[c]); \
|
||||
if(dif>maxdif) {maxdif=dif; strcpy(field, #name);} }
|
||||
|
||||
MJOPTION_VECTORS
|
||||
#undef X
|
||||
|
||||
// mjVisual and mjStatistics ignored for now
|
||||
|
||||
return maxdif;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
// print help if arguments are missing
|
||||
if( argc<2 )
|
||||
return finish(helpstring);
|
||||
|
||||
// get filename, check file type
|
||||
std::string filename(argv[1]);
|
||||
if( filename.find(".xml")==std::string::npos )
|
||||
return finish("xml model file is required");
|
||||
|
||||
// load model
|
||||
char error[1000];
|
||||
mjModel* m = mj_loadXML(argv[1], 0, error, 1000);
|
||||
if( !m )
|
||||
return finish(error);
|
||||
|
||||
// make data
|
||||
mjData* d = mj_makeData(m);
|
||||
if( !d )
|
||||
return finish("Could not allocate mjData", m);
|
||||
|
||||
// prepare temp filename in the same directory as original (for asset loading)
|
||||
std::string tempfile;
|
||||
size_t lastpath = filename.find_last_of("/\\");
|
||||
if( lastpath==std::string::npos )
|
||||
tempfile = "_tempfile_.xml";
|
||||
else
|
||||
tempfile = filename.substr(0, lastpath+1) + "_tempfile_.xml";
|
||||
|
||||
// save
|
||||
if( !mj_saveLastXML(tempfile.c_str(), m, error, 1000) )
|
||||
return finish(error, m, d);
|
||||
|
||||
// load back
|
||||
mjModel* mtemp = mj_loadXML(tempfile.c_str(), 0, error, 100);
|
||||
if( !mtemp )
|
||||
return finish(error, m, d);
|
||||
|
||||
// compare
|
||||
char field[500] = "";
|
||||
mjtNum result = compareModel(m, mtemp, field);
|
||||
printf("\nComparison of original and saved model\n");
|
||||
printf(" Max difference : %.3g\n", result);
|
||||
printf(" Field name : %s\n", field);
|
||||
|
||||
// delete temp model and file
|
||||
mj_deleteModel(mtemp);
|
||||
remove(tempfile.c_str());
|
||||
|
||||
// finalize
|
||||
return finish();
|
||||
}
|
||||
Reference in New Issue
Block a user