Minimal MuJoCo C++ example

CMakeLists.txt

cmake_minimum_required(VERSION 3.26)
project(mujco_test_app)

set(CMAKE_CXX_STANDARD 17)

set (MUJOCO_INSTALL_PATH /home/michalpelka/physics/mujoco_install/lib/cmake/)
find_package(mujoco REQUIRED PATHS ${MUJOCO_INSTALL_PATH} NO_DEFAULT_PATH)

find_package(glfw3 REQUIRED)

add_executable(mujco_test_app main.cpp)
target_link_libraries(mujco_test_app PUBLIC mujoco::mujoco)

add_executable(gripper_test gripper_test.cpp)
target_link_libraries(gripper_test PUBLIC mujoco::mujoco glfw)

gripper_test.cpp

// 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 <cstdio>
#include <cstring>

#include <GLFW/glfw3.h>
#include <mujoco/mujoco.h>
#include <iostream>
#include <functional>
#include <map>
#include <assert.h>
#include <fstream>

// 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);
}

std::array<double,3> GetBodyPosition(mjData *d, mjModel *m, int bodyId )
{
    assert(m);
    assert(d);
    assert (bodyId < m->nbody);
    double * bodyPtr = d->xpos+ 3*bodyId;
    return {bodyPtr[0],bodyPtr[1],bodyPtr[2]};
}
// main function
int main(int argc, const char** argv) {
    // check command-line arguments
    if (argc!=2) {
        std::printf(" USAGE:  basic modelfile\n");
        return 0;
    }

    // load and compile model
    char error[1000] = "Could not load binary model";
    if (std::strlen(argv[1])>4 && !std::strcmp(argv[1]+std::strlen(argv[1])-4, ".mjb")) {
        m = mj_loadModel(argv[1], 0);
    } else {
        m = mj_loadXML(argv[1], 0, error, 1000);
    }
    if (!m) {
        mju_error("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);

    // set keyframe
    std::cout << "Number of keyframes " <<  m->nkey << std::endl;
    std::ofstream logForces("forces.csv");
    std::ofstream boxPos("boxPos.csv");

    std::map<int, std::function<void()>> commands;
    commands[1] = [&](){
        mj_resetDataKeyframe(m,d,2);
    };

    boxPos << "frame,box_x,box_y,box_z"<<std::endl;
    commands[20] = [&](){
       int actuator_id = mj_name2id(m, mjOBJ_ACTUATOR, "actuator8");
       assert(actuator_id>=0);
       d->ctrl[actuator_id] = 0; //Newton force
    };
    commands[50] = [&](){
        int actuator_id = mj_name2id(m, mjOBJ_ACTUATOR, "actuator2");
        assert(actuator_id>=0);
        d->ctrl[actuator_id] =d->ctrl[actuator_id] - 0.25;
    };

    commands[400] = [&](){
        logForces.close();
        std::abort();
    };



    // report force
    logForces<<"frame,";
    for (int i = 0; i < m->nu; ++i) {
        const char* name = mj_id2name(m, mjOBJ_ACTUATOR, i);
        logForces << name << ",";
    }
    logForces << std::endl;


    // run main loop, target real-time simulation and 60 fps rendering
    int frameNo = 0;

    while (!glfwWindowShouldClose(window)) {

        frameNo++;
        if(auto it = commands.find(frameNo); it!=commands.end())
        {
            (it->second)();
        }
        logForces<<frameNo<<",";
        for (int i=0; i < m->nu; i++)
        {
            logForces <<  d->actuator_force[i] << ",";
        }
        logForces<<std::endl;


        int box_id = mj_name2id(m, mjOBJ_BODY, "box");

        auto pos = GetBodyPosition(d,m,box_id);
        boxPos <<frameNo<<"," << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;

        // 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;
}

main.cpp

#include <cstdio>
#include <cstring>
#include <assert.h>
#include <GLFW/glfw3.h>
#include <mujoco/mujoco.h>
#include <iostream>
#include <array>
// MuJoCo data structures
mjSpec* spec = NULL;
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;

const char* dropped_ball_xml = R"(
<mujoco model="dropped_ball">
  <option gravity="0 0 -9.81"/>
  <worldbody>
    <!-- Ground plane -->
    <geom name="floor" type="plane" pos="0 0 0" size="5 5 0.1" rgba="0.2 0.3 0.4 1"/>
    <!-- Dropped ball -->
    <body name="ball" pos="0 0 2">
      <joint type="free"/>
      <geom type="sphere" size="0.1" rgba="1 0 0 1"/>
    </body>
  </worldbody>
</mujoco>
)";

// main function

std::array<double,3> GetBodyPosition(mjData *d, mjModel *m, int bodyId )
{
    assert(m);
    assert(d);
    assert (bodyId < m->nbody);
    double * bodyPtr = d->xpos+ 3*bodyId;
    return {bodyPtr[0],bodyPtr[1],bodyPtr[2]};
}

int main(int argc, const char** argv) {



    constexpr int errorSz = 1024;
    char error[errorSz] = "";

    spec = mj_parseXMLString(dropped_ball_xml, 0, error, errorSz);

    if (!spec )
    {
        std::cerr << error;
    }

    assert(spec);

    m = mj_compile(spec, 0);

    assert(m);

    mjData* d = mj_makeData(m);

    // Simulate for 1 second (1000 steps at 1000 Hz)
    for (int i = 0; i < 1000; i++) {
        mj_step(m, d);

        auto ballPos = GetBodyPosition(d,m,1);
        std::cout  << d->time << " " << ballPos[0] << " "<< ballPos[1] << " "<< ballPos[2] << std::endl;
    }

    // make data
    d = mj_makeData(m);


    // free MuJoCo model and data
    mj_deleteData(d);
    mj_deleteModel(m);
    mj_deleteSpec(spec);

    return 0;
}