511 lines
14 KiB
C++
511 lines
14 KiB
C++
#pragma once
|
||
|
||
#include "simulate.h"
|
||
|
||
#include <chrono>
|
||
#include <cstdint>
|
||
#include <cstdio>
|
||
#include <cstdlib>
|
||
#include <cstring>
|
||
#include <iostream>
|
||
#include <memory>
|
||
#include <mutex>
|
||
#include <new>
|
||
#include <string>
|
||
#include <thread>
|
||
|
||
#include "lodepng.h"
|
||
#include <mujoco/mjdata.h>
|
||
#include <mujoco/mjui.h>
|
||
#include <mujoco/mjvisualize.h>
|
||
#include <mujoco/mjxmacro.h>
|
||
#include <mujoco/mujoco.h>
|
||
#include "platform_ui_adapter.h"
|
||
#include "array_safety.h"
|
||
|
||
#define MUJOCO_PLUGIN_DIR "mujoco_plugin"
|
||
|
||
extern "C" {
|
||
#if defined(_WIN32) || defined(__CYGWIN__)
|
||
#include <windows.h>
|
||
#else
|
||
#if defined(__APPLE__)
|
||
#include <mach-o/dyld.h>
|
||
#include "glfw_corevideo.h"
|
||
#endif
|
||
#include <sys/errno.h>
|
||
#include <unistd.h>
|
||
#endif
|
||
}
|
||
|
||
#if defined(USE_ROS_COMM)
|
||
#include "QuadROSComm.hpp"
|
||
#elif defined(USE_DDS_COMM)
|
||
#include "QuadDDSComm.hpp"
|
||
#else
|
||
#include "SHM.hpp"
|
||
#endif
|
||
|
||
// holders of one step history of time and position to calculate dertivatives
|
||
mjtNum position_history = 0;
|
||
mjtNum previous_time = 0;
|
||
mjtNum timezero = 0;
|
||
|
||
// controller related variables
|
||
float_t ctrl_update_freq = 500;
|
||
mjtNum last_update = 0.0;
|
||
mjtNum ctrl;
|
||
|
||
QuadrupedSensorData sensor_data;
|
||
QuadrupedCommandData joint_command_data;
|
||
QuadrupedMeasurementData measurement_data;
|
||
// SHM comm_data(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR);
|
||
#if defined(USE_ROS_COMM)
|
||
std::shared_ptr<QuadROSComm> comm_data_ptr;
|
||
#elif defined(USE_DDS_COMM)
|
||
std::shared_ptr<QuadDDSComm> comm_data_ptr;
|
||
#else
|
||
std::shared_ptr<SHM> comm_data_ptr;
|
||
#endif
|
||
|
||
std::chrono::time_point<std::chrono::high_resolution_clock> m_startTimePoint;
|
||
double t_curr = 0;
|
||
|
||
double updateTimer() {
|
||
auto currTimePont = std::chrono::high_resolution_clock::now();
|
||
auto start = std::chrono::time_point_cast<std::chrono::microseconds>(m_startTimePoint).time_since_epoch().count();
|
||
auto curr = std::chrono::time_point_cast<std::chrono::microseconds>(currTimePont).time_since_epoch().count();
|
||
auto duration = curr - start;
|
||
|
||
return duration * double(1e-6);
|
||
}
|
||
|
||
void UpdateSensorData(mjData*);
|
||
void CustomController(const mjModel*, mjData*);
|
||
|
||
namespace {
|
||
namespace mj = ::mujoco;
|
||
namespace mju = ::mujoco::sample_util;
|
||
|
||
// constants
|
||
const double syncMisalign = 0.1; // maximum mis-alignment before re-sync (simulation seconds)
|
||
const double simRefreshFraction = 0.7; // fraction of refresh available for simulation
|
||
const int kErrorLength = 1024; // load error string length
|
||
|
||
// model and data
|
||
mjModel* m = nullptr;
|
||
mjData* d = nullptr;
|
||
|
||
// control noise variables
|
||
mjtNum* ctrlnoise = nullptr;
|
||
|
||
using Seconds = std::chrono::duration<double>;
|
||
|
||
|
||
//---------------------------------------- plugin handling -----------------------------------------
|
||
|
||
// return the path to the directory containing the current executable
|
||
// used to determine the location of auto-loaded plugin libraries
|
||
std::string getExecutableDir() {
|
||
#if defined(_WIN32) || defined(__CYGWIN__)
|
||
constexpr char kPathSep = '\\';
|
||
std::string realpath = [&]() -> std::string {
|
||
std::unique_ptr<char[]> realpath(nullptr);
|
||
DWORD buf_size = 128;
|
||
bool success = false;
|
||
while (!success) {
|
||
realpath.reset(new(std::nothrow) char[buf_size]);
|
||
if (!realpath) {
|
||
std::cerr << "cannot allocate memory to store executable path\n";
|
||
return "";
|
||
}
|
||
|
||
DWORD written = GetModuleFileNameA(nullptr, realpath.get(), buf_size);
|
||
if (written < buf_size) {
|
||
success = true;
|
||
} else if (written == buf_size) {
|
||
// realpath is too small, grow and retry
|
||
buf_size *=2;
|
||
} else {
|
||
std::cerr << "failed to retrieve executable path: " << GetLastError() << "\n";
|
||
return "";
|
||
}
|
||
}
|
||
return realpath.get();
|
||
}();
|
||
#else
|
||
constexpr char kPathSep = '/';
|
||
#if defined(__APPLE__)
|
||
std::unique_ptr<char[]> buf(nullptr);
|
||
{
|
||
std::uint32_t buf_size = 0;
|
||
_NSGetExecutablePath(nullptr, &buf_size);
|
||
buf.reset(new char[buf_size]);
|
||
if (!buf) {
|
||
std::cerr << "cannot allocate memory to store executable path\n";
|
||
return "";
|
||
}
|
||
if (_NSGetExecutablePath(buf.get(), &buf_size)) {
|
||
std::cerr << "unexpected error from _NSGetExecutablePath\n";
|
||
}
|
||
}
|
||
const char* path = buf.get();
|
||
#else
|
||
const char* path = "/proc/self/exe";
|
||
#endif
|
||
std::string realpath = [&]() -> std::string {
|
||
std::unique_ptr<char[]> realpath(nullptr);
|
||
std::uint32_t buf_size = 128;
|
||
bool success = false;
|
||
while (!success) {
|
||
realpath.reset(new(std::nothrow) char[buf_size]);
|
||
if (!realpath) {
|
||
std::cerr << "cannot allocate memory to store executable path\n";
|
||
return "";
|
||
}
|
||
|
||
std::size_t written = readlink(path, realpath.get(), buf_size);
|
||
if (written < buf_size) {
|
||
realpath.get()[written] = '\0';
|
||
success = true;
|
||
} else if (written == -1) {
|
||
if (errno == EINVAL) {
|
||
// path is already not a symlink, just use it
|
||
return path;
|
||
}
|
||
|
||
std::cerr << "error while resolving executable path: " << strerror(errno) << '\n';
|
||
return "";
|
||
} else {
|
||
// realpath is too small, grow and retry
|
||
buf_size *= 2;
|
||
}
|
||
}
|
||
return realpath.get();
|
||
}();
|
||
#endif
|
||
|
||
if (realpath.empty()) {
|
||
return "";
|
||
}
|
||
|
||
for (std::size_t i = realpath.size() - 1; i > 0; --i) {
|
||
if (realpath.c_str()[i] == kPathSep) {
|
||
return realpath.substr(0, i);
|
||
}
|
||
}
|
||
|
||
// don't scan through the entire file system's root
|
||
return "";
|
||
}
|
||
|
||
|
||
|
||
// scan for libraries in the plugin directory to load additional plugins
|
||
void scanPluginLibraries() {
|
||
// check and print plugins that are linked directly into the executable
|
||
int nplugin = mjp_pluginCount();
|
||
if (nplugin) {
|
||
std::printf("Built-in plugins:\n");
|
||
for (int i = 0; i < nplugin; ++i) {
|
||
std::printf(" %s\n", mjp_getPluginAtSlot(i)->name);
|
||
}
|
||
}
|
||
|
||
// define platform-specific strings
|
||
#if defined(_WIN32) || defined(__CYGWIN__)
|
||
const std::string sep = "\\";
|
||
#else
|
||
const std::string sep = "/";
|
||
#endif
|
||
|
||
|
||
// try to open the ${EXECDIR}/plugin directory
|
||
// ${EXECDIR} is the directory containing the simulate binary itself
|
||
const std::string executable_dir = getExecutableDir();
|
||
if (executable_dir.empty()) {
|
||
return;
|
||
}
|
||
|
||
const std::string plugin_dir = getExecutableDir() + sep + MUJOCO_PLUGIN_DIR;
|
||
mj_loadAllPluginLibraries(
|
||
plugin_dir.c_str(), +[](const char* filename, int first, int count) {
|
||
std::printf("Plugins registered by library '%s':\n", filename);
|
||
for (int i = first; i < first + count; ++i) {
|
||
std::printf(" %s\n", mjp_getPluginAtSlot(i)->name);
|
||
}
|
||
});
|
||
}
|
||
|
||
|
||
//------------------------------------------- simulation -------------------------------------------
|
||
|
||
|
||
mjModel* LoadModel(const char* file, mj::Simulate& sim) {
|
||
// this copy is needed so that the mju::strlen call below compiles
|
||
char filename[mj::Simulate::kMaxFilenameLength];
|
||
mju::strcpy_arr(filename, file);
|
||
|
||
// make sure filename is not empty
|
||
if (!filename[0]) {
|
||
return nullptr;
|
||
}
|
||
|
||
// load and compile
|
||
char loadError[kErrorLength] = "";
|
||
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, nullptr);
|
||
if (!mnew) {
|
||
mju::strcpy_arr(loadError, "could not load binary model");
|
||
}
|
||
} else {
|
||
mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength);
|
||
// remove trailing newline character from loadError
|
||
if (loadError[0]) {
|
||
int error_length = mju::strlen_arr(loadError);
|
||
if (loadError[error_length-1] == '\n') {
|
||
loadError[error_length-1] = '\0';
|
||
}
|
||
}
|
||
}
|
||
|
||
mju::strcpy_arr(sim.load_error, loadError);
|
||
|
||
if (!mnew) {
|
||
std::printf("%s\n", loadError);
|
||
return nullptr;
|
||
}
|
||
|
||
// compiler warning: print and pause
|
||
if (loadError[0]) {
|
||
// mj_forward() below will print the warning message
|
||
std::printf("Model compiled, but simulation warning (paused):\n %s\n", loadError);
|
||
sim.run = 0;
|
||
}
|
||
|
||
return mnew;
|
||
}
|
||
|
||
// simulate in background thread (while rendering in main thread)
|
||
void PhysicsLoop(mj::Simulate& sim) {
|
||
// cpu-sim syncronization point
|
||
std::chrono::time_point<mj::Simulate::Clock> syncCPU;
|
||
mjtNum syncSim = 0;
|
||
|
||
// run until asked to exit
|
||
while (!sim.exitrequest.load()) {
|
||
if (sim.droploadrequest.load()) {
|
||
sim.LoadMessage(sim.dropfilename);
|
||
mjModel* mnew = LoadModel(sim.dropfilename, sim);
|
||
sim.droploadrequest.store(false);
|
||
|
||
mjData* dnew = nullptr;
|
||
if (mnew) dnew = mj_makeData(mnew);
|
||
if (dnew) {
|
||
sim.Load(mnew, dnew, sim.dropfilename);
|
||
|
||
// lock the sim mutex
|
||
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
|
||
|
||
mj_deleteData(d);
|
||
mj_deleteModel(m);
|
||
|
||
m = mnew;
|
||
d = dnew;
|
||
mj_forward(m, d);
|
||
|
||
// allocate ctrlnoise
|
||
free(ctrlnoise);
|
||
ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
|
||
mju_zero(ctrlnoise, m->nu);
|
||
} else {
|
||
sim.LoadMessageClear();
|
||
}
|
||
}
|
||
|
||
if (sim.uiloadrequest.load()) {
|
||
sim.uiloadrequest.fetch_sub(1);
|
||
sim.LoadMessage(sim.filename);
|
||
mjModel* mnew = LoadModel(sim.filename, sim);
|
||
mjData* dnew = nullptr;
|
||
if (mnew) dnew = mj_makeData(mnew);
|
||
if (dnew) {
|
||
sim.Load(mnew, dnew, sim.filename);
|
||
|
||
// lock the sim mutex
|
||
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
|
||
|
||
mj_deleteData(d);
|
||
mj_deleteModel(m);
|
||
|
||
m = mnew;
|
||
d = dnew;
|
||
mj_forward(m, d);
|
||
|
||
// allocate ctrlnoise
|
||
free(ctrlnoise);
|
||
ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
|
||
mju_zero(ctrlnoise, m->nu);
|
||
} else {
|
||
sim.LoadMessageClear();
|
||
}
|
||
}
|
||
|
||
// 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 (sim.run && sim.busywait) {
|
||
std::this_thread::yield();
|
||
} else {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||
}
|
||
|
||
{
|
||
// lock the sim mutex
|
||
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
|
||
|
||
// run only if model is present
|
||
if (m) {
|
||
// running
|
||
if (sim.run) {
|
||
bool stepped = false;
|
||
|
||
// record cpu time at start of iteration
|
||
const auto startCPU = mj::Simulate::Clock::now();
|
||
|
||
// elapsed CPU and simulation time since last sync
|
||
const auto elapsedCPU = startCPU - syncCPU;
|
||
double elapsedSim = d->time - syncSim;
|
||
|
||
// inject noise
|
||
if (sim.ctrl_noise_std) {
|
||
// convert rate and scale to discrete time (Ornstein–Uhlenbeck)
|
||
mjtNum rate = mju_exp(-m->opt.timestep / mju_max(sim.ctrl_noise_rate, mjMINVAL));
|
||
mjtNum scale = sim.ctrl_noise_std * 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];
|
||
}
|
||
}
|
||
|
||
// requested slow-down factor
|
||
double slowdown = 100 / sim.percentRealTime[sim.real_time_index];
|
||
|
||
// misalignment condition: distance from target sim time is bigger than syncmisalign
|
||
bool misaligned =
|
||
mju_abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign;
|
||
|
||
// out-of-sync (for any reason): reset sync times, step
|
||
if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 ||
|
||
misaligned || sim.speed_changed) {
|
||
// re-sync
|
||
syncCPU = startCPU;
|
||
syncSim = d->time;
|
||
sim.speed_changed = false;
|
||
|
||
|
||
// run single step, let next iteration deal with timing
|
||
mj_step(m, d);
|
||
stepped = true;
|
||
}
|
||
|
||
// in-sync: step until ahead of cpu
|
||
else {
|
||
bool measured = false;
|
||
mjtNum prevSim = d->time;
|
||
|
||
double refreshTime = simRefreshFraction/sim.refresh_rate;
|
||
|
||
// step while sim lags behind cpu and within refreshTime
|
||
while (Seconds((d->time - syncSim)*slowdown) < mj::Simulate::Clock::now() - syncCPU &&
|
||
mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime)) {
|
||
// measure slowdown before first step
|
||
if (!measured && elapsedSim) {
|
||
sim.measured_slowdown =
|
||
std::chrono::duration<double>(elapsedCPU).count() / elapsedSim;
|
||
measured = true;
|
||
}
|
||
|
||
// call mj_step
|
||
mj_step(m, d);
|
||
stepped = true;
|
||
|
||
// break if reset
|
||
if (d->time < prevSim) {
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// save current state to history buffer
|
||
if (stepped) {
|
||
sim.AddToHistory();
|
||
}
|
||
}
|
||
|
||
// paused
|
||
else {
|
||
// run mj_forward, to update rendering and joint sliders
|
||
mj_forward(m, d);
|
||
sim.speed_changed = true;
|
||
}
|
||
}
|
||
} // release std::lock_guard<std::mutex>
|
||
}
|
||
}
|
||
} // namespace
|
||
|
||
//-------------------------------------- physics_thread --------------------------------------------
|
||
|
||
void PhysicsThread(mj::Simulate* sim, const char* filename) {
|
||
// request loadmodel if file given (otherwise drag-and-drop)
|
||
if (filename != nullptr) {
|
||
sim->LoadMessage(filename);
|
||
m = LoadModel(filename, *sim);
|
||
if (m) {
|
||
// lock the sim mutex
|
||
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
|
||
|
||
d = mj_makeData(m);
|
||
}
|
||
if (d) {
|
||
sim->Load(m, d, filename);
|
||
|
||
// lock the sim mutex
|
||
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
|
||
|
||
mj_forward(m, d);
|
||
|
||
// allocate ctrlnoise
|
||
free(ctrlnoise);
|
||
ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
|
||
mju_zero(ctrlnoise, m->nu);
|
||
} else {
|
||
sim->LoadMessageClear();
|
||
}
|
||
}
|
||
|
||
PhysicsLoop(*sim);
|
||
|
||
// delete everything we allocated
|
||
free(ctrlnoise);
|
||
mj_deleteData(d);
|
||
mj_deleteModel(m);
|
||
}
|
||
|
||
//------------------------------------------ main --------------------------------------------------
|
||
|
||
// machinery for replacing command line error by a macOS dialog box when running under Rosetta
|
||
#if defined(__APPLE__) && defined(__AVX__)
|
||
extern void DisplayErrorDialogBox(const char* title, const char* msg);
|
||
static const char* rosetta_error_msg = nullptr;
|
||
__attribute__((used, visibility("default"))) extern "C" void _mj_rosettaError(const char* msg) {
|
||
rosetta_error_msg = msg;
|
||
}
|
||
#endif |