Move simlib into experimental folder and rename it toolbox.
Also: - update the namespace to `toolbox` (instead of `filament`) - Add copyright notices to all files PiperOrigin-RevId: 808474353 Change-Id: Id1fe86c5d183c8e91bec8509b6c0830b5fe84c1c
This commit is contained in:
committed by
Copybara-Service
parent
8e9eb2fa38
commit
6f5a7576f9
@@ -0,0 +1,231 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/helpers.h"
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <ios>
|
||||
#include <iterator>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "third_party/libwebp/src/webp/encode.h"
|
||||
#include "third_party/libwebp/src/webp/types.h"
|
||||
#include <mujoco/mjrender.h>
|
||||
#include <mujoco/mjxmacro.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "xml/xml_api.h"
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
mjModel* LoadMujocoModel(const std::string& model_file, const mjVFS* vfs) {
|
||||
mjModel* model = nullptr;
|
||||
|
||||
if (model_file.empty()) {
|
||||
auto spec = mj_makeSpec();
|
||||
model = mj_compile(spec, 0);
|
||||
mj_deleteSpec(spec);
|
||||
} else if (model_file.ends_with(".mjb")) {
|
||||
model = mj_loadModel(model_file.c_str(), 0);
|
||||
if (!model) {
|
||||
mju_error("Could not load binary model");
|
||||
}
|
||||
} else if (model_file.ends_with(".xml")) {
|
||||
char error[1000] = "";
|
||||
model = mj_loadXML(model_file.c_str(), vfs, error, sizeof(error));
|
||||
if (!model) {
|
||||
mju_error("Load model error: %s", error);
|
||||
}
|
||||
} else {
|
||||
char error[1000] = "";
|
||||
auto spec =
|
||||
mj_parseXMLString(model_file.c_str(), nullptr, error, sizeof(error));
|
||||
if (!spec) {
|
||||
mju_error("Load model error: %s", error);
|
||||
}
|
||||
model = mj_compile(spec, 0);
|
||||
mj_deleteSpec(spec);
|
||||
}
|
||||
return model;
|
||||
}
|
||||
|
||||
void SaveText(const std::string& contents, const std::string& filename) {
|
||||
std::ofstream file(filename);
|
||||
file.write(contents.data(), contents.size());
|
||||
file.close();
|
||||
}
|
||||
|
||||
std::string LoadText(const std::string& filename) {
|
||||
std::ifstream file(filename);
|
||||
std::string contents((std::istreambuf_iterator<char>(file)),
|
||||
std::istreambuf_iterator<char>());
|
||||
file.close();
|
||||
return contents;
|
||||
}
|
||||
|
||||
void SaveColorToWebp(int width, int height, const unsigned char* data,
|
||||
const std::string& filename) {
|
||||
uint8_t* webp = nullptr;
|
||||
const size_t size =
|
||||
WebPEncodeLosslessRGB(data, width, height, width * 3, &webp);
|
||||
|
||||
std::ofstream file(filename, std::ios::binary);
|
||||
file.write(reinterpret_cast<const char*>(webp), size);
|
||||
file.close();
|
||||
WebPFree(webp);
|
||||
}
|
||||
|
||||
void SaveDepthToWebp(int width, int height, const float* data,
|
||||
const std::string& filename) {
|
||||
const int size = width * height;
|
||||
|
||||
// Turn the depth buffer into a greyscale color buffer.
|
||||
std::vector<unsigned char> byte_buffer;
|
||||
byte_buffer.reserve(size * 3);
|
||||
for (int i = 0; i < size; ++i) {
|
||||
auto byte = static_cast<int>(255.0 * data[i]);
|
||||
byte_buffer.push_back(byte);
|
||||
byte_buffer.push_back(byte);
|
||||
byte_buffer.push_back(byte);
|
||||
}
|
||||
SaveColorToWebp(width, height, byte_buffer.data(), filename);
|
||||
}
|
||||
|
||||
void SaveScreenshotToWebp(int width, int height, mjrContext* con,
|
||||
const std::string& filename) {
|
||||
mjr_setBuffer(mjFB_OFFSCREEN, con);
|
||||
auto rgb_buffer = std::vector<unsigned char>(3 * width * height);
|
||||
auto depth_buffer = std::vector<float>(width * height, 1.0f);
|
||||
mjrRect viewport = {0, 0, width, height};
|
||||
mjr_readPixels(rgb_buffer.data(), depth_buffer.data(), viewport, con);
|
||||
mjr_setBuffer(mjFB_WINDOW, con);
|
||||
SaveColorToWebp(width, height, rgb_buffer.data(), filename);
|
||||
}
|
||||
|
||||
const void* GetValue(const mjModel* model, const mjData* data,
|
||||
const char* field, int index) {
|
||||
MJDATA_POINTERS_PREAMBLE(model);
|
||||
#define X(TYPE, NAME, NR, NC) \
|
||||
if (!std::strcmp(#NAME, field) && !std::strcmp(#TYPE, "mjtNum")) { \
|
||||
if (index >= 0 && index < model->NR * NC) { \
|
||||
return &data->NAME[index]; \
|
||||
} else { \
|
||||
return nullptr; \
|
||||
} \
|
||||
}
|
||||
MJDATA_POINTERS
|
||||
#undef X
|
||||
return nullptr; // Invalid field.
|
||||
}
|
||||
|
||||
std::string CameraToString(const mjvScene* scene) {
|
||||
const mjvGLCamera* cameras = scene->camera;
|
||||
const float pos_x = (cameras[0].pos[0] + cameras[1].pos[0]) / 2;
|
||||
const float pos_y = (cameras[0].pos[1] + cameras[1].pos[1]) / 2;
|
||||
const float pos_z = (cameras[0].pos[2] + cameras[1].pos[2]) / 2;
|
||||
|
||||
mjtNum cam_forward[3];
|
||||
mju_f2n(cam_forward, cameras[0].forward, 3);
|
||||
mjtNum cam_up[3];
|
||||
mju_f2n(cam_up, cameras[0].up, 3);
|
||||
mjtNum cam_right[3];
|
||||
mju_cross(cam_right, cam_forward, cam_up);
|
||||
|
||||
char str[500];
|
||||
std::snprintf(str, sizeof(str),
|
||||
"<camera pos=\"%.3f %.3f %.3f\" xyaxes=\"%.3f %.3f %.3f %.3f "
|
||||
"%.3f %.3f\"/>\n",
|
||||
pos_x, pos_y, pos_z, cam_right[0], cam_right[1], cam_right[2],
|
||||
cam_up[0], cam_up[1], cam_up[2]);
|
||||
return str;
|
||||
}
|
||||
|
||||
std::string KeyframeToString(const mjModel* model, const mjData* data,
|
||||
bool full_precision) {
|
||||
const int kStrLen = 5000;
|
||||
|
||||
char buf[200];
|
||||
const char p_regular[] = "%g";
|
||||
const char p_full[] = "%-22.16g";
|
||||
const char* format = full_precision ? p_full : p_regular;
|
||||
|
||||
char str[kStrLen] = "<key\n";
|
||||
|
||||
// time
|
||||
std::strncat(str, " time=\"", kStrLen);
|
||||
std::snprintf(buf, sizeof(buf), format, data->time);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
|
||||
// qpos
|
||||
std::strncat(str, "\"\n qpos=\"", kStrLen);
|
||||
for (int i = 0; i < model->nq; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->qpos[i]);
|
||||
if (i < model->nq - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
|
||||
// qvel
|
||||
std::strncat(str, "\"\n qvel=\"", kStrLen);
|
||||
for (int i = 0; i < model->nv; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->qvel[i]);
|
||||
if (i < model->nv - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
|
||||
// act
|
||||
if (model->na > 0) {
|
||||
std::strncat(str, "\"\n act=\"", kStrLen);
|
||||
for (int i = 0; i < model->na; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->act[i]);
|
||||
if (i < model->na - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
}
|
||||
|
||||
// ctrl
|
||||
if (model->nu > 0) {
|
||||
std::strncat(str, "\"\n ctrl=\"", kStrLen);
|
||||
for (int i = 0; i < model->nu; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->ctrl[i]);
|
||||
if (i < model->nu - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
}
|
||||
|
||||
if (model->nmocap > 0) {
|
||||
std::strncat(str, "\"\n mpos=\"", kStrLen);
|
||||
for (int i = 0; i < 3 * model->nmocap; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->mocap_pos[i]);
|
||||
if (i < 3 * model->nmocap - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
|
||||
// mocap_quat
|
||||
std::strncat(str, "\"\n mquat=\"", kStrLen);
|
||||
for (int i = 0; i < 4 * model->nmocap; i++) {
|
||||
std::snprintf(buf, sizeof(buf), format, data->mocap_quat[i]);
|
||||
if (i < 4 * model->nmocap - 1) std::strncat(buf, " ", 200);
|
||||
std::strncat(str, buf, kStrLen);
|
||||
}
|
||||
}
|
||||
|
||||
std::strncat(str, "\"\n/>", kStrLen);
|
||||
return str;
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,66 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
// Standalone functions used by Simulate.
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_HELPERS_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_HELPERS_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjrender.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
// Function signature for loading assets from a given path.
|
||||
using LoadAssetFn = std::function<std::vector<std::byte>(std::string_view)>;
|
||||
|
||||
// Save/load for simple ascii files.
|
||||
void SaveText(const std::string& contents, const std::string& filename);
|
||||
std::string LoadText(const std::string& filename);
|
||||
|
||||
// Exports the given color buffer to a webp file.
|
||||
void SaveColorToWebp(int width, int height, const unsigned char* data,
|
||||
const std::string& filename);
|
||||
|
||||
// Exports the given depth buffer to a webp file.
|
||||
void SaveDepthToWebp(int width, int height, const float* data,
|
||||
const std::string& filename);
|
||||
|
||||
// Exports the current state of the mjrContext to a webp file.
|
||||
void SaveScreenshotToWebp(int width, int height, mjrContext* con,
|
||||
const std::string& filename);
|
||||
|
||||
// Loads a MuJoCo model from the given file.
|
||||
mjModel* LoadMujocoModel(const std::string& model_file, const mjVFS* vfs);
|
||||
|
||||
// Returns a pointer to the value of the given field in the given data.
|
||||
// Returns nullptr if the field is not found or the index is out of bounds.
|
||||
const void* GetValue(const mjModel* model, const mjData* data,
|
||||
const char* field, int index);
|
||||
|
||||
// Returns an XML string representation of the scene cameras.
|
||||
std::string CameraToString(const mjvScene* scene);
|
||||
|
||||
// Returns an XML string representation of current data keyframe.
|
||||
std::string KeyframeToString(const mjModel* model, const mjData* data,
|
||||
bool full_precision = false);
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_HELPERS_H_
|
||||
@@ -0,0 +1,92 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/imgui_widgets.h"
|
||||
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "third_party/dear_imgui/imgui.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
void AppendIniSection(std::string& ini, const std::string& section,
|
||||
const KeyValues& key_values) {
|
||||
if (section.front() != '[' || section.back() != ']') {
|
||||
mju_error("Section must be enclosed in square brackets.");
|
||||
}
|
||||
ini += "\n" + std::string(section) + "\n";
|
||||
for (auto& [key, value] : key_values) {
|
||||
ini += key + "=" + value + "\n";
|
||||
}
|
||||
}
|
||||
|
||||
KeyValues ReadIniSection(const std::string& contents,
|
||||
const std::string& section) {
|
||||
if (section.front() != '[' || section.back() != ']') {
|
||||
mju_error("Section must be enclosed in square brackets.");
|
||||
}
|
||||
bool in_section = false;
|
||||
|
||||
KeyValues key_values;
|
||||
std::istringstream f(contents);
|
||||
std::string line;
|
||||
while (std::getline(f, line)) {
|
||||
if (line[0] == '[') {
|
||||
in_section = (line == section);
|
||||
} else if (in_section) {
|
||||
std::string::size_type pos = line.find('=');
|
||||
if (pos != std::string::npos) {
|
||||
key_values[line.substr(0, pos)] = line.substr(pos + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
return key_values;
|
||||
}
|
||||
|
||||
bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) {
|
||||
float f = *value;
|
||||
const bool res = ImGui::SliderFloat(name, &f, min, max);
|
||||
if (res) {
|
||||
*value = f;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
bool ImGui_FileDialog(char* buf, int len) {
|
||||
bool ok = false;
|
||||
ImGui::Text("Filename");
|
||||
ImGui::SameLine();
|
||||
ImGui::InputText("##Filename", buf, len);
|
||||
if (ImGui::Button("OK", ImVec2(120, 0))) {
|
||||
ok = true;
|
||||
ImGui::CloseCurrentPopup();
|
||||
}
|
||||
ImGui::SetItemDefaultFocus();
|
||||
ImGui::SameLine();
|
||||
if (ImGui::Button("Cancel", ImVec2(120, 0))) {
|
||||
ImGui::CloseCurrentPopup();
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
void MaybeSaveToClipboard(const std::string& contents) {
|
||||
if (ImGui::GetIO().SetClipboardTextFn) {
|
||||
ImGui::GetIO().SetClipboardTextFn(nullptr, contents.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,239 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_IMGUI_WIDGETS_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_IMGUI_WIDGETS_H_
|
||||
|
||||
#include <cstring>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "third_party/dear_imgui/imgui.h"
|
||||
#include "third_party/dear_imgui/imgui_internal.h" // For ButtonEx and PressedOnClick
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
using KeyValues = std::unordered_map<std::string, std::string>;
|
||||
|
||||
// Appends key/value pairs to an Ini file.
|
||||
void AppendIniSection(std::string& ini, const std::string& section,
|
||||
const KeyValues& key_values);
|
||||
|
||||
// Reads key/value pairs from an Ini file section.
|
||||
KeyValues ReadIniSection(const std::string& contents,
|
||||
const std::string& section);
|
||||
|
||||
// ImGui file dialog.
|
||||
bool ImGui_FileDialog(char* buf, int len);
|
||||
|
||||
// ImGui Slider that supports both float and double types.
|
||||
bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max);
|
||||
|
||||
template <typename T>
|
||||
bool ImGui_Checkbox(const char* name, T& value) {
|
||||
static_assert(std::is_integral<T>());
|
||||
bool b = (value != 0);
|
||||
const bool res = ImGui::Checkbox(name, &b);
|
||||
if (res) {
|
||||
value = b ? 1 : 0;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
enum class ToggleKind {
|
||||
// Solid when ON and transparent when OFF.
|
||||
kButton,
|
||||
|
||||
// Slider which is right when ON and left when OFF.
|
||||
kSlider,
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
bool Toggle(const char* label, T& boolean,
|
||||
ToggleKind kind = ToggleKind::kButton, bool set_width = true) {
|
||||
static_assert(std::is_integral_v<T>, "Toggle only supports integral types.");
|
||||
|
||||
// Compute this width once and cache it. Only used when set_width is true.
|
||||
static int toggle_width = []() {
|
||||
int longest = 0;
|
||||
const char* longest_label = "";
|
||||
for (int i = 0; i < mjNVISFLAG; ++i) {
|
||||
int length = static_cast<int>(strlen(mjVISSTRING[i][0]));
|
||||
if (length > longest) {
|
||||
longest_label = mjVISSTRING[i][0];
|
||||
longest = length;
|
||||
}
|
||||
}
|
||||
return ImGui::CalcTextSize(longest_label).x + 5;
|
||||
}();
|
||||
|
||||
ImGui::PushID(label);
|
||||
bool changed = false;
|
||||
switch (kind) {
|
||||
case ToggleKind::kButton: {
|
||||
bool b = (boolean != 0);
|
||||
bool transparent = !b;
|
||||
|
||||
// NOTE(matijak): Its nice to have the button trigger on click but this
|
||||
// requires using the currently internal PressedOnClick flag and ButtonEx
|
||||
// function. It looks like this API has been stable for a long time, but
|
||||
// in case it changes in a way which breaks and is annoying to maintain we
|
||||
// can revert to the else clause and remove the imgui_internal.h include.
|
||||
// Note that the else clause overrides different style colors since the UI
|
||||
// is more intuitive with different settings.
|
||||
if constexpr (true) {
|
||||
ImColor button = ImGui::GetStyle().Colors[ImGuiCol_Button];
|
||||
if (transparent) button.Value.w = 0.0f;
|
||||
ImGui::PushStyleColor(ImGuiCol_Button, (ImU32)button);
|
||||
ImGui::PushStyleColor(ImGuiCol_ButtonActive, (ImU32)button);
|
||||
ImGui::PushStyleColor(ImGuiCol_ButtonHovered, (ImU32)button);
|
||||
|
||||
// Button width is set via an explicit size parameter, not via the
|
||||
// SetNextItemWidth function.
|
||||
ImVec2 size = set_width ? ImVec2(toggle_width, 0) : ImVec2(0, 0);
|
||||
changed = ImGui::ButtonEx(label, size, ImGuiButtonFlags_PressedOnClick);
|
||||
|
||||
if (changed) {
|
||||
b = !b;
|
||||
}
|
||||
boolean = b;
|
||||
|
||||
ImGui::PopStyleColor(3);
|
||||
} else {
|
||||
if (transparent) {
|
||||
ImColor button = ImGui::GetStyle().Colors[ImGuiCol_Button];
|
||||
button.Value.w = 0.0f;
|
||||
ImGui::PushStyleColor(ImGuiCol_Button, (ImU32)button);
|
||||
}
|
||||
|
||||
// Button width is set via an explicit size parameter, not via the
|
||||
// SetNextItemWidth function.
|
||||
ImVec2 size = set_width ? ImVec2(toggle_width, 0) : ImVec2(0, 0);
|
||||
changed = ImGui::Button(label, size);
|
||||
|
||||
if (changed) {
|
||||
b = !b;
|
||||
}
|
||||
boolean = b;
|
||||
|
||||
if (transparent) {
|
||||
ImGui::PopStyleColor(1);
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
case ToggleKind::kSlider: {
|
||||
int i = (int)boolean;
|
||||
const char* labels[2] = {label, label};
|
||||
const ImGuiSliderFlags flags = ImGuiSliderFlags_NoInput;
|
||||
if (set_width) ImGui::SetNextItemWidth(toggle_width);
|
||||
changed = ImGui::SliderInt("", &i, 0, 1, labels[i], flags);
|
||||
boolean = (i != 0);
|
||||
} break;
|
||||
}
|
||||
ImGui::PopID();
|
||||
return changed;
|
||||
}
|
||||
|
||||
inline bool ToggleBit(const char* label, int& flags, int flags_value,
|
||||
ToggleKind kind = ToggleKind::kButton,
|
||||
bool set_width = true) {
|
||||
bool boolean = flags & flags_value;
|
||||
bool changed = Toggle(label, boolean, kind, set_width);
|
||||
if (changed) {
|
||||
flags = boolean ? (flags | flags_value) : (flags & ~flags_value);
|
||||
}
|
||||
return changed;
|
||||
}
|
||||
|
||||
// Options for ImGui_InputN (see below).
|
||||
template <typename T>
|
||||
struct ImGuiOpts {
|
||||
std::optional<T> min;
|
||||
std::optional<T> max;
|
||||
std::optional<T> step;
|
||||
std::optional<T> step_fast;
|
||||
std::optional<float> width;
|
||||
const char* format = std::is_floating_point_v<T> ? "%.3g" : "%d";
|
||||
};
|
||||
|
||||
// A compile-time wrapper around ImGui::InputScalarN. This is useful because
|
||||
// MuJoCo uses an `mjtNum` type which is an alias for float or double.
|
||||
//
|
||||
// Options can be used to specify step sizes, clamp ranges, and formatting.
|
||||
template <typename T>
|
||||
bool ImGui_InputN(const char* name, T* value, int num, ImGuiOpts<T> opts = {}) {
|
||||
bool res = false;
|
||||
if (opts.width) {
|
||||
ImGui::SetNextItemWidth(opts.width.value());
|
||||
}
|
||||
if constexpr (std::is_same_v<T, int>) {
|
||||
const int step = opts.step.value_or(1);
|
||||
const int step_fast = opts.step_fast.value_or(100);
|
||||
const char* format = opts.format;
|
||||
res = ImGui::InputScalarN(name, ImGuiDataType_S32, value, num, &step,
|
||||
&step_fast, format);
|
||||
|
||||
} else if constexpr (std::is_same_v<T, float>) {
|
||||
const float step = opts.step.value_or(0.f);
|
||||
const float step_fast = opts.step_fast.value_or(0.f);
|
||||
const float* pstep = opts.step.has_value() ? &step : nullptr;
|
||||
const float* pstep_fast = opts.step_fast.has_value() ? &step_fast : nullptr;
|
||||
const char* format = opts.format ? opts.format : "%.3f";
|
||||
res = ImGui::InputScalarN(name, ImGuiDataType_Float, value, num, pstep,
|
||||
pstep_fast, format);
|
||||
|
||||
} else if constexpr (std::is_same_v<T, double>) {
|
||||
const double step = opts.step.value_or(0.0);
|
||||
const double step_fast = opts.step_fast.value_or(0.0);
|
||||
const double* pstep = opts.step.has_value() ? &step : nullptr;
|
||||
const double* pstep_fast =
|
||||
opts.step_fast.has_value() ? &step_fast : nullptr;
|
||||
const char* format = opts.format ? opts.format : "%.3f";
|
||||
res = ImGui::InputScalarN(name, ImGuiDataType_Double, value, num, pstep,
|
||||
pstep_fast, format);
|
||||
} else {
|
||||
static_assert(false, "Unsupported type");
|
||||
}
|
||||
|
||||
if (opts.min.has_value()) {
|
||||
if (*value < *opts.min) *value = *opts.min;
|
||||
}
|
||||
if (opts.max.has_value()) {
|
||||
if (*value > *opts.max) *value = *opts.max;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool ImGui_Input(const char* name, T* value, ImGuiOpts<T> opts = {}) {
|
||||
return ImGui_InputN(name, value, 1, opts);
|
||||
}
|
||||
|
||||
// Returns true if the given chord is has _just_ been pressed in this frame.
|
||||
// (This is opposed to "Pressed" which means the chord is active, i.e. the user
|
||||
// is holding down the keys.)
|
||||
inline bool ImGui_IsChordJustPressed(ImGuiKeyChord chord) {
|
||||
return ImGui::IsKeyChordPressed(chord, 0);
|
||||
}
|
||||
|
||||
// Saves the given contents to the clipboard if the clipboard is available.
|
||||
void MaybeSaveToClipboard(const std::string& contents);
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_IMGUI_WIDGETS_H_
|
||||
@@ -0,0 +1,281 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/interaction.h"
|
||||
#include <algorithm>
|
||||
|
||||
#include "third_party/dear_imgui/imgui.h"
|
||||
#include "experimental/toolbox/imgui_widgets.h"
|
||||
#include "experimental/toolbox/physics.h"
|
||||
#include "experimental/toolbox/renderer.h"
|
||||
#include "experimental/toolbox/window.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
static void ToggleFlag(mjtByte& flag) { flag = flag ? 0 : 1; }
|
||||
|
||||
static void SelectParentPerturb(const mjModel* model, mjvPerturb& perturb) {
|
||||
if (perturb.select > 0) {
|
||||
perturb.select = model->body_parentid[perturb.select];
|
||||
perturb.flexselect = -1;
|
||||
perturb.skinselect = -1;
|
||||
if (perturb.select <= 0) {
|
||||
perturb.active = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PickResult Pick(float x, float y, Window* window, Renderer* renderer,
|
||||
Physics* physics, const mjvOption& vis_options) {
|
||||
const float w = static_cast<float>(window->GetWidth());
|
||||
const float h = static_cast<float>(window->GetHeight());
|
||||
const float aspect_ratio = w / h;
|
||||
|
||||
PickResult result;
|
||||
result.body =
|
||||
mjv_select(physics->GetModel(), physics->GetData(), &vis_options,
|
||||
aspect_ratio, x, 1.0f - y, &renderer->GetScene(), result.point,
|
||||
&result.geom, &result.flex, &result.skin);
|
||||
return result;
|
||||
}
|
||||
|
||||
int SetCamera(const mjModel& model, mjvCamera& camera, int request_idx) {
|
||||
// 0 = free, 1 = tracking, 2+ = fixed
|
||||
int camera_idx = std::clamp(request_idx, 0, std::max(model.ncam + 1, 0));
|
||||
if (camera_idx == 0) {
|
||||
camera.type = mjCAMERA_FREE;
|
||||
} else if (camera_idx == 1) {
|
||||
if (camera.trackbodyid >= 0) {
|
||||
camera.type = mjCAMERA_TRACKING;
|
||||
camera.fixedcamid = -1;
|
||||
} else {
|
||||
camera.type = mjCAMERA_FREE;
|
||||
camera_idx = 0;
|
||||
}
|
||||
} else {
|
||||
camera.type = mjCAMERA_FIXED;
|
||||
camera.fixedcamid = camera_idx - 2;
|
||||
}
|
||||
|
||||
return camera_idx;
|
||||
}
|
||||
|
||||
void HandleMouseEvents(Window* window, Renderer* renderer,
|
||||
Physics* physics, mjvPerturb& perturb,
|
||||
mjvOption& vis_options, mjvCamera& camera,
|
||||
int& camera_idx) {
|
||||
auto& io = ImGui::GetIO();
|
||||
|
||||
if (io.WantCaptureMouse) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Normalize mouse positions and movement to display size.
|
||||
const float mouse_x = io.MousePos.x / io.DisplaySize.x;
|
||||
const float mouse_y = io.MousePos.y / io.DisplaySize.y;
|
||||
const float mouse_dx = io.MouseDelta.x / io.DisplaySize.x;
|
||||
const float mouse_dy = io.MouseDelta.y / io.DisplaySize.y;
|
||||
const float mouse_scroll = io.MouseWheel / 50.0f;
|
||||
|
||||
mjModel* model = physics->GetModel();
|
||||
mjData* data = physics->GetData();
|
||||
mjvScene& scene = renderer->GetScene();
|
||||
|
||||
// Determine the mouse action based on which buttons are down.
|
||||
mjtMouse action = mjMOUSE_NONE;
|
||||
if (ImGui::IsMouseDown(ImGuiMouseButton_Left)) {
|
||||
action = io.KeyShift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
|
||||
} else if (ImGui::IsMouseDown(ImGuiMouseButton_Right)) {
|
||||
action = io.KeyShift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
|
||||
} else if (ImGui::IsMouseDown(ImGuiMouseButton_Middle)) {
|
||||
action = mjMOUSE_ZOOM;
|
||||
} else {
|
||||
// If no mouse buttons are down, end any active perturbations.
|
||||
perturb.active = 0;
|
||||
}
|
||||
|
||||
// Mouse scroll.
|
||||
if (model && mouse_scroll != 0.0f) {
|
||||
mjv_moveCamera(model, mjMOUSE_ZOOM, 0, mouse_scroll, &scene, &camera);
|
||||
}
|
||||
|
||||
// Mouse drag.
|
||||
if (model && data && action != mjMOUSE_NONE &&
|
||||
(mouse_dx != 0.0f || mouse_dy != 0.0f)) {
|
||||
// If ctrl is pressed, move the perturbation, otherwise move the camera.
|
||||
if (io.KeyCtrl) {
|
||||
if (perturb.select > 0) {
|
||||
const int active =
|
||||
action == mjMOUSE_MOVE_V ? mjPERT_TRANSLATE : mjPERT_ROTATE;
|
||||
if (active != perturb.active) {
|
||||
mjv_initPerturb(model, data, &scene, &perturb);
|
||||
perturb.active = active;
|
||||
}
|
||||
mjv_movePerturb(model, data, action, mouse_dx, mouse_dy, &scene,
|
||||
&perturb);
|
||||
}
|
||||
} else {
|
||||
mjv_moveCamera(model, action, mouse_dx, mouse_dy, &scene, &camera);
|
||||
}
|
||||
}
|
||||
|
||||
// Left double click.
|
||||
if (data && ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Left)) {
|
||||
PickResult picked =
|
||||
Pick(mouse_x, mouse_y, window, renderer, physics, vis_options);
|
||||
if (picked.body >= 0) {
|
||||
perturb.select = picked.body;
|
||||
perturb.flexselect = picked.flex;
|
||||
perturb.skinselect = picked.skin;
|
||||
|
||||
// Compute the local position of the selected object in the world.
|
||||
mjtNum tmp[3];
|
||||
mju_sub3(tmp, picked.point, data->xpos + 3 * picked.body);
|
||||
mju_mulMatTVec(perturb.localpos, data->xmat + 9 * picked.body, tmp, 3, 3);
|
||||
} else {
|
||||
perturb.select = 0;
|
||||
perturb.flexselect = -1;
|
||||
perturb.skinselect = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Right double click.
|
||||
if (ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Right)) {
|
||||
PickResult picked =
|
||||
Pick(mouse_x, mouse_y, window, renderer, physics, vis_options);
|
||||
mju_copy3(camera.lookat, picked.point);
|
||||
if (picked.body > 0 && io.KeyCtrl) {
|
||||
camera.type = mjCAMERA_TRACKING;
|
||||
camera.trackbodyid = picked.body;
|
||||
camera.fixedcamid = -1;
|
||||
camera_idx = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HandleKeyboardEvents(Window* window, Renderer* renderer,
|
||||
Physics* physics, mjvPerturb& perturb,
|
||||
mjvOption& vis_options, mjvCamera& camera,
|
||||
int& camera_idx) {
|
||||
if (ImGui::GetIO().WantCaptureKeyboard) {
|
||||
return;
|
||||
}
|
||||
|
||||
mjModel* model = physics->GetModel();
|
||||
|
||||
// Physics control shortcuts.
|
||||
if (ImGui_IsChordJustPressed(ImGuiKey_Space)) {
|
||||
physics->TogglePause();
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Backspace)) {
|
||||
physics->Reset();
|
||||
}
|
||||
|
||||
// Camera shortcuts.
|
||||
if (model) {
|
||||
if (ImGui_IsChordJustPressed(ImGuiKey_Escape)) {
|
||||
camera_idx = SetCamera(*model, camera, 0);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_LeftBracket)) {
|
||||
camera_idx = SetCamera(*model, camera, camera_idx - 1);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_RightBracket)) {
|
||||
camera_idx = SetCamera(*model, camera, camera_idx + 1);
|
||||
}
|
||||
}
|
||||
|
||||
// Perturb shortcuts.
|
||||
if (ImGui_IsChordJustPressed(ImGuiKey_PageUp)) {
|
||||
SelectParentPerturb(model, perturb);
|
||||
}
|
||||
|
||||
// Visualization shortcuts.
|
||||
if (ImGui_IsChordJustPressed(ImGuiKey_F6)) {
|
||||
vis_options.frame = (vis_options.frame + 1) % mjNFRAME;
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_F7)) {
|
||||
vis_options.label = (vis_options.label + 1) % mjNLABEL;
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Comma)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_ACTIVATION]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Backslash)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_MESHBVH]);
|
||||
// } else if (ImGui_IsChordJustPressed(ImGuiKey_Backquote)) {
|
||||
// ToggleFlag(vis_options.flags[mjVIS_BODYBVH]);
|
||||
// } else if (ImGui_IsChordJustPressed(ImGuiKey_Quote)) {
|
||||
// ToggleFlag(vis_options.flags[mjVIS_SCLINERTIA]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Semicolon)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_SKIN]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_U)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_ACTUATOR]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Q)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CAMERA]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_M)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_COM]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_F)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CONTACTFORCE]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_C)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CONTACTPOINT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_P)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CONTACTSPLIT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_H)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CONVEXHULL]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_E)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_CONSTRAINT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_I)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_ISLAND]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_J)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_JOINT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Z)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_LIGHT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_B)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_PERTFORCE]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_O)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_PERTOBJ]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_Y)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_RANGEFINDER]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_V)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_TENDON]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_X)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_TEXTURE]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_T)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_TRANSPARENT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_K)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_AUTOCONNECT]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_G)) {
|
||||
ToggleFlag(vis_options.flags[mjVIS_STATIC]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_0 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[0]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_1 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[1]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_2 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[2]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_3 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[3]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_4 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[4]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_5 | ImGuiMod_Shift)) {
|
||||
ToggleFlag(vis_options.sitegroup[5]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_0)) {
|
||||
ToggleFlag(vis_options.geomgroup[0]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_1)) {
|
||||
ToggleFlag(vis_options.geomgroup[1]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_2)) {
|
||||
ToggleFlag(vis_options.geomgroup[2]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_3)) {
|
||||
ToggleFlag(vis_options.geomgroup[3]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_4)) {
|
||||
ToggleFlag(vis_options.geomgroup[4]);
|
||||
} else if (ImGui_IsChordJustPressed(ImGuiKey_5)) {
|
||||
ToggleFlag(vis_options.geomgroup[5]);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_INTERACTION_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_INTERACTION_H_
|
||||
|
||||
#include "experimental/toolbox/physics.h"
|
||||
#include "experimental/toolbox/renderer.h"
|
||||
#include "experimental/toolbox/window.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
// The result of a pick operation.
|
||||
struct PickResult {
|
||||
mjtNum point[3]; // World coordinates
|
||||
int body = -1;
|
||||
int geom = -1;
|
||||
int flex = -1;
|
||||
int skin = -1;
|
||||
};
|
||||
|
||||
// Returns information about the object (if any) under the mouse cursor.
|
||||
PickResult Pick(float x, float y, Window* window, Renderer* renderer,
|
||||
Physics* physics, const mjvOption& vis_options);
|
||||
|
||||
// Updates the camera according to the requested index using this convention:
|
||||
//
|
||||
// 0 : selects the free camera (not defined in the model)
|
||||
// 1 : selects the tracking camera (also not defined in the model)
|
||||
// 2+ : selects a camera in the model; e.g. index 2 => model.cam[0];
|
||||
//
|
||||
// The function returns the index of the used camera following the same
|
||||
// convention. Note the returned index may differ from the request if the
|
||||
// request was invalid (index was out of range or tracking camera was not
|
||||
// available).
|
||||
int SetCamera(const mjModel& model, mjvCamera& camera, int request_idx);
|
||||
|
||||
// Handles canonical mouse events.
|
||||
void HandleMouseEvents(Window* window, Renderer* renderer, Physics* physics,
|
||||
mjvPerturb& perturb, mjvOption& vis_options,
|
||||
mjvCamera& camera, int& camera_idx);
|
||||
|
||||
// Handles canonical keyboard events.
|
||||
void HandleKeyboardEvents(Window* window, Renderer* renderer, Physics* physics,
|
||||
mjvPerturb& perturb, mjvOption& vis_options,
|
||||
mjvCamera& camera, int& camera_idx);
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_INTERACTION_H_
|
||||
@@ -0,0 +1,222 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/physics.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <climits>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <ratio>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "experimental/toolbox/helpers.h"
|
||||
#include "experimental/toolbox/step_control.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
static mjtNum Timer() {
|
||||
using Clock = std::chrono::steady_clock;
|
||||
using Milliseconds = std::chrono::duration<double, std::milli>;
|
||||
static Clock::time_point start = Clock::now();
|
||||
return Milliseconds(Clock::now() - start).count();
|
||||
}
|
||||
|
||||
Physics::Physics(OnModelLoadedFn on_model_loaded)
|
||||
: on_model_loaded_(std::move(on_model_loaded)) {
|
||||
mjcb_time = Timer;
|
||||
}
|
||||
|
||||
Physics::~Physics() { Clear(); }
|
||||
|
||||
void Physics::LoadModel(std::string model_file, const mjVFS* vfs) {
|
||||
pending_load_ = std::move(model_file);
|
||||
vfs_ = vfs;
|
||||
}
|
||||
|
||||
void Physics::ProcessPendingLoad() {
|
||||
if (!pending_load_.has_value()) {
|
||||
return;
|
||||
}
|
||||
|
||||
Clear();
|
||||
|
||||
std::string model_file = std::move(pending_load_.value());
|
||||
pending_load_.reset();
|
||||
|
||||
model_ = LoadMujocoModel(model_file, vfs_);
|
||||
if (!model_) mju_error("Error loading model");
|
||||
|
||||
data_ = mj_makeData(model_);
|
||||
if (!data_) mju_error("Error making data.");
|
||||
|
||||
on_model_loaded_(model_file);
|
||||
|
||||
InitHistory();
|
||||
}
|
||||
|
||||
void Physics::Clear() {
|
||||
if (model_) {
|
||||
mj_deleteData(data_);
|
||||
data_ = nullptr;
|
||||
mj_deleteModel(model_);
|
||||
model_ = nullptr;
|
||||
|
||||
history_.clear();
|
||||
history_cursor_ = 0;
|
||||
steps_ = 0;
|
||||
GetStepControl().SetSpeed(100.f);
|
||||
|
||||
error_ = "";
|
||||
}
|
||||
}
|
||||
|
||||
void Physics::Reset() {
|
||||
mj_resetData(model_, data_);
|
||||
mj_forward(model_, data_);
|
||||
error_ = "";
|
||||
history_cursor_ = 0;
|
||||
}
|
||||
|
||||
bool Physics::Update(const mjvPerturb* perturb) {
|
||||
ProcessPendingLoad();
|
||||
|
||||
if (!model_ || !data_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (data_) {
|
||||
for (int i = 0; i < mjNTIMER; i++) {
|
||||
data_->timer[i].duration = 0;
|
||||
data_->timer[i].number = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (!IsPaused()) {
|
||||
mju_zero(data_->xfrc_applied, 6 * model_->nbody);
|
||||
mjv_applyPerturbPose(model_, data_, perturb, 0);
|
||||
mjv_applyPerturbForce(model_, data_, perturb);
|
||||
} else {
|
||||
mjv_applyPerturbPose(model_, data_, perturb, 1);
|
||||
}
|
||||
|
||||
if (IsPaused() && !single_step_) {
|
||||
// run mj_forward, to update rendering and joint sliders
|
||||
mj_forward(model_, data_);
|
||||
if (pause_update_) {
|
||||
mju_copy(data_->qacc_warmstart, data_->qacc, model_->nv);
|
||||
}
|
||||
|
||||
// When unpaused make sure we sync to immediately and step once. Without
|
||||
// this we could step many times before rendering resulting in a noticeable
|
||||
// delay before the simulation restarts (especially for large slowdowns)
|
||||
GetStepControl().ForceSync();
|
||||
} else {
|
||||
if (single_step_) {
|
||||
GetStepControl().ForceSync();
|
||||
single_step_ = false;
|
||||
}
|
||||
|
||||
StepControl::Status status = GetStepControl().Advance(model_, data_);
|
||||
if (status == StepControl::Status::kOk) {
|
||||
AddToHistory();
|
||||
} else if (status == StepControl::Status::kAutoReset) {
|
||||
Reset();
|
||||
} else if (status == StepControl::Status::kDiverged) {
|
||||
for (mjtWarning w : StepControl::kDivergedWarnings) {
|
||||
if (data_->warning[w].number > 0) {
|
||||
paused_ = true;
|
||||
error_ = mju_warningText(w, data_->warning[w].lastinfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Physics::UpdateState(mjtNum* state, unsigned int state_sig) {
|
||||
ProcessPendingLoad();
|
||||
if (!model_ || !data_) {
|
||||
return false;
|
||||
}
|
||||
mj_setState(model_, data_, state, state_sig);
|
||||
mj_forward(model_, data_);
|
||||
return true;
|
||||
}
|
||||
|
||||
void Physics::TogglePause() { paused_ = !paused_; }
|
||||
|
||||
void Physics::RequestSingleStep() { single_step_ = true; }
|
||||
|
||||
void Physics::InitHistory() {
|
||||
const int state_size = mj_stateSize(model_, mjSTATE_INTEGRATION);
|
||||
|
||||
// History buffer will be smaller of 2000 states or 100 MB.
|
||||
constexpr int kMaxBytes = 1e8;
|
||||
constexpr int kMaxHistory = 2000;
|
||||
const int state_bytes = state_size * sizeof(mjtNum);
|
||||
const int history_length = std::min(INT_MAX / state_bytes, kMaxHistory);
|
||||
const int history_bytes = std::min(state_bytes * history_length, kMaxBytes);
|
||||
const int num_history = history_bytes / state_bytes;
|
||||
|
||||
history_.resize(num_history);
|
||||
for (std::vector<mjtNum>& state : history_) {
|
||||
state.resize(state_size, 0);
|
||||
}
|
||||
history_cursor_ = 0;
|
||||
}
|
||||
|
||||
void Physics::AddToHistory() {
|
||||
if (!history_.empty()) {
|
||||
mjtNum* state = history_[history_cursor_].data();
|
||||
mj_getState(model_, data_, state, mjSTATE_INTEGRATION);
|
||||
history_cursor_ = (history_cursor_ + 1) % history_.size();
|
||||
steps_++;
|
||||
|
||||
// If we are adding to the history we didn't have a divergence error
|
||||
error_ = "";
|
||||
}
|
||||
}
|
||||
|
||||
int Physics::LoadHistory(int offset) {
|
||||
// No history to load.
|
||||
if (steps_ == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Pause simulation when entering history mode.
|
||||
paused_ = true;
|
||||
|
||||
// Ensure the offset is within a valid range. It's a negative value since
|
||||
// we will be going backwards from the "latest" frame.
|
||||
const int max_history = std::min<int>(steps_, history_.size());
|
||||
offset = std::clamp(offset, -max_history + 1, 0);
|
||||
|
||||
// Determine the index in the history buffer that corresponds to the frame
|
||||
// index.
|
||||
const int idx = (history_cursor_ + offset - 1) % history_.size();
|
||||
const mjtNum* state = history_[idx].data();
|
||||
|
||||
// Load the state into the data buffer.
|
||||
mj_setState(model_, data_, state, mjSTATE_INTEGRATION);
|
||||
mj_forward(model_, data_);
|
||||
return offset;
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,126 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_PHYSICS_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_PHYSICS_H_
|
||||
|
||||
#include <functional>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <vector>
|
||||
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "experimental/toolbox/step_control.h"
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
// Owns the MuJoCo simulation state (e.g. mjModel and mjData) and is responsible
|
||||
// for updating the state of the simulation.
|
||||
class Physics {
|
||||
public:
|
||||
using OnModelLoadedFn = std::function<void(std::string_view)>;
|
||||
|
||||
explicit Physics(OnModelLoadedFn on_model_loaded);
|
||||
~Physics();
|
||||
|
||||
Physics(const Physics&) = delete;
|
||||
Physics& operator=(const Physics&) = delete;
|
||||
|
||||
// Access the step controller
|
||||
StepControl& GetStepControl() { return step_control_; }
|
||||
|
||||
// Loads a model from the given path. An empty string will load an empty
|
||||
// scene.
|
||||
void LoadModel(std::string model_file, const mjVFS* vfs = nullptr);
|
||||
|
||||
// Clears the simulation, clearing all loaded state.
|
||||
void Clear();
|
||||
|
||||
// Resets the simulation using mj_resetData
|
||||
void Reset();
|
||||
|
||||
// Advances the state of the simulation.
|
||||
bool Update(const mjvPerturb* perturb);
|
||||
|
||||
// Sets the state of the simulation.
|
||||
bool UpdateState(mjtNum* state, unsigned int state_sig);
|
||||
|
||||
// Renders the state of the simulation.
|
||||
void Render();
|
||||
|
||||
// Returns true if the simulation is paused.
|
||||
bool IsPaused() { return paused_; }
|
||||
|
||||
// Pauses/unpauses the simulation.
|
||||
void TogglePause();
|
||||
|
||||
// If the simulation is paused, will perform a single step on the next
|
||||
// Update() call.
|
||||
void RequestSingleStep();
|
||||
|
||||
// Returns the number of steps the simulation has taken.
|
||||
int GetStepCount() const { return steps_; }
|
||||
|
||||
// Returns the number of states in the history buffer.
|
||||
int GetHistorySize() const { return history_.size(); }
|
||||
|
||||
// Loads a state from the history buffer at the given offset into the current
|
||||
// physics state.
|
||||
//
|
||||
// Calling this function will automatically pause the simulation.
|
||||
int LoadHistory(int offset);
|
||||
|
||||
// Selects the parent of the currently selected perturb object.
|
||||
void SelectParentPerturb();
|
||||
|
||||
// Returns the MuJoCo data structures owned by this Simulation object.
|
||||
mjModel* GetModel() { return model_; }
|
||||
mjData* GetData() { return data_; }
|
||||
|
||||
// Returns the error message from the simulation.
|
||||
std::string_view GetError() const { return error_; }
|
||||
|
||||
private:
|
||||
void ProcessPendingLoad();
|
||||
void InitHistory();
|
||||
void AddToHistory();
|
||||
|
||||
mjModel* model_ = nullptr;
|
||||
mjData* data_ = nullptr;
|
||||
std::vector<std::vector<mjtNum>> history_;
|
||||
int history_cursor_ = 0;
|
||||
|
||||
OnModelLoadedFn on_model_loaded_;
|
||||
|
||||
// If true and paused, d->qacc_warmstart is set to d->qacc after mj_forward
|
||||
// which has the effect of making the constraint solver eventually converge
|
||||
// while the simulation is paused.
|
||||
bool pause_update_ = false;
|
||||
|
||||
bool paused_ = false;
|
||||
bool single_step_ = false;
|
||||
int steps_ = 0;
|
||||
std::optional<std::string> pending_load_;
|
||||
const mjVFS* vfs_;
|
||||
|
||||
std::string error_;
|
||||
|
||||
StepControl step_control_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_PHYSICS_H_
|
||||
@@ -0,0 +1,82 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/renderer.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
|
||||
#include "experimental/toolbox/helpers.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
Renderer::Renderer(MakeContextFn make_context_fn)
|
||||
: make_context_fn_(make_context_fn) {
|
||||
}
|
||||
|
||||
Renderer::~Renderer() { Deinit(); }
|
||||
|
||||
void Renderer::Init(const mjModel* model) {
|
||||
if (initialized_) {
|
||||
Deinit();
|
||||
}
|
||||
|
||||
if (model == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
make_context_fn_(model, &render_context_);
|
||||
mjv_defaultScene(&scene_);
|
||||
mjv_makeScene(model, &scene_, 2000);
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
void Renderer::Deinit() {
|
||||
mjv_freeScene(&scene_);
|
||||
mjr_freeContext(&render_context_);
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
void Renderer::Sync(const mjModel* model, mjData* data,
|
||||
const mjvPerturb* perturb, mjvCamera* camera,
|
||||
const mjvOption* vis_option) {
|
||||
mjv_updateScene(model, data, vis_option, perturb, camera, mjCAT_ALL,
|
||||
&scene_);
|
||||
}
|
||||
|
||||
void Renderer::Render(const mjModel* model, mjData* data,
|
||||
const mjvPerturb* perturb, mjvCamera* camera,
|
||||
const mjvOption* vis_option, int width, int height) {
|
||||
mjrRect main_viewport = {0, 0, width, height};
|
||||
mjr_render(main_viewport, data ? &scene_ : nullptr, &render_context_);
|
||||
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto delta_time = now - last_fps_update_;
|
||||
const double interval = std::chrono::duration<double>(delta_time).count();
|
||||
|
||||
++frames_;
|
||||
if (interval > 0.2) { // only update FPS stat at most 5 times per second
|
||||
last_fps_update_ = now;
|
||||
fps_ = frames_ / interval;
|
||||
frames_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Renderer::SaveScreenshot(const std::string& filename, int width,
|
||||
int height) {
|
||||
SaveScreenshotToWebp(width, height, &render_context_, filename);
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,82 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_RENDERER_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_RENDERER_H_
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
// Renders the mujoco simulation and the imgui state into the active window
|
||||
// using the filament rendering backend.
|
||||
class Renderer {
|
||||
public:
|
||||
// Function that creates a mjrContext for the given model. We use a function
|
||||
// to allow different mjrContext implementations to be created without
|
||||
// requiring a direct dependency on them.
|
||||
using MakeContextFn = std::function<void(const mjModel* m, mjrContext* con)>;
|
||||
|
||||
explicit Renderer(MakeContextFn make_context_fn);
|
||||
~Renderer();
|
||||
|
||||
Renderer(const Renderer&) = delete;
|
||||
Renderer& operator=(const Renderer&) = delete;
|
||||
|
||||
// Initializes the renderer with the given mjModel.
|
||||
void Init(const mjModel* model);
|
||||
|
||||
// Updates the render scene with the current simulation state.
|
||||
void Sync(const mjModel* model, mjData* data, const mjvPerturb* perturb,
|
||||
mjvCamera* camera, const mjvOption* vis_option);
|
||||
|
||||
// Renders the simulation state into the active window. Also renders the imgui
|
||||
// state, but that is obtained directly from the ImGui library.
|
||||
void Render(const mjModel* model, mjData* data, const mjvPerturb* perturb,
|
||||
mjvCamera* camera, const mjvOption* vis_option, int width,
|
||||
int height);
|
||||
|
||||
// Saves a screenshot of the simulation state into the given file.
|
||||
void SaveScreenshot(const std::string& filename, int width, int height);
|
||||
|
||||
// Returns the mjvScene used by the renderer.
|
||||
mjvScene& GetScene() { return scene_; }
|
||||
|
||||
// Returns the current, average frame rate.
|
||||
double GetFrameRate() const { return fps_; }
|
||||
|
||||
private:
|
||||
using TimePoint = std::chrono::time_point<std::chrono::steady_clock>;
|
||||
|
||||
// Resets the renderer; no rendering will occur until Init() is called again.
|
||||
void Deinit();
|
||||
|
||||
MakeContextFn make_context_fn_;
|
||||
mjrContext render_context_;
|
||||
mjvScene scene_;
|
||||
bool initialized_ = false;
|
||||
|
||||
int frames_ = 0;
|
||||
TimePoint last_fps_update_;
|
||||
double fps_ = 0;
|
||||
};
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_RENDERER_H_
|
||||
@@ -0,0 +1,178 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/step_control.h"
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
float StepControl::GetSpeedMeasured() const {
|
||||
return speed_measured_;
|
||||
}
|
||||
|
||||
float StepControl::GetSpeed() const {
|
||||
return speed_;
|
||||
}
|
||||
|
||||
void StepControl::SetSpeed(float speed_percent_real_time) {
|
||||
speed_ = std::clamp(speed_percent_real_time, .1f, 100.f);
|
||||
ForceSync();
|
||||
}
|
||||
|
||||
void StepControl::ForceSync() { force_sync_ = true; }
|
||||
|
||||
void StepControl::GetNoiseParameters(float& ctrl_noise_scale,
|
||||
float& ctrl_noise_rate) const {
|
||||
ctrl_noise_scale = ctrl_noise_std_;
|
||||
ctrl_noise_rate = ctrl_noise_rate_;
|
||||
}
|
||||
|
||||
void StepControl::SetNoiseParameters(float ctrl_noise_scale,
|
||||
float ctrl_noise_rate) {
|
||||
ctrl_noise_std_ = ctrl_noise_scale;
|
||||
ctrl_noise_rate_ = ctrl_noise_rate;
|
||||
}
|
||||
|
||||
StepControl::Status StepControl::Advance(const mjModel* m, mjData* d) {
|
||||
if (!m) {
|
||||
return Status::kOk;
|
||||
}
|
||||
|
||||
const Clock::time_point start_cpu = Clock::now();
|
||||
const double slowdown = 100. / std::clamp<double>(speed_, 0.001, 100.);
|
||||
double elapsed_cpu = Seconds(start_cpu - sync_cpu_).count();
|
||||
double elapsed_sim = d->time - sync_sim_;
|
||||
|
||||
bool resync = false;
|
||||
|
||||
// Resync if we're forced to.
|
||||
if (force_sync_) {
|
||||
force_sync_ = false;
|
||||
resync = true;
|
||||
}
|
||||
|
||||
// Resync if we've never synced.
|
||||
if (sync_cpu_.time_since_epoch().count() == 0) {
|
||||
resync = true;
|
||||
}
|
||||
|
||||
// Resync if any elapsed time is negative.
|
||||
if (elapsed_cpu < 0 || elapsed_sim < 0) {
|
||||
resync = true;
|
||||
}
|
||||
|
||||
// Resync if the distance from the target simulation time is bigger than
|
||||
// sync_misalign_ (misalignment condition).
|
||||
if (std::abs(elapsed_cpu / slowdown - elapsed_sim) > sync_misalign_) {
|
||||
resync = true;
|
||||
}
|
||||
|
||||
if (resync) {
|
||||
// Reset sync times.
|
||||
sync_cpu_ = start_cpu;
|
||||
sync_sim_ = d->time;
|
||||
}
|
||||
|
||||
// Stepping loop.
|
||||
while (true) {
|
||||
const Clock::time_point now_cpu = Clock::now();
|
||||
elapsed_cpu = Seconds(now_cpu - sync_cpu_).count();
|
||||
elapsed_sim = d->time - sync_sim_;
|
||||
|
||||
// Stop stepping if simulation no longer lags cpu.
|
||||
if (elapsed_sim * slowdown >= elapsed_cpu) {
|
||||
return Status::kOk;
|
||||
}
|
||||
|
||||
// Stop stepping if simulation is taking too long to catch up.
|
||||
// Note: 12ms == 70% of 1/60 seconds/frame.
|
||||
constexpr Clock::duration kMaxCpuTimeForSim = std::chrono::milliseconds(12);
|
||||
if (now_cpu - start_cpu >= kMaxCpuTimeForSim) {
|
||||
// Note: GetSpeed() and GetSpeedMeasured() will be different in this case.
|
||||
return Status::kOk;
|
||||
}
|
||||
|
||||
// Measure slowdown here in first viable in-sync step. This update location
|
||||
// is chosen to minimize visual noise caused by changing measurements.
|
||||
if (elapsed_sim > 0) {
|
||||
double measured_slowdown = elapsed_cpu / elapsed_sim;
|
||||
speed_measured_ = 100. / measured_slowdown;
|
||||
}
|
||||
|
||||
mjtNum prev_time = d->time;
|
||||
InjectNoise(m, d);
|
||||
mj_step(m, d);
|
||||
|
||||
if (mjDISABLED(mjDSBL_AUTORESET)) {
|
||||
for (mjtWarning w : kDivergedWarnings) {
|
||||
// Stop stepping if the simulation diverged.
|
||||
if (d->warning[w].number > 0) {
|
||||
return Status::kDiverged;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Stop stepping if we auto reset.
|
||||
if (d->time < prev_time) {
|
||||
return Status::kAutoReset;
|
||||
}
|
||||
}
|
||||
|
||||
// Stop after one step if we resynced; next iteration will deal with timing.
|
||||
if (resync) {
|
||||
return Status::kOk;
|
||||
}
|
||||
}
|
||||
|
||||
return Status::kDiverged; // Unreachable
|
||||
}
|
||||
|
||||
void StepControl::InjectNoise(const mjModel* m, mjData* d) {
|
||||
// no noise, return
|
||||
if (ctrl_noise_std_ <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// convert rate and scale to discrete time (Ornstein–Uhlenbeck)
|
||||
mjtNum rate = mju_exp(-m->opt.timestep / ctrl_noise_rate_);
|
||||
mjtNum scale = ctrl_noise_std_ * mju_sqrt(1-rate*rate);
|
||||
|
||||
for (int i = 0; i < m->nu; i++) {
|
||||
mjtNum bottom = 0;
|
||||
mjtNum top = 0;
|
||||
mjtNum midpoint = 0;
|
||||
mjtNum halfrange = 1;
|
||||
if (m->actuator_ctrllimited[i]) {
|
||||
bottom = m->actuator_ctrlrange[2*i];
|
||||
top = m->actuator_ctrlrange[2*i+1];
|
||||
midpoint = 0.5 * (top + bottom); // target of exponential decay
|
||||
halfrange = 0.5 * (top - bottom); // scales noise
|
||||
}
|
||||
|
||||
// exponential convergence to midpoint at ctrl_noise_rate
|
||||
d->ctrl[i] = rate * d->ctrl[i] + (1-rate) * midpoint;
|
||||
|
||||
// add noise
|
||||
d->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr);
|
||||
|
||||
// clip to range if limited
|
||||
if (m->actuator_ctrllimited[i]) {
|
||||
d->ctrl[i] = mju_clip(d->ctrl[i], bottom, top);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,93 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_STEP_CONTROL_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_STEP_CONTROL_H_
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
using Seconds = std::chrono::duration<double>;
|
||||
using Clock = std::chrono::steady_clock;
|
||||
|
||||
// State and logic for physics synchronization and stepping.
|
||||
class StepControl {
|
||||
public:
|
||||
enum class Status {
|
||||
kOk,
|
||||
|
||||
// Simulation diverged with autoreset enabled.
|
||||
kAutoReset,
|
||||
|
||||
// Simulation diverged with autoreset disabled.
|
||||
// Note: Consider reporting mjData warning diagnostics in kDivergedWarnings.
|
||||
kDiverged,
|
||||
};
|
||||
|
||||
// List of warnings that are checked to determine simulation divergence.
|
||||
static constexpr mjtWarning kDivergedWarnings[] = {
|
||||
mjWARN_BADQACC, mjWARN_BADQVEL, mjWARN_BADQPOS};
|
||||
|
||||
// Steps physics forward, respecting speed settings and refresh budget.
|
||||
Status Advance(const mjModel* m, mjData* d);
|
||||
|
||||
// Ensures the next call to Advance() will synchronize time and step once.
|
||||
void ForceSync();
|
||||
|
||||
// Gets/sets the desired simulation speed as a percentage of real time.
|
||||
float GetSpeed() const;
|
||||
float GetSpeedMeasured() const;
|
||||
void SetSpeed(float speed); // speed is clamped to [0.1%, 100%]
|
||||
|
||||
// Gets/sets the control noise parameters applied before stepping.
|
||||
void GetNoiseParameters(float& noise_scale, float& noise_rate) const;
|
||||
void SetNoiseParameters(float noise_scale, float noise_rate);
|
||||
|
||||
private:
|
||||
std::string AdvanceOneStep(const mjModel* m, mjData* d);
|
||||
|
||||
void InjectNoise(const mjModel* m, mjData* d);
|
||||
|
||||
// Control noise standard deviation
|
||||
double ctrl_noise_std_ = 0;
|
||||
|
||||
// Control noise correlation rate
|
||||
double ctrl_noise_rate_ = 0;
|
||||
|
||||
// Desired simulation speed as a percentage of real time
|
||||
float speed_ = 100;
|
||||
|
||||
// Measured simulation speed as a percentage of real time
|
||||
float speed_measured_ = -1;
|
||||
|
||||
// If true, the next call to Advance() will synchronize time step once.
|
||||
bool force_sync_ = true;
|
||||
|
||||
// CPU time (aka wall time) of the last synchronization event
|
||||
std::chrono::time_point<Clock> sync_cpu_;
|
||||
|
||||
// Simulation time of the last synchronization event
|
||||
mjtNum sync_sim_ = 0;
|
||||
|
||||
// Maximum mis-alignment before re-sync (simulation seconds)
|
||||
double sync_misalign_ = .1;
|
||||
};
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_STEP_CONTROL_H_
|
||||
@@ -0,0 +1,168 @@
|
||||
// Copyright 2025 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 "experimental/toolbox/window.h"
|
||||
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
|
||||
#include "third_party/SDL2/include/SDL.h"
|
||||
#include "third_party/SDL2/include/SDL_error.h"
|
||||
#include "third_party/SDL2/include/SDL_events.h"
|
||||
#include "third_party/SDL2/include/SDL_hints.h"
|
||||
#include "third_party/SDL2/include/SDL_syswm.h"
|
||||
#include "third_party/SDL2/include/SDL_version.h"
|
||||
#include "third_party/SDL2/include/SDL_video.h"
|
||||
#include "third_party/dear_imgui/backends/imgui_impl_sdl2.h"
|
||||
#include "third_party/dear_imgui/imgui.h"
|
||||
#include "third_party/implot/implot.h"
|
||||
#include "experimental/toolbox/helpers.h"
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
static void InitImGui(SDL_Window* window, const LoadAssetFn& load_asset_fn) {
|
||||
ImGui::CreateContext();
|
||||
ImPlot::CreateContext();
|
||||
|
||||
ImGuiIO& io = ImGui::GetIO();
|
||||
io.ConfigFlags |= ImGuiConfigFlags_DockingEnable;
|
||||
io.IniFilename = nullptr;
|
||||
ImGui::StyleColorsDark();
|
||||
ImGui_ImplSDL2_InitForOther(window);
|
||||
|
||||
#ifndef __EMSCRIPTEN__
|
||||
// TODO: Get font loading working for wasm.
|
||||
ImFontConfig main_cfg;
|
||||
main_cfg.OversampleH = 8;
|
||||
main_cfg.OversampleV = 4;
|
||||
main_cfg.GlyphExtraSpacing.x = 0.3f;
|
||||
|
||||
auto main_font = load_asset_fn("Roboto-Regular.ttf");
|
||||
io.Fonts->AddFontFromMemoryTTF(main_font.data(), main_font.size(), 18.f,
|
||||
&main_cfg);
|
||||
|
||||
ImFontConfig icon_cfg;
|
||||
icon_cfg.OversampleH = 3;
|
||||
icon_cfg.OversampleV = 3;
|
||||
icon_cfg.MergeMode = true;
|
||||
icon_cfg.GlyphMinAdvanceX = 14.0f;
|
||||
auto icon_font = load_asset_fn("fontawesome-webfont.ttf");
|
||||
constexpr ImWchar icon_ranges[] = {0xf000, 0xf3ff, 0x000};
|
||||
io.Fonts->AddFontFromMemoryTTF(icon_font.data(), icon_font.size(), 14.f,
|
||||
&icon_cfg, icon_ranges);
|
||||
#endif
|
||||
io.Fonts->Build();
|
||||
}
|
||||
|
||||
Window::Window(std::string_view title, int width, int height, Config config,
|
||||
const LoadAssetFn& load_asset_fn)
|
||||
: width_(width), height_(height), config_(config) {
|
||||
SDL_SetHint(SDL_HINT_FRAMEBUFFER_ACCELERATION, "1");
|
||||
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLEBUFFERS, 1);
|
||||
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLESAMPLES, 16);
|
||||
if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_EVENTS) != 0) {
|
||||
mju_error("Error initializing SDL: %s", SDL_GetError());
|
||||
}
|
||||
|
||||
int window_flags = SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI;
|
||||
|
||||
if (config == kFilamentVulkan) {
|
||||
window_flags |= SDL_WINDOW_VULKAN;
|
||||
} else if (config == kFilamentWebGL) {
|
||||
window_flags |= SDL_WINDOW_OPENGL;
|
||||
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
|
||||
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 0);
|
||||
SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_ES);
|
||||
} else if (config == kMujocoOpenGL || config == kFilamentOpenGL) {
|
||||
window_flags |= SDL_WINDOW_OPENGL;
|
||||
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
|
||||
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 0);
|
||||
} else {
|
||||
mju_error("Unsupported window config: %d", config);
|
||||
}
|
||||
|
||||
sdl_window_ =
|
||||
SDL_CreateWindow(title.data(), SDL_WINDOWPOS_UNDEFINED,
|
||||
SDL_WINDOWPOS_UNDEFINED, width, height, window_flags);
|
||||
if (!sdl_window_) {
|
||||
mju_error("Error creating window: %s", SDL_GetError());
|
||||
}
|
||||
|
||||
InitImGui(sdl_window_, load_asset_fn);
|
||||
|
||||
if (config == kFilamentWebGL || config == kMujocoOpenGL) {
|
||||
SDL_GLContext gl_context = SDL_GL_CreateContext(sdl_window_);
|
||||
SDL_GL_MakeCurrent(sdl_window_, gl_context);
|
||||
}
|
||||
|
||||
#ifdef __linux__
|
||||
SDL_SysWMinfo wmi;
|
||||
SDL_VERSION(&wmi.version);
|
||||
SDL_GetWindowWMInfo(sdl_window_, &wmi);
|
||||
native_window_ = reinterpret_cast<void*>(wmi.info.x11.window);
|
||||
#endif
|
||||
}
|
||||
|
||||
Window::~Window() {
|
||||
SDL_DestroyWindow(sdl_window_);
|
||||
SDL_Quit();
|
||||
}
|
||||
|
||||
void Window::SetTitle(std::string_view title) {
|
||||
SDL_SetWindowTitle(sdl_window_, title.data());
|
||||
}
|
||||
|
||||
std::string Window::GetDropFile() {
|
||||
std::string tmp;
|
||||
std::swap(tmp, drop_file_);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
Window::Status Window::ProcessEvents() {
|
||||
SDL_Event event;
|
||||
while (SDL_PollEvent(&event)) {
|
||||
ImGui_ImplSDL2_ProcessEvent(&event);
|
||||
|
||||
if (event.type == SDL_QUIT) {
|
||||
should_exit_ = true;
|
||||
} else if (event.type == SDL_APP_WILLENTERBACKGROUND) {
|
||||
should_exit_ = true;
|
||||
} else if (event.type == SDL_WINDOWEVENT) {
|
||||
if (event.window.event == SDL_WINDOWEVENT_RESIZED) {
|
||||
SDL_GetWindowSize(sdl_window_, &width_, &height_);
|
||||
int drawable_width = width_;
|
||||
int drawable_height = height_;
|
||||
SDL_GL_GetDrawableSize(sdl_window_, &drawable_width, &drawable_height);
|
||||
scale_ = (float)drawable_width / (float)width_;
|
||||
}
|
||||
} else if (event.type == SDL_DROPFILE) {
|
||||
drop_file_ = event.drop.file;
|
||||
}
|
||||
}
|
||||
|
||||
ImGui_ImplSDL2_NewFrame();
|
||||
ImGui::NewFrame();
|
||||
|
||||
return should_exit_ ? kQuitting : kRunning;
|
||||
}
|
||||
|
||||
void Window::Present() {
|
||||
// Filament (with the exception of WebGL) handles the swapchain internally.
|
||||
if (config_ != kFilamentVulkan && config_ != kFilamentOpenGL) {
|
||||
SDL_GL_SwapWindow(sdl_window_);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
@@ -0,0 +1,87 @@
|
||||
// Copyright 2025 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.
|
||||
|
||||
#ifndef MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_WINDOW_H_
|
||||
#define MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_WINDOW_H_
|
||||
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
|
||||
#include "experimental/toolbox/helpers.h"
|
||||
#include "third_party/SDL2/include/SDL_video.h"
|
||||
|
||||
namespace mujoco::toolbox {
|
||||
|
||||
// Platform-independent window abstraction using SDL2.
|
||||
//
|
||||
// Initializes the SDL2 and ImGui libraries, creates/owns the native window, and
|
||||
// handles events from the window.
|
||||
class Window {
|
||||
public:
|
||||
// Configures the window for the specified rendering backend.
|
||||
enum Config {
|
||||
kMujocoOpenGL,
|
||||
kFilamentVulkan,
|
||||
kFilamentOpenGL,
|
||||
kFilamentWebGL,
|
||||
};
|
||||
|
||||
Window(std::string_view title, int width, int height, Config config,
|
||||
const LoadAssetFn& load_asset_fn);
|
||||
~Window();
|
||||
|
||||
Window(const Window&) = delete;
|
||||
Window& operator=(const Window&) = delete;
|
||||
|
||||
// The status of the window.
|
||||
enum Status {
|
||||
kRunning,
|
||||
kQuitting,
|
||||
};
|
||||
|
||||
// Processes all pendings window events, returning the status of the window.
|
||||
Status ProcessEvents();
|
||||
|
||||
// Swaps and presents the window buffer.
|
||||
void Present();
|
||||
|
||||
// Sets the title of the window.
|
||||
void SetTitle(std::string_view title);
|
||||
|
||||
// Returns the current size of the window.
|
||||
int GetWidth() const { return width_; }
|
||||
int GetHeight() const { return height_; }
|
||||
float GetScale() const { return scale_; }
|
||||
|
||||
// Returns the path to a file that was dropped on the window. Once called,
|
||||
// the value will be cleared until the next time a file is dropped.
|
||||
std::string GetDropFile();
|
||||
|
||||
// Returns the handle to the underlying native window.
|
||||
void* GetNativeWindowHandle() { return native_window_; }
|
||||
|
||||
private:
|
||||
int width_ = 0;
|
||||
int height_ = 0;
|
||||
float scale_ = 1.0f;
|
||||
Config config_;
|
||||
void* native_window_ = nullptr;
|
||||
SDL_Window* sdl_window_ = nullptr;
|
||||
bool should_exit_ = false;
|
||||
std::string drop_file_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::toolbox
|
||||
|
||||
#endif // MUJOCO_SRC_EXPERIMENTAL_TOOLBOX_WINDOW_H_
|
||||
Reference in New Issue
Block a user