SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(mujoco): split world and viewer runtime

Browse Source
This commit is contained in:
lgv 2026-06-30 16:03:00 +08:00
parent 922f73adb5
commit 25d44d5fec
24 changed files with 1673 additions and 7103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt Normal file
View File

@ -0,0 +1,7 @@
viewers {
id: "mujoco_viewer"
world_id: "mujoco_world"
camera_distance: 3.0
camera_azimuth: 0.0
camera_elevation: -30.0
}

7
cmvr-es/config/devices/mujoco/mujoco_world.pb.txt Normal file
View File

@ -0,0 +1,7 @@
worlds {
id: "mujoco_world"
model_path: "model/xiaoyan_description/dual_arm.xml"
timestep_s: 0.001
realtime_factor: 1.0
require_actuator: true
}

1
cmvr-es/simulate/mujoco/CMakeLists.txt
View File

@ -1 +1,2 @@
add_subdirectory(mujoco_world)
add_subdirectory(mujoco_viewer)

42
cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt
View File

@ -1,14 +1,15 @@
file(GLOB SRC
${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/*.cc
add_library(mujoco_viewer SHARED
src/mujoco_viewer.cpp
)
add_library(mujoco_viewer SHARED ${SRC})
target_include_directories(mujoco_viewer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(mujoco_viewer PUBLIC
cmvr_es::proto
cmvr_es::common
cmvr_es::mujoco_world
simulate
mujoco
glfw
GL
@ -24,25 +25,14 @@ install(TARGETS mujoco_viewer LIBRARY DESTINATION lib)
## --------------------------------------------------------
## Unit test
## --------------------------------------------------------
#find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
#find_package(Python3 COMPONENTS NumPy)
#
#
#add_executable(mujoco_viewer_test
# ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer_test.cpp
#)
#
#
#target_link_libraries(mujoco_viewer_test
# PRIVATE
# gtest
# gtest_main
# pthread
# glog
# cmvr_es::mujoco_viewer
# cmvr_es::algorithms::controller
# Python3::Python
# Python3::NumPy
#)
add_executable(mujoco_viewer_test
${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer_test.cpp
)
target_link_libraries(mujoco_viewer_test
PRIVATE
gtest
gtest_main
cmvr_es::mujoco_viewer
cmvr_es::mujoco_world
)

105
cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h
View File

@ -1,105 +0,0 @@
// 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.
#ifndef MUJOCO_SAMPLE_ARRAY_SAFETY_H_
#define MUJOCO_SAMPLE_ARRAY_SAFETY_H_
#include <algorithm>
#include <cstdarg>
#include <cstddef>
#include <cstdio>
#include <cstring>
// Provides safe alternatives to the sizeof() operator and standard library functions for handling
// null-terminated (C-style) strings in raw char arrays.
//
// These functions make use of compile-time array sizes to limit read and write operations to within
// the array bounds. They are designed to trigger a compile error if the array size cannot be
// determined at compile time (e.g. when an array has decayed into a pointer).
//
// They do not perform runtime bound checks.
namespace mujoco {
namespace sample_util {
// returns sizeof(arr)
// use instead of sizeof() to avoid unintended array-to-pointer decay
template <typename T, int N>
static constexpr std::size_t sizeof_arr(const T(&arr)[N]) {
return sizeof(arr);
}
// like std::strcmp but it will not read beyond the bound of either lhs or rhs
template <std::size_t N1, std::size_t N2>
static inline int strcmp_arr(const char (&lhs)[N1], const char (&rhs)[N2]) {
return std::strncmp(lhs, rhs, std::min(N1, N2));
}
// like std::strlen but it will not read beyond the bound of str
// if str is not null-terminated, returns sizeof(str)
template <std::size_t N>
static inline std::size_t strlen_arr(const char (&str)[N]) {
for (std::size_t i = 0; i < N; ++i) {
if (str[i] == '\0') {
return i;
}
}
return N;
}
// like std::sprintf but will not write beyond the bound of dest
// dest is guaranteed to be null-terminated
template <std::size_t N>
static inline int sprintf_arr(char (&dest)[N], const char* format, ...) {
std::va_list vargs;
va_start(vargs, format);
int retval = std::vsnprintf(dest, N, format, vargs);
va_end(vargs);
return retval;
}
// like std::strcat but will not write beyond the bound of dest
// dest is guaranteed to be null-terminated
template <std::size_t N>
static inline char* strcat_arr(char (&dest)[N], const char* src) {
const std::size_t dest_len = strlen_arr(dest);
const std::size_t dest_size = sizeof_arr(dest);
for (std::size_t i = dest_len; i < dest_size; ++i) {
dest[i] = src[i - dest_len];
if (!dest[i]) {
break;
}
}
dest[dest_size - 1] = '\0';
return dest;
}
// like std::strcpy but won't write beyond the bound of dest
// dest is guaranteed to be null-terminated
template <std::size_t N>
static inline char* strcpy_arr(char (&dest)[N], const char* src) {
{
std::size_t i = 0;
for (; src[i] && i < N - 1; ++i) {
dest[i] = src[i];
}
dest[i] = '\0';
}
return &dest[0];
}
} // namespace sample_util
} // namespace mujoco
#endif // MUJOCO_SAMPLE_ARRAY_SAFETY_H_

78
cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h
View File

@ -1,78 +0,0 @@
// Copyright 2023 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_SIMULATE_GLFW_ADAPTER_H_
#define MUJOCO_SIMULATE_GLFW_ADAPTER_H_
#include <utility>
#include <GLFW/glfw3.h>
#include <mujoco/mujoco.h>
#include "platform_ui_adapter.h"
#ifdef __APPLE__
#include <optional>
#include "glfw_corevideo.h"
#endif
namespace mujoco {
class GlfwAdapter : public PlatformUIAdapter {
public:
GlfwAdapter();
~GlfwAdapter() override;
std::pair<double, double> GetCursorPosition() const override;
double GetDisplayPixelsPerInch() const override;
std::pair<int, int> GetFramebufferSize() const override;
std::pair<int, int> GetWindowSize() const override;
bool IsGPUAccelerated() const override;
void PollEvents() override;
void SetClipboardString(const char* text) override;
void SetVSync(bool enabled) override;
void SetWindowTitle(const char* title) override;
bool ShouldCloseWindow() const override;
void SwapBuffers() override;
void ToggleFullscreen() override;
bool IsLeftMouseButtonPressed() const override;
bool IsMiddleMouseButtonPressed() const override;
bool IsRightMouseButtonPressed() const override;
bool IsAltKeyPressed() const override;
bool IsCtrlKeyPressed() const override;
bool IsShiftKeyPressed() const override;
bool IsMouseButtonDownEvent(int act) const override;
bool IsKeyDownEvent(int act) const override;
int TranslateKeyCode(int key) const override;
mjtButton TranslateMouseButton(int button) const override;
private:
GLFWvidmode vidmode_;
GLFWwindow* window_;
// store last window information when going to full screen
std::pair<int, int> window_pos_;
std::pair<int, int> window_size_;
#ifdef __APPLE__
// Workaround for perpertually broken OpenGL VSync on macOS,
// most recently https://github.com/glfw/glfw/issues/2249.
std::optional<GlfwCoreVideo> core_video_;
#endif
};
} // namespace mujoco
#endif // MUJOCO_SIMULATE_GLFW_ADAPTER_H_

57
cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h
View File

@ -1,57 +0,0 @@
// Copyright 2023 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_SIMULATE_GLFW_COREVIDEO_H_
#define MUJOCO_SIMULATE_GLFW_COREVIDEO_H_
#ifndef __APPLE__
#error "This header only works on macOS."
#endif
#include <atomic>
#include <condition_variable>
#include <mutex>
#include "glfw_dispatch.h"
#ifdef __OBJC__
#import <CoreVideo/CoreVideo.h>
#else
typedef void* CVDisplayLinkRef;
#endif
// Workaround for perpertually broken OpenGL VSync on macOS,
// most recently https://github.com/glfw/glfw/issues/2249.
namespace mujoco {
class GlfwCoreVideo {
public:
GlfwCoreVideo(GLFWwindow* window);
~GlfwCoreVideo();
void WaitForDisplayRefresh();
int DisplayLinkCallback();
void UpdateDisplayLink();
private:
GLFWwindow* window_;
CVDisplayLinkRef display_link_;
std::atomic_bool waiting_;
std::mutex mu_;
std::condition_variable cond_;
};
} // namespace mujoco
#endif // MUJOCO_SIMULATE_GLFW_COREVIDEO_H_

77
cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h
View File

@ -1,77 +0,0 @@
// Copyright 2022 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_SIMULATE_GLFW_DISPATCH_H_
#define MUJOCO_SIMULATE_GLFW_DISPATCH_H_
#include <GLFW/glfw3.h>
#ifdef __APPLE__
#define GLFW_EXPOSE_NATIVE_NSGL
#include <GLFW/glfw3native.h>
#endif
namespace mujoco {
// Dynamic dispatch table for GLFW functions required by Simulate.
// This allows us to use GLFW without introducing a link-time dependency on the
// library, which is useful e.g. when using GLFW via Python.
struct Glfw {
#define mjGLFW_DECLARE_SYMBOL(func) decltype(&::func) func
// go/keep-sorted start
mjGLFW_DECLARE_SYMBOL(glfwCreateWindow);
mjGLFW_DECLARE_SYMBOL(glfwDestroyWindow);
mjGLFW_DECLARE_SYMBOL(glfwGetCursorPos);
mjGLFW_DECLARE_SYMBOL(glfwGetFramebufferSize);
mjGLFW_DECLARE_SYMBOL(glfwGetKey);
mjGLFW_DECLARE_SYMBOL(glfwGetMonitorPhysicalSize);
mjGLFW_DECLARE_SYMBOL(glfwGetMouseButton);
mjGLFW_DECLARE_SYMBOL(glfwGetPrimaryMonitor);
mjGLFW_DECLARE_SYMBOL(glfwGetTime);
mjGLFW_DECLARE_SYMBOL(glfwGetVideoMode);
mjGLFW_DECLARE_SYMBOL(glfwGetWindowMonitor);
mjGLFW_DECLARE_SYMBOL(glfwGetWindowPos);
mjGLFW_DECLARE_SYMBOL(glfwGetWindowSize);
mjGLFW_DECLARE_SYMBOL(glfwGetWindowUserPointer);
mjGLFW_DECLARE_SYMBOL(glfwInit);
mjGLFW_DECLARE_SYMBOL(glfwMakeContextCurrent);
mjGLFW_DECLARE_SYMBOL(glfwPollEvents);
mjGLFW_DECLARE_SYMBOL(glfwSetClipboardString);
mjGLFW_DECLARE_SYMBOL(glfwSetCursorPosCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetDropCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetKeyCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetMouseButtonCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetScrollCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetWindowMonitor);
mjGLFW_DECLARE_SYMBOL(glfwSetWindowRefreshCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetWindowSizeCallback);
mjGLFW_DECLARE_SYMBOL(glfwSetWindowTitle);
mjGLFW_DECLARE_SYMBOL(glfwSetWindowUserPointer);
mjGLFW_DECLARE_SYMBOL(glfwSwapBuffers);
mjGLFW_DECLARE_SYMBOL(glfwSwapInterval);
mjGLFW_DECLARE_SYMBOL(glfwTerminate);
mjGLFW_DECLARE_SYMBOL(glfwWindowHint);
mjGLFW_DECLARE_SYMBOL(glfwWindowShouldClose);
// go/keep-sorted end
#ifdef __APPLE__
mjGLFW_DECLARE_SYMBOL(glfwGetNSGLContext);
#endif
#undef mjGLFW_DECLARE_SYMBOL
};
const struct Glfw& Glfw(void* dlhandle = nullptr);
} // namespace mujoco
#endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_

2173
cmvr-es/simulate/mujoco/mujoco_viewer/include/lodepng.h
View File

File diff suppressed because it is too large Load Diff

94
cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h
View File

@ -10,16 +10,19 @@
#include <thread>
#include <vector>
#include "cmvr/config/camera_config/camera_config.pb.h"
#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h"
#include "devices/abstract_device.h"
#include "mujoco/mujoco.h"
#include "simulate/mujoco/mujoco_viewer/include/simulate.h"
#include "common/base/constants.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
#include <simulate/simulate.h>
namespace cmvr {
class PiPGlfwAdapter;
class MuJocoViewer {
friend class PiPGlfwAdapter;
public:
explicit MuJocoViewer(const char *model_path);
explicit MuJocoViewer(std::shared_ptr<simulate::MujocoWorld> world);
~MuJocoViewer();
@ -32,8 +35,8 @@ namespace cmvr {
void setRunning(bool running);
void requestStop();
mjModel *model() const { return m_; }
mjData *data() const { return d_; }
mjModel *model() const;
mjData *data() const;
// 同一窗口画中画：显示模型内固定相机视角（像素坐标）
void enablePiPCamera(const char *camera_name);
@ -42,6 +45,13 @@ namespace cmvr {
int bottom,
int width,
int height);
void enablePiPCamera(const char *camera_name,
int left,
int bottom,
int display_width,
int display_height,
int render_width,
int render_height);
void disablePiPCamera();
// 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
// depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
@ -54,22 +64,7 @@ namespace cmvr {
// 只拿 frame_id，便于 physics 线程判断是否新帧
uint64_t getPiPCameraFrameId() const;
protected:
// 每次 mj_step 前，physics 线程回调控制逻辑
virtual void controlCallback(mjModel *m, mjData *d) {
UNUSED_VARIABLE(m, d);
}
// 点击reset 会调用这个函数
virtual void onReset(mjModel *m, mjData *d) {
UNUSED_VARIABLE(m, d);
}
// 只在第一次进控制循环时调用一次
virtual void initOnce(mjModel *m, mjData *d) {
UNUSED_VARIABLE(m, d);
}
public:
void setupCamera(double distance = 3.0,
double azimuth = 0.0,
double elevation = -30.0); // 设置相机视角参数
@ -85,39 +80,27 @@ namespace cmvr {
private:
void renderPiP();
void initSim(); // 只创建 Simulate，不 load
void physicsThreadFunc(); // 加载模型 + 物理循环
void physicsLoop(); // 真正的一步一步仿真
void controlDispatch(mjModel *m, mjData *d);
static mjModel *LoadModelSimple(const char *file,
char *load_error,
int error_sz);
void initSim();
void syncThreadFunc();
private:
std::string model_path_;
mjModel *m_ = nullptr;
mjData *d_ = nullptr;
std::shared_ptr<simulate::MujocoWorld> world_;
mjvCamera cam_;
mjvOption opt_;
mjvPerturb pert_;
std::unique_ptr<mujoco::Simulate> sim_;
std::thread physics_thread_;
std::atomic<bool> physics_started_{false};
bool inited_ = false;
mjtNum last_time_ = 0.0;
std::thread sync_thread_;
bool pip_enabled_ = false;
std::string pip_camera_name_;
int pip_camera_id_ = -1;
int pip_width_ = 320;
int pip_height_ = 240;
int pip_render_width_ = 320;
int pip_render_height_ = 240;
bool pip_render_size_warning_logged_ = false;
int pip_margin_ = 10;
bool pip_custom_pos_ = false;
int pip_left_ = 0;
@ -135,4 +118,35 @@ namespace cmvr {
uint64_t pip_frame_id_ = 0; // 新增：帧序号
};
class MujocoViewerDevice final : public device::AbstractDevice {
public:
explicit MujocoViewerDevice(config::MujocoViewerConfig config);
~MujocoViewerDevice() override;
device::DeviceKind kind() const noexcept override { return device::DeviceKind::MujocoViewer; }
std::string typeName() const override { return "MujocoViewerDevice"; }
bool init() override;
bool start() override;
bool stop() override;
bool runOnMainThread();
bool setPiPCameraConfig(const config::MujocoCameraConfig& camera_config);
bool getPiPCameraRGBD(std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) const;
private:
config::MujocoViewerConfig config_;
config::MujocoCameraConfig pip_camera_config_;
std::shared_ptr<simulate::MujocoWorld> world_;
std::unique_ptr<MuJocoViewer> viewer_;
std::thread viewer_thread_;
mutable std::mutex mtx_;
bool has_pip_camera_config_ = false;
bool running_ = false;
bool stop_requested_ = false;
};
} // namespace cmvr

101
cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h
View File

@ -1,101 +0,0 @@
// Copyright 2023 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_SIMULATE_PLATFORM_UI_ADAPTER_H_
#define MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
#include <utility>
#include <mujoco/mujoco.h>
namespace mujoco {
class PlatformUIAdapter {
public:
virtual ~PlatformUIAdapter() = default;
inline mjuiState& state() { return state_; }
inline const mjuiState& state() const { return state_; }
inline mjrContext& mjr_context() { return con_; }
inline const mjrContext& mjr_context() const { return con_; }
inline void SetEventCallback(void (*event_callback)(mjuiState*)) {
event_callback_ = event_callback;
}
inline void SetLayoutCallback(void (*layout_callback)(mjuiState*)) {
layout_callback_ = layout_callback;
}
// Optionally overridable function to (re)create an mjrContext for an mjModel
virtual bool RefreshMjrContext(const mjModel* m, int fontscale);
virtual bool EnsureContextSize();
// Pure virtual functions to be implemented by individual adapters
virtual std::pair<double, double> GetCursorPosition() const = 0;
virtual double GetDisplayPixelsPerInch() const = 0;
virtual std::pair<int, int> GetFramebufferSize() const = 0;
virtual std::pair<int, int> GetWindowSize() const = 0;
virtual bool IsGPUAccelerated() const = 0;
virtual void PollEvents() = 0;
virtual void SetClipboardString(const char* text) = 0;
virtual void SetVSync(bool enabled) = 0;
virtual void SetWindowTitle(const char* title) = 0;
virtual bool ShouldCloseWindow() const = 0;
virtual void SwapBuffers() = 0;
virtual void ToggleFullscreen() = 0;
virtual bool IsLeftMouseButtonPressed() const = 0;
virtual bool IsMiddleMouseButtonPressed() const = 0;
virtual bool IsRightMouseButtonPressed() const = 0;
virtual bool IsAltKeyPressed() const = 0;
virtual bool IsCtrlKeyPressed() const = 0;
virtual bool IsShiftKeyPressed() const = 0;
virtual bool IsMouseButtonDownEvent(int act) const = 0;
virtual bool IsKeyDownEvent(int act) const = 0;
virtual int TranslateKeyCode(int key) const = 0;
virtual mjtButton TranslateMouseButton(int button) const = 0;
protected:
PlatformUIAdapter();
void FreeMjrContext();
// Event handlers
void OnFilesDrop(int count, const char** paths);
virtual void OnKey(int key, int scancode, int act);
void OnMouseButton(int button, int act);
void OnMouseMove(double x, double y);
void OnScroll(double xoffset, double yoffset);
void OnWindowRefresh();
void OnWindowResize(int width, int height);
mjuiState state_;
int last_key_;
void (*event_callback_)(mjuiState*);
void (*layout_callback_)(mjuiState*);
mjrContext con_;
const mjModel* last_model_ = nullptr;
int last_fontscale_ = -1;
private:
void UpdateMjuiState();
};
} // namespace mujoco
#endif // MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_

353
cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h
View File

@ -1,353 +0,0 @@
// 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.
#ifndef MUJOCO_SIMULATE_SIMULATE_H_
#define MUJOCO_SIMULATE_SIMULATE_H_
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <optional>
#include <ratio>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#include <mujoco/mjui.h>
#include <mujoco/mujoco.h>
#include "platform_ui_adapter.h"
namespace mujoco {
// The viewer itself doesn't require a reentrant mutex, however we use it in
// order to provide a Python sync API that doesn't require separate locking
// (since sync is by far the most common operation), but that also won't
// deadlock if called when a lock is already held by the user script on the
// same thread.
class SimulateMutex : public std::recursive_mutex {};
using MutexLock = std::unique_lock<std::recursive_mutex>;
// Simulate states not contained in MuJoCo structures
class Simulate {
public:
using Clock = std::chrono::steady_clock;
static_assert(std::ratio_less_equal_v<Clock::period, std::milli>);
static constexpr int kMaxGeom = 100000;
// create object and initialize the simulate ui
Simulate(
std::unique_ptr<PlatformUIAdapter> platform_ui_adapter,
mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, bool is_passive);
// Synchronize state with UI inputs, and update visualization. If state_only
// is false mjData and mjModel will be updated, otherwise only the subset of
// mjData corresponding to mjSTATE_INTEGRATION will be synced.
void Sync(bool state_only = false);
void UpdateHField(int hfieldid);
void UpdateMesh(int meshid);
void UpdateTexture(int texid);
// Request that the Simulate UI display a "loading" message
// Called prior to Load or LoadMessageClear
void LoadMessage(const char* displayed_filename);
// Request that the Simulate UI thread render a new model
void Load(mjModel* m, mjData* d, const char* displayed_filename);
// Clear the loading message
// Can be called instead of Load to clear the message without
// requesting the UI load a model
void LoadMessageClear(void);
// functions below are used by the renderthread
// load mjb or xml model that has been requested by load()
void LoadOnRenderThread();
// render the ui to the window
void Render();
// loop to render the UI (must be called from main thread because of MacOS)
void RenderLoop();
// add state to history buffer
void AddToHistory();
// inject control noise
void InjectNoise(int key);
// constants
static constexpr int kMaxFilenameLength = 1000;
// whether the viewer is operating in passive mode, where it cannot assume
// that it has exclusive access to mjModel, mjData, and various mjv objects
bool is_passive_ = false;
// model and data to be visualized
mjModel* mnew_ = nullptr;
mjData* dnew_ = nullptr;
mjModel* m_ = nullptr;
mjData* d_ = nullptr;
int ncam_ = 0;
int nkey_ = 0;
int state_size_ = 0; // number of mjtNums in a history buffer state
int nhistory_ = 0; // number of states saved in history buffer
int history_cursor_ = 0; // cursor pointing at last saved state
std::vector<int> body_parentid_;
std::vector<int> jnt_type_;
std::vector<int> jnt_group_;
std::vector<int> jnt_qposadr_;
std::vector<std::optional<std::pair<mjtNum, mjtNum>>> jnt_range_;
std::vector<std::string> jnt_names_;
std::vector<int> actuator_group_;
std::vector<std::optional<std::pair<mjtNum, mjtNum>>> actuator_ctrlrange_;
std::vector<std::string> actuator_names_;
std::vector<std::string> equality_names_;
std::vector<mjtNum> history_; // history buffer (nhistory x state_size)
// mjModel and mjData fields that can be modified by the user through the GUI
std::vector<mjtNum> qpos_;
std::vector<mjtNum> qpos_prev_;
std::vector<mjtNum> ctrl_;
std::vector<mjtNum> ctrl_prev_;
std::vector<mjtByte> eq_active_;
std::vector<mjtByte> eq_active_prev_;
// in passive mode the user owns m_ and d_, these "passive" instances are
// owned by Simulate, updated from the user by the Sync() method
mjModel* m_passive_ = nullptr;
mjData* d_passive_ = nullptr;
std::vector<mjvGeom> user_scn_geoms_;
mjOption mjopt_prev_;
mjVisual mjvis_prev_;
mjStatistic mjstat_prev_;
mjvOption opt_prev_;
mjvCamera cam_prev_;
int warn_vgeomfull_prev_;
// pending GUI-driven actions, to be applied at the next call to Sync
struct {
std::optional<std::string> save_xml;
std::optional<std::string> save_mjb;
std::optional<std::string> print_model;
std::optional<std::string> print_data;
bool reset;
bool align;
bool copy_key;
bool copy_key_full_precision;
bool load_from_history;
bool load_key;
bool save_key;
bool zero_ctrl;
int newperturb;
bool select;
mjuiState select_state;
bool ui_update_simulation;
bool ui_update_physics;
bool ui_update_rendering;
bool ui_update_visualization;
bool ui_update_joint;
bool ui_update_ctrl;
bool ui_update_equality;
bool ui_remake_ctrl;
} pending_ = {};
SimulateMutex mtx;
std::condition_variable_any cond_loadrequest;
int frames_ = 0;
std::chrono::time_point<Clock> last_fps_update_;
double fps_ = 0;
// options
int spacing = 0;
int color = 0;
int font = 0;
int ui0_enable = 1;
int ui1_enable = 1;
int help = 0;
int info = 0;
int profiler = 0;
int sensor = 0;
int pause_update = 0;
int fullscreen = 0;
int vsync = 1;
int busywait = 0;
// keyframe index
int key = -1;
// index of history-scrubber slider
int scrub_index = 0;
// simulation
int run = 1;
// atomics for cross-thread messages
std::atomic_int exitrequest = 0;
std::atomic_int droploadrequest = 0;
std::atomic_int screenshotrequest = 0;
std::atomic_int uiloadrequest = 0;
std::atomic_int newfigurerequest = 0;
std::atomic_int newtextrequest = 0;
std::atomic_int newimagerequest = 0;
// loadrequest
// 3: display a loading message
// 2: render thread asked to update its model
// 1: showing "loading" label, about to load
// 0: model loaded or no load requested.
int loadrequest = 0;
// strings
char load_error[kMaxFilenameLength] = "";
char dropfilename[kMaxFilenameLength] = "";
char filename[kMaxFilenameLength] = "";
char previous_filename[kMaxFilenameLength] = "";
// time synchronization
int real_time_index = 0;
bool speed_changed = true;
float measured_slowdown = 1.0;
// logarithmically spaced real-time slow-down coefficients (percent)
static constexpr float percentRealTime[] = {
100, 80, 66, 50, 40, 33, 25, 20, 16, 13,
10, 8, 6.6, 5.0, 4, 3.3, 2.5, 2, 1.6, 1.3,
1, .8, .66, .5, .4, .33, .25, .2, .16, .13,
.1
};
// control noise
double ctrl_noise_std = 0.0;
double ctrl_noise_rate = 0.0;
// watch
char field[mjMAXUITEXT] = "qpos";
int index = 0;
// physics: need sync
int disable[mjNDISABLE] = {0};
int enable[mjNENABLE] = {0};
int enableactuator[mjNGROUP] = {0};
// rendering: need sync
int camera = 0;
// abstract visualization
mjvScene scn;
mjvCamera& cam;
mjvOption& opt;
mjvPerturb& pert;
mjvFigure figconstraint = {};
mjvFigure figcost = {};
mjvFigure figtimer = {};
mjvFigure figsize = {};
mjvFigure figsensor = {};
// additional user-defined visualization
mjvScene* user_scn = nullptr;
mjtByte user_scn_flags_prev_[mjNRNDFLAG];
std::vector<std::pair<mjrRect, mjvFigure>> user_figures_;
std::vector<std::pair<mjrRect, mjvFigure>> user_figures_new_;
std::vector<std::tuple<int, int, std::string, std::string>> user_texts_;
std::vector<std::tuple<int, int, std::string, std::string>> user_texts_new_;
std::vector<std::tuple<mjrRect, std::unique_ptr<unsigned char[]>>> user_images_;
std::vector<std::tuple<mjrRect, std::unique_ptr<unsigned char[]>>> user_images_new_;
// OpenGL rendering and UI
int refresh_rate = 60;
int window_pos[2] = {0};
int window_size[2] = {0};
std::unique_ptr<PlatformUIAdapter> platform_ui;
mjuiState& uistate;
mjUI ui0 = {};
mjUI ui1 = {};
// Constant arrays needed for the option section of UI and the UI interface
// TODO setting the size here is not ideal
const mjuiDef def_option[13] = {
{mjITEM_SECTION, "Option", mjPRESERVE, nullptr, "AO"},
{mjITEM_CHECKINT, "Help", 2, &this->help, " #290"},
{mjITEM_CHECKINT, "Info", 2, &this->info, " #291"},
{mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"},
{mjITEM_CHECKINT, "Sensor", 2, &this->sensor, " #293"},
{mjITEM_CHECKINT, "Pause update", 2, &this->pause_update, ""},
#ifdef __APPLE__
{mjITEM_CHECKINT, "Fullscreen", 0, &this->fullscreen, " #294"},
#else
{mjITEM_CHECKINT, "Fullscreen", 1, &this->fullscreen, " #294"},
#endif
{mjITEM_CHECKINT, "Vertical Sync", 1, &this->vsync, ""},
{mjITEM_CHECKINT, "Busy Wait", 1, &this->busywait, ""},
{mjITEM_SELECT, "Spacing", 1, &this->spacing, "Tight\nWide"},
{mjITEM_SELECT, "Color", 1, &this->color, "Default\nOrange\nWhite\nBlack"},
{mjITEM_SELECT, "Font", 1, &this->font, "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
{mjITEM_END}
};
// simulation section of UI
const mjuiDef def_simulation[14] = {
{mjITEM_SECTION, "Simulation", mjPRESERVE, nullptr, "AS"},
{mjITEM_RADIO, "", 5, &this->run, "Pause\nRun"},
{mjITEM_BUTTON, "Reset", 2, nullptr, " #259"},
{mjITEM_BUTTON, "Reload", 5, nullptr, "CL"},
{mjITEM_BUTTON, "Align", 2, nullptr, "CA"},
{mjITEM_BUTTON, "Copy state", 2, nullptr, "CC"},
{mjITEM_SLIDERINT, "Key", 3, &this->key, "0 0"},
{mjITEM_BUTTON, "Load key", 3},
{mjITEM_BUTTON, "Save key", 3},
{mjITEM_SLIDERNUM, "Noise scale", 5, &this->ctrl_noise_std, "0 1"},
{mjITEM_SLIDERNUM, "Noise rate", 5, &this->ctrl_noise_rate, "0 4"},
{mjITEM_SEPARATOR, "History", 1},
{mjITEM_SLIDERINT, "", 5, &this->scrub_index, "0 0"},
{mjITEM_END}
};
// watch section of UI
const mjuiDef def_watch[5] = {
{mjITEM_SECTION, "Watch", mjPRESERVE, nullptr, "AW"},
{mjITEM_EDITTXT, "Field", 2, this->field, "qpos"},
{mjITEM_EDITINT, "Index", 2, &this->index, "1"},
{mjITEM_STATIC, "Value", 2, nullptr, " "},
{mjITEM_END}
};
// info strings
char info_title[Simulate::kMaxFilenameLength] = {0};
char info_content[Simulate::kMaxFilenameLength] = {0};
// pending uploads
std::condition_variable_any cond_upload_;
int texture_upload_ = -1;
int mesh_upload_ = -1;
int hfield_upload_ = -1;
};
} // namespace mujoco
#endif

252
cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc
View File

@ -1,252 +0,0 @@
// Copyright 2023 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 "simulate/mujoco/mujoco_viewer/include/glfw_adapter.h"
#include <cstdlib>
#include <utility>
#include <GLFW/glfw3.h>
#include <mujoco/mjui.h>
#include <mujoco/mujoco.h>
#include "simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h"
#ifdef __APPLE__
#include "glfw_corevideo.h"
#endif
namespace mujoco {
namespace {
int MaybeGlfwInit() {
static const int is_initialized = []() {
auto success = Glfw().glfwInit();
if (success == GLFW_TRUE) {
std::atexit(Glfw().glfwTerminate);
}
return success;
}();
return is_initialized;
}
GlfwAdapter& GlfwAdapterFromWindow(GLFWwindow* window) {
return *static_cast<GlfwAdapter*>(Glfw().glfwGetWindowUserPointer(window));
}
} // namespace
GlfwAdapter::GlfwAdapter() {
if (MaybeGlfwInit() != GLFW_TRUE) {
mju_error("could not initialize GLFW");
}
// multisampling
Glfw().glfwWindowHint(GLFW_SAMPLES, 4);
Glfw().glfwWindowHint(GLFW_VISIBLE, 1);
// get video mode and save
vidmode_ = *Glfw().glfwGetVideoMode(Glfw().glfwGetPrimaryMonitor());
// create window
window_ = Glfw().glfwCreateWindow((2 * vidmode_.width) / 3,
(2 * vidmode_.height) / 3,
"MuJoCo", nullptr, nullptr);
if (!window_) {
mju_error("could not create window");
}
// save window position and size
Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
Glfw().glfwGetWindowSize(window_, &window_size_.first, &window_size_.second);
// set callbacks
Glfw().glfwSetWindowUserPointer(window_, this);
Glfw().glfwSetDropCallback(
window_, +[](GLFWwindow* window, int count, const char** paths) {
GlfwAdapterFromWindow(window).OnFilesDrop(count, paths);
});
Glfw().glfwSetKeyCallback(
window_, +[](GLFWwindow* window, int key, int scancode, int act, int mods) {
GlfwAdapterFromWindow(window).OnKey(key, scancode, act);
});
Glfw().glfwSetMouseButtonCallback(
window_, +[](GLFWwindow* window, int button, int act, int mods) {
GlfwAdapterFromWindow(window).OnMouseButton(button, act);
});
Glfw().glfwSetCursorPosCallback(
window_, +[](GLFWwindow* window, double x, double y) {
GlfwAdapterFromWindow(window).OnMouseMove(x, y);
});
Glfw().glfwSetScrollCallback(
window_, +[](GLFWwindow* window, double xoffset, double yoffset) {
GlfwAdapterFromWindow(window).OnScroll(xoffset, yoffset);
});
Glfw().glfwSetWindowRefreshCallback(
window_, +[](GLFWwindow* window) {
#ifdef __APPLE__
auto& core_video = GlfwAdapterFromWindow(window).core_video_;
if (core_video.has_value()) {
core_video->UpdateDisplayLink();
}
#endif
GlfwAdapterFromWindow(window).OnWindowRefresh();
});
Glfw().glfwSetWindowSizeCallback(
window_, +[](GLFWwindow* window, int width, int height) {
GlfwAdapterFromWindow(window).OnWindowResize(width, height);
});
// make context current
Glfw().glfwMakeContextCurrent(window_);
}
GlfwAdapter::~GlfwAdapter() {
FreeMjrContext();
Glfw().glfwMakeContextCurrent(nullptr);
Glfw().glfwDestroyWindow(window_);
}
std::pair<double, double> GlfwAdapter::GetCursorPosition() const {
double x, y;
Glfw().glfwGetCursorPos(window_, &x, &y);
return {x, y};
}
double GlfwAdapter::GetDisplayPixelsPerInch() const {
int width_mm, height_mm;
Glfw().glfwGetMonitorPhysicalSize(
Glfw().glfwGetPrimaryMonitor(), &width_mm, &height_mm);
return 25.4 * vidmode_.width / width_mm;
}
std::pair<int, int> GlfwAdapter::GetFramebufferSize() const {
int width, height;
Glfw().glfwGetFramebufferSize(window_, &width, &height);
return {width, height};
}
std::pair<int, int> GlfwAdapter::GetWindowSize() const {
int width, height;
Glfw().glfwGetWindowSize(window_, &width, &height);
return {width, height};
}
bool GlfwAdapter::IsGPUAccelerated() const {
return true;
}
void GlfwAdapter::PollEvents() {
Glfw().glfwPollEvents();
}
void GlfwAdapter::SetClipboardString(const char* text) {
Glfw().glfwSetClipboardString(window_, text);
}
void GlfwAdapter::SetVSync(bool enabled){
#ifdef __APPLE__
Glfw().glfwSwapInterval(0);
if (enabled && !core_video_.has_value()) {
core_video_.emplace(window_);
} else if (!enabled && core_video_.has_value()) {
core_video_.reset();
}
#else
Glfw().glfwSwapInterval(enabled);
#endif
}
void GlfwAdapter::SetWindowTitle(const char* title) {
Glfw().glfwSetWindowTitle(window_, title);
}
bool GlfwAdapter::ShouldCloseWindow() const {
return Glfw().glfwWindowShouldClose(window_);
}
void GlfwAdapter::SwapBuffers() {
#ifdef __APPLE__
if (core_video_.has_value()) {
core_video_->WaitForDisplayRefresh();
}
#endif
Glfw().glfwSwapBuffers(window_);
}
void GlfwAdapter::ToggleFullscreen() {
// currently full screen: switch to windowed
if (Glfw().glfwGetWindowMonitor(window_)) {
// restore window from saved data
Glfw().glfwSetWindowMonitor(window_, nullptr, window_pos_.first, window_pos_.second,
window_size_.first, window_size_.second, 0);
}
// currently windowed: switch to full screen
else {
// save window data
Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
Glfw().glfwGetWindowSize(window_, &window_size_.first,
&window_size_.second);
// switch
Glfw().glfwSetWindowMonitor(window_, Glfw().glfwGetPrimaryMonitor(), 0,
0, vidmode_.width, vidmode_.height,
vidmode_.refreshRate);
}
}
bool GlfwAdapter::IsLeftMouseButtonPressed() const {
return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS;
}
bool GlfwAdapter::IsMiddleMouseButtonPressed() const {
return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS;
}
bool GlfwAdapter::IsRightMouseButtonPressed() const {
return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS;
}
bool GlfwAdapter::IsAltKeyPressed() const {
return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_ALT) == GLFW_PRESS ||
Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS;
}
bool GlfwAdapter::IsCtrlKeyPressed() const {
return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS ||
Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS;
}
bool GlfwAdapter::IsShiftKeyPressed() const {
return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS ||
Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS;
}
bool GlfwAdapter::IsMouseButtonDownEvent(int act) const {
return act == GLFW_PRESS;
}
bool GlfwAdapter::IsKeyDownEvent(int act) const { return act == GLFW_PRESS; }
int GlfwAdapter::TranslateKeyCode(int key) const { return key; }
mjtButton GlfwAdapter::TranslateMouseButton(int button) const {
if (button == GLFW_MOUSE_BUTTON_LEFT) {
return mjBUTTON_LEFT;
} else if (button == GLFW_MOUSE_BUTTON_RIGHT) {
return mjBUTTON_RIGHT;
} else if (button == GLFW_MOUSE_BUTTON_MIDDLE) {
return mjBUTTON_MIDDLE;
}
return mjBUTTON_NONE;
}
} // namespace mujoco

127
cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc
View File

@ -1,127 +0,0 @@
// Copyright 2022 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 "simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h"
#ifdef mjGLFW_DYNAMIC_SYMBOLS
#ifdef _MSC_VER
#include <windows.h>
#include <libloaderapi.h>
#else
#include <dlfcn.h>
#endif
#endif
#include <cstdlib>
#include <iostream>
namespace mujoco {
// return dispatch table for glfw functions
const struct Glfw& Glfw(void* dlhandle) {
{
// set static init_dlhandle
static const void* init_dlhandle = dlhandle;
// check that not already initialized
if (dlhandle && dlhandle != init_dlhandle) {
std::cerr << "dlhandle is specified when GLFW dispatch table is already "
"initialized\n";
abort();
}
}
// make and intialize dispatch table
static const struct Glfw glfw = [&]() { // create and call constructor
// allocate
struct Glfw glfw;
// load glfw dynamically
#ifdef mjGLFW_DYNAMIC_SYMBOLS
#ifdef _MSC_VER
if (!dlhandle) dlhandle = LoadLibraryA("glfw3.dll");
if (!dlhandle) {
std::cerr << "cannot obtain a shared object handle\n";
abort();
}
#define mjGLFW_RESOLVE_SYMBOL(func) \
glfw.func = reinterpret_cast<decltype(glfw.func)>( \
GetProcAddress(reinterpret_cast<HMODULE>(dlhandle), #func))
#else
if (!dlhandle) dlhandle = dlopen("nullptr", RTLD_GLOBAL | RTLD_NOW);
if (!dlhandle) {
std::cerr << "cannot obtain a shared object handle\n";
abort();
}
#define mjGLFW_RESOLVE_SYMBOL(func) \
glfw.func = reinterpret_cast<decltype(glfw.func)>(dlsym(dlhandle, #func))
#endif
#else
#define mjGLFW_RESOLVE_SYMBOL(func) glfw.func = &::func
#endif
// set pointers in dispatch table
#define mjGLFW_INITIALIZE_SYMBOL(func) \
if (!(mjGLFW_RESOLVE_SYMBOL(func))) { \
std::cerr << "cannot dlsym " #func "\n"; \
abort(); \
}
// go/keep-sorted start
mjGLFW_INITIALIZE_SYMBOL(glfwCreateWindow);
mjGLFW_INITIALIZE_SYMBOL(glfwDestroyWindow);
mjGLFW_INITIALIZE_SYMBOL(glfwGetCursorPos);
mjGLFW_INITIALIZE_SYMBOL(glfwGetFramebufferSize);
mjGLFW_INITIALIZE_SYMBOL(glfwGetKey);
mjGLFW_INITIALIZE_SYMBOL(glfwGetMonitorPhysicalSize);
mjGLFW_INITIALIZE_SYMBOL(glfwGetMouseButton);
mjGLFW_INITIALIZE_SYMBOL(glfwGetPrimaryMonitor);
mjGLFW_INITIALIZE_SYMBOL(glfwGetTime);
mjGLFW_INITIALIZE_SYMBOL(glfwGetVideoMode);
mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowMonitor);
mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowPos);
mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowSize);
mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowUserPointer);
mjGLFW_INITIALIZE_SYMBOL(glfwInit);
mjGLFW_INITIALIZE_SYMBOL(glfwMakeContextCurrent);
mjGLFW_INITIALIZE_SYMBOL(glfwPollEvents);
mjGLFW_INITIALIZE_SYMBOL(glfwSetClipboardString);
mjGLFW_INITIALIZE_SYMBOL(glfwSetCursorPosCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetDropCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetKeyCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetMouseButtonCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetScrollCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowMonitor);
mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowRefreshCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowSizeCallback);
mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowTitle);
mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowUserPointer);
mjGLFW_INITIALIZE_SYMBOL(glfwSwapBuffers);
mjGLFW_INITIALIZE_SYMBOL(glfwSwapInterval);
mjGLFW_INITIALIZE_SYMBOL(glfwTerminate);
mjGLFW_INITIALIZE_SYMBOL(glfwWindowHint);
mjGLFW_INITIALIZE_SYMBOL(glfwWindowShouldClose);
// go/keep-sorted end
#ifdef __APPLE__
mjGLFW_INITIALIZE_SYMBOL(glfwGetNSGLContext);
#endif
#undef mjGLFW_INITIALIZE_SYMBOL
return glfw;
}();
return glfw;
}
} // namespace mujoco

548
cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp
View File

@ -4,22 +4,20 @@
#include <algorithm>
#include <cstdio>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <limits>
#include <thread>
#include <utility>
#include "common/base/logging/logger.h"
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
#include "simulate/mujoco/mujoco_viewer/include/array_safety.h"
#include "simulate/mujoco/mujoco_viewer/include/glfw_adapter.h"
#include <simulate/glfw_adapter.h>
namespace cmvr {
namespace mj = ::mujoco;
namespace mju = ::mujoco::sample_util;
using Seconds = std::chrono::duration<double>;
constexpr double kSyncMisalign = 0.1;
constexpr double kSimRefreshFraction = 0.7;
constexpr int kPiPMaxGeom = 100000;
class PiPGlfwAdapter : public mj::GlfwAdapter {
@ -38,55 +36,15 @@ namespace cmvr {
MuJocoViewer *owner_;
};
static const char *CheckDiverged(int disableflags, const mjData *d) {
if (disableflags & mjDSBL_AUTORESET) {
for (mjtWarning w: {mjWARN_BADQACC, mjWARN_BADQVEL, mjWARN_BADQPOS}) {
if (d->warning[w].number > 0) {
return mju_warningText(w, d->warning[w].lastinfo);
}
}
}
return nullptr;
}
mjModel *MuJocoViewer::LoadModelSimple(const char *file,
char *load_error,
int error_sz) {
load_error[0] = '\0';
if (!file || !file[0]) {
std::snprintf(load_error, error_sz, "empty model filename");
return nullptr;
}
mjModel *mnew = nullptr;
const char *dot = std::strrchr(file, '.');
if (dot && std::strcmp(dot, ".mjb") == 0) {
mnew = mj_loadModel(file, nullptr);
if (!mnew) {
std::snprintf(load_error, error_sz,
"could not load binary model: %s", file);
}
} else {
mnew = mj_loadXML(file, nullptr, load_error, error_sz);
if (load_error[0]) {
int len = std::strlen(load_error);
if (len > 0 && load_error[len - 1] == '\n') {
load_error[len - 1] = '\0';
}
}
}
return mnew;
}
MuJocoViewer::MuJocoViewer(const char *model_path)
: model_path_(model_path ? model_path : "") {
MuJocoViewer::MuJocoViewer(std::shared_ptr<simulate::MujocoWorld> world)
: world_(std::move(world)) {
std::printf("MuJoCo version %s\n", mj_versionString());
if (mjVERSION_HEADER != mj_version()) {
mju_error("Headers and library have different versions");
}
if (!world_) {
mju_error("MuJocoViewer requires a MujocoWorld");
}
initSim();
}
@ -102,7 +60,7 @@ namespace cmvr {
sim_ = std::make_unique<mj::Simulate>(
std::move(platform_ui),
&cam_, &opt_, &pert_,
/*is_passive=*/false
/*is_passive=*/true
);
}
@ -110,47 +68,22 @@ namespace cmvr {
if (sim_) {
sim_->exitrequest.store(1);
}
if (physics_started_.load() && physics_thread_.joinable()) {
physics_thread_.join();
if (sync_thread_.joinable()) {
sync_thread_.join();
}
if (pip_scene_inited_) {
mjv_freeScene(&pip_scene_);
pip_scene_inited_ = false;
}
if (d_) mj_deleteData(d_);
if (m_) mj_deleteModel(m_);
}
void MuJocoViewer::physicsThreadFunc() {
// 1) 加载模型
char error[1024];
m_ = LoadModelSimple(model_path_.c_str(), error, sizeof(error));
if (!m_) {
mju_error("Load model failed: %s", error);
}
mjModel* MuJocoViewer::model() const {
return world_ ? world_->model() : nullptr;
}
d_ = mj_makeData(m_);
if (!d_) {
mj_deleteModel(m_);
m_ = nullptr;
mju_error("mj_makeData failed");
}
// 2) 用官方的方式：先告诉 Simulate 要加载，然后等 RenderLoop 处理
sim_->Load(m_, d_, model_path_.c_str());
{
// official main 里也是在 Load 之后先 forward 一下
const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
mj_forward(m_, d_);
}
physics_started_.store(true);
// 3) 进入物理循环
physicsLoop();
mjData* MuJocoViewer::data() const {
return world_ ? world_->data() : nullptr;
}
void MuJocoViewer::getCameraState(double &distance,
@ -165,12 +98,12 @@ namespace cmvr {
double azimuth,
double elevation) {
// 相机初始化：根据模型设置一个合理的 free camera
if (m_) {
mjv_defaultFreeCamera(m_, &cam_);
if (model()) {
mjv_defaultFreeCamera(model(), &cam_);
// 以模型中心为观察点
cam_.lookat[0] = m_->stat.center[0];
cam_.lookat[1] = m_->stat.center[1];
cam_.lookat[2] = m_->stat.center[2];
cam_.lookat[0] = model()->stat.center[0];
cam_.lookat[1] = model()->stat.center[1];
cam_.lookat[2] = model()->stat.center[2];
} else {
mjv_defaultCamera(&cam_);
}
@ -187,6 +120,8 @@ namespace cmvr {
pip_camera_id_ = -1;
pip_width_ = 320;
pip_height_ = 240;
pip_render_width_ = pip_width_;
pip_render_height_ = pip_height_;
pip_custom_pos_ = false;
}
@ -195,13 +130,25 @@ namespace cmvr {
int bottom,
int width,
int height) {
enablePiPCamera(camera_name, left, bottom, width, height, width, height);
}
void MuJocoViewer::enablePiPCamera(const char *camera_name,
int left,
int bottom,
int display_width,
int display_height,
int render_width,
int render_height) {
pip_enabled_ = true;
pip_camera_name_ = camera_name ? camera_name : "";
pip_camera_id_ = -1;
pip_left_ = left;
pip_bottom_ = bottom;
pip_width_ = width > 0 ? width : 320;
pip_height_ = height > 0 ? height : 240;
pip_width_ = display_width > 0 ? display_width : 320;
pip_height_ = display_height > 0 ? display_height : 240;
pip_render_width_ = render_width > 0 ? render_width : pip_width_;
pip_render_height_ = render_height > 0 ? render_height : pip_height_;
pip_custom_pos_ = true;
}
@ -212,11 +159,15 @@ namespace cmvr {
void MuJocoViewer::renderPiP() {
if (!pip_enabled_ || !sim_ || !m_ || !d_) return;
if (!pip_enabled_ || !sim_) return;
if (pip_camera_name_.empty()) return;
mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_;
mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_;
if (!render_model || !render_data) return;
if (pip_camera_id_ < 0) {
pip_camera_id_ = mj_name2id(m_, mjOBJ_CAMERA, pip_camera_name_.c_str());
pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str());
if (pip_camera_id_ < 0) {
return;
}
@ -231,10 +182,16 @@ namespace cmvr {
int height = 0;
if (pip_custom_pos_) {
left = std::max(0, std::min(pip_left_, fb_width - 1));
bottom = std::max(0, std::min(pip_bottom_, fb_height - 1));
width = std::min(pip_width_, fb_width - left);
height = std::min(pip_height_, fb_height - bottom);
width = std::min(pip_width_, fb_width);
height = std::min(pip_height_, fb_height);
const int max_left = fb_width - width;
const int max_bottom = fb_height - height;
left = pip_left_ >= 0
? std::max(0, std::min(pip_left_, max_left))
: std::max(0, std::min(fb_width - width + pip_left_, max_left));
bottom = pip_bottom_ >= 0
? std::max(0, std::min(pip_bottom_, max_bottom))
: std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom));
} else {
width = std::min(pip_width_, fb_width - 2 * pip_margin_);
height = std::min(pip_height_, fb_height - 2 * pip_margin_);
@ -244,41 +201,80 @@ namespace cmvr {
if (width <= 0 || height <= 0) return;
mjrRect rect;
rect.width = width;
rect.height = height;
rect.left = left;
rect.bottom = bottom;
mjrRect display_rect;
display_rect.width = width;
display_rect.height = height;
display_rect.left = left;
display_rect.bottom = bottom;
const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
if (!pip_scene_inited_ || pip_scene_model_ != m_) {
if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
if (pip_scene_inited_) {
mjv_freeScene(&pip_scene_);
}
mjv_makeScene(m_, &pip_scene_, kPiPMaxGeom);
mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom);
pip_scene_inited_ = true;
pip_scene_model_ = m_;
pip_scene_model_ = render_model;
}
pip_cam_.type = mjCAMERA_FIXED;
pip_cam_.fixedcamid = pip_camera_id_;
pip_cam_.trackbodyid = -1;
mjv_updateScene(m_, d_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
mjr_render(rect, &pip_scene_, &sim_->platform_ui->mjr_context());
mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
auto& context = sim_->platform_ui->mjr_context();
const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width);
const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height);
if (!pip_render_size_warning_logged_ &&
(render_width != pip_render_width_ || render_height != pip_render_height_)) {
CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
<< ", requested=" << pip_render_width_ << "x" << pip_render_height_
<< ", actual=" << render_width << "x" << render_height
<< ", offscreen=" << offscreen_width << "x" << offscreen_height;
pip_render_size_warning_logged_ = true;
}
const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height;
mjrRect render_rect;
render_rect.left = 0;
render_rect.bottom = 0;
render_rect.width = render_width;
render_rect.height = render_height;
bool rendered_offscreen = false;
if (use_offscreen) {
mjr_setBuffer(mjFB_OFFSCREEN, &context);
if (context.currentBuffer == mjFB_OFFSCREEN) {
mjr_render(render_rect, &pip_scene_, &context);
rendered_offscreen = true;
} else {
mjr_render(display_rect, &pip_scene_, &context);
render_rect = display_rect;
}
} else {
mjr_render(display_rect, &pip_scene_, &context);
render_rect = display_rect;
}
{
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
const int w = rect.width;
const int h = rect.height;
const int w = render_rect.width;
const int h = render_rect.height;
if (w > 0 && h > 0) {
pip_rgb_.resize(static_cast<size_t>(3 * w * h));
pip_depth_.resize(static_cast<size_t>(w * h));
// 同时读 RGB 和 depth(z-buffer 0..1)
mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
rect, &sim_->platform_ui->mjr_context());
render_rect, &context);
if (rendered_offscreen) {
mjr_setBuffer(mjFB_WINDOW, &context);
mjr_render(display_rect, &pip_scene_, &context);
}
// OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
for (int r = 0; r < h / 2; ++r) {
@ -294,8 +290,10 @@ namespace cmvr {
}
// 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
const double znear = static_cast<double>(m_->vis.map.znear) * static_cast<double>(m_->stat.extent);
const double zfar = static_cast<double>(m_->vis.map.zfar) * static_cast<double>(m_->stat.extent);
const double znear = static_cast<double>(render_model->vis.map.znear) *
static_cast<double>(render_model->stat.extent);
const double zfar = static_cast<double>(render_model->vis.map.zfar) *
static_cast<double>(render_model->stat.extent);
if (znear > 0.0 && zfar > znear) {
const double two_nf = 2.0 * znear * zfar;
const double f_plus_n = zfar + znear;
@ -323,137 +321,51 @@ namespace cmvr {
}
}
void MuJocoViewer::physicsLoop() {
using Clock = mj::Simulate::Clock;
void MuJocoViewer::syncThreadFunc() {
if (!world_->isLoaded()) {
mju_error("MuJocoViewer requires a loaded MujocoWorld");
}
std::chrono::time_point<Clock> syncCPU;
mjtNum syncSim = 0;
{
std::lock_guard<std::mutex> lock(world_->mutex());
if (!world_->model() || !world_->data()) {
mju_error("MuJocoViewer world has null model/data");
}
if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) {
mjModel* model = world_->model();
const int old_width = model->vis.global.offwidth;
const int old_height = model->vis.global.offheight;
model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_);
model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_);
if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) {
CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation"
<< ", old=" << old_width << "x" << old_height
<< ", new=" << model->vis.global.offwidth << "x" << model->vis.global.offheight;
}
}
sim_->Load(world_->model(), world_->data(), world_->modelPath().c_str());
}
while (!sim_->exitrequest.load()) {
// 给 UI 线程一点时间
if (sim_->run && sim_->busywait) {
std::this_thread::yield();
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
{
const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
if (!m_ || !d_) continue;
if (sim_->run) {
bool stepped = false;
const auto startCPU = Clock::now();
const auto elapsedCPU = startCPU - syncCPU;
double elapsedSim = d_->time - syncSim;
double slowdown = 100.0 / sim_->percentRealTime[sim_->real_time_index];
bool misaligned =
std::abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > kSyncMisalign;
if (elapsedSim < 0 || elapsedCPU.count() < 0 ||
syncCPU.time_since_epoch().count() == 0 ||
misaligned || sim_->speed_changed) {
syncCPU = startCPU;
syncSim = d_->time;
sim_->speed_changed = false;
sim_->InjectNoise(sim_->key);
controlDispatch(m_, d_);
mj_step(m_, d_);
const char *msg = CheckDiverged(m_->opt.disableflags, d_);
if (msg) {
sim_->run = 0;
mju::strcpy_arr(sim_->load_error, msg);
} else {
stepped = true;
}
} else {
bool measured = false;
mjtNum prevSim = d_->time;
double refreshTime = kSimRefreshFraction / sim_->refresh_rate;
while (Seconds((d_->time - syncSim) * slowdown) <
(Clock::now() - syncCPU) &&
(Clock::now() - startCPU) < Seconds(refreshTime)) {
if (!measured && elapsedSim) {
sim_->measured_slowdown =
Seconds(elapsedCPU).count() / elapsedSim;
measured = true;
}
sim_->InjectNoise(sim_->key);
controlDispatch(m_, d_);
mj_step(m_, d_);
const char *msg = CheckDiverged(m_->opt.disableflags, d_);
if (msg) {
sim_->run = 0;
mju::strcpy_arr(sim_->load_error, msg);
} else {
stepped = true;
}
if (d_->time < prevSim) {
break;
}
}
}
if (stepped) {
sim_->AddToHistory();
}
} else {
mj_forward(m_, d_);
if (sim_->pause_update) {
mju_copy(d_->qacc_warmstart, d_->qacc, m_->nv);
}
sim_->speed_changed = true;
}
if (d_->time == 0.0) {
onReset(m_, d_);
}
std::lock_guard<std::mutex> lock(world_->mutex());
sim_->Sync(/*state_only=*/true);
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
void MuJocoViewer::controlDispatch(mjModel *m, mjData *d) {
// 第一次：只跑一次 initOnce
if (!inited_) {
initOnce(m, d);
inited_ = true;
last_time_ = d->time;
}
// 检测 reset：时间从大跳到小（比如从 3.2 回到 0）
if (d->time < last_time_) {
onReset(m, d);
inited_ = false;
}
last_time_ = d->time;
// 每步控制
controlCallback(m, d);
}
void MuJocoViewer::run() {
// 先起 physics 线程（里面会 sim_->Load）
physics_thread_ = std::thread(&MuJocoViewer::physicsThreadFunc, this);
sync_thread_ = std::thread(&MuJocoViewer::syncThreadFunc, this);
// 当前线程跑 UI + 渲染（阻塞）
sim_->RenderLoop();
// UI 退出后，通知 physics 线程结束
// UI 退出后，通知同步线程结束
sim_->exitrequest.store(2);
if (physics_thread_.joinable()) {
physics_thread_.join();
if (sync_thread_.joinable()) {
sync_thread_.join();
}
}
@ -492,4 +404,180 @@ namespace cmvr {
frame_id = pip_frame_id_;
return true;
}
MujocoViewerDevice::MujocoViewerDevice(config::MujocoViewerConfig config)
: device::AbstractDevice(config.id()), config_(std::move(config)) {}
MujocoViewerDevice::~MujocoViewerDevice() {
stop();
}
bool MujocoViewerDevice::init() {
std::lock_guard<std::mutex> lock(mtx_);
if (world_) {
return true;
}
if (config_.id().empty()) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] id is empty";
return false;
}
if (config_.world_id().empty()) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] world_id is empty, id=" << id_;
return false;
}
auto world = simulate::MujocoWorldDevice::worldFor(config_.world_id());
if (!world) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] MuJoCo world not found: "
<< config_.world_id();
return false;
}
if (!world->isLoaded()) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] MuJoCo world is not loaded: "
<< config_.world_id();
return false;
}
world_ = std::move(world);
CMVR_LOG(INFO) << "[MujocoViewerDevice] initialized, id=" << id_
<< ", world_id=" << config_.world_id();
return true;
}
bool MujocoViewerDevice::start() {
CMVR_LOG(INFO) << "[MujocoViewerDevice] start skipped, id=" << id_
<< ", call runOnMainThread() from main thread to show UI";
return true;
}
bool MujocoViewerDevice::runOnMainThread() {
if (!init()) {
return false;
}
std::shared_ptr<simulate::MujocoWorld> world;
{
std::lock_guard<std::mutex> lock(mtx_);
if (running_) {
return true;
}
if (!world_) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] world is null, id=" << id_;
return false;
}
world = world_;
stop_requested_ = false;
running_ = true;
}
if (!world->isRunning() && !world->start()) {
CMVR_LOG(ERROR) << "[MujocoViewerDevice] failed to start world, id=" << id_
<< ", error=" << world->lastError();
std::lock_guard<std::mutex> lock(mtx_);
running_ = false;
return false;
}
auto viewer = std::make_unique<MuJocoViewer>(world);
const double distance = config_.camera_distance() > 0.0
? config_.camera_distance()
: 3.0;
viewer->setupCamera(distance,
config_.camera_azimuth(),
config_.camera_elevation());
if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) {
const auto& pip = pip_camera_config_.viewer_pip();
const auto& render = pip_camera_config_.render();
if (pip.width() > 0 && pip.height() > 0) {
viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
} else {
viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
}
}
{
std::lock_guard<std::mutex> lock(mtx_);
viewer_ = std::move(viewer);
}
CMVR_LOG(INFO) << "[MujocoViewerDevice] run UI on main thread, id=" << id_;
viewer_->run();
{
std::lock_guard<std::mutex> lock(mtx_);
viewer_.reset();
running_ = false;
}
return true;
}
bool MujocoViewerDevice::setPiPCameraConfig(const config::MujocoCameraConfig& camera_config) {
if (!camera_config.viewer_pip().enable()) {
return false;
}
if (camera_config.world_id() != config_.world_id()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
if (has_pip_camera_config_) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
<< ", viewer_id=" << id_
<< ", current_camera=" << pip_camera_config_.camera_name()
<< ", ignored_camera=" << camera_config.camera_name();
return false;
}
pip_camera_config_ = camera_config;
has_pip_camera_config_ = true;
CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
<< ", viewer_id=" << id_
<< ", camera=" << camera_config.camera_name()
<< ", world_id=" << camera_config.world_id();
return true;
}
bool MujocoViewerDevice::getPiPCameraRGBD(std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) const {
std::lock_guard<std::mutex> lock(mtx_);
if (!viewer_) {
return false;
}
return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
}
bool MujocoViewerDevice::stop() {
std::thread thread_to_join;
{
std::lock_guard<std::mutex> lock(mtx_);
stop_requested_ = true;
if (viewer_) {
viewer_->requestStop();
}
if (viewer_thread_.joinable()) {
thread_to_join = std::move(viewer_thread_);
}
}
if (thread_to_join.joinable()) {
thread_to_join.join();
}
{
std::lock_guard<std::mutex> lock(mtx_);
running_ = false;
viewer_.reset();
}
return true;
}
} // namespace cmvr

303
cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp
View File

@ -1,270 +1,65 @@
//
// Created by lgv on 11/25/25.
//
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <memory>
#include <string>
#include "algorithms/controllers/pid/include/pid_controller.h"
#include <gtest/gtest.h>
#include "gtest/gtest.h"
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
#include <thread>
#include <array>
#include <vector>
#include <cstdio>
namespace {
#include "common/vision/matplotlibcpp.h"
namespace plt = matplotlibcpp;
using namespace cmvr;
class MyViewer : public cmvr::MuJocoViewer {
public:
using MuJocoViewer::MuJocoViewer;
// 对外接口：设置右臂关节目标角度（rad）
void moveJ(const std::array<double, 7>& q_target) {
q_target_ = q_target;
}
// 仿真结束后调用：画出 7 个关节的目标值与实际值曲线（7 行子图）
void plotJointResponse(const std::string& filename = "right_arm_joint_response.png");
private:
int print_counter_ = 0; // 打印计数器
protected:
static constexpr int kRightArmDOF = 7;
int act_ids_[kRightArmDOF]; // actuator 索引
int qpos_ids_[kRightArmDOF]; // qpos 索引
int qvel_ids_[kRightArmDOF]; // qvel 索引
std::array<double, kRightArmDOF> q_target_{}; // 目标关节角
// PID 控制器
PidController pid_;
double dt_ = 0.0; // 仿真时间步长
// 日志：时间 & 每帧的 7 关节实际角 / 目标角
std::vector<double> time_log_;
std::vector<std::array<double, kRightArmDOF>> q_log_;
std::vector<std::array<double, kRightArmDOF>> q_des_log_;
void initOnce(mjModel* m, mjData* d) override
{
// 设置相机视角参数
setupCamera(3.0, -170, -41);
dt_ = m->opt.timestep;
const char* act_names[kRightArmDOF] = {
"R_SHOULDER_P_tau",
"R_SHOULDER_R_tau",
"R_SHOULDER_Y_tau",
"R_ELBOW_R_tau",
"R_WRIST_P_tau",
"R_WRIST_Y_tau",
"R_WRIST_R_tau"
};
const char* jnt_names[kRightArmDOF] = {
"R_SHOULDER_P",
"R_SHOULDER_R",
"R_SHOULDER_Y",
"R_ELBOW_R",
"R_WRIST_P",
"R_WRIST_Y",
"R_WRIST_R"
};
for (int i = 0; i < kRightArmDOF; ++i) {
// actuator 索引
act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
if (act_ids_[i] < 0) {
std::fprintf(stderr, "Cannot find actuator %s\n", act_names[i]);
}
// joint -> qpos/qvel 索引
int jnt_id = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]);
if (jnt_id < 0) {
std::fprintf(stderr, "Cannot find joint %s\n", jnt_names[i]);
qpos_ids_[i] = -1;
qvel_ids_[i] = -1;
} else {
qpos_ids_[i] = m->jnt_qposadr[jnt_id]; // 位置
qvel_ids_[i] = m->jnt_dofadr[jnt_id]; // 速度
}
}
// === 初始化 PID 控制器 ===
// 注意：这里用的是你自己的接口，如果你的是 setDof，就改成 pid_.setDof(kRightArmDOF);
pid_.init(kRightArmDOF);
// 简单一组增益（后面可以按关节单独调）
Eigen::VectorXd Kp = Eigen::VectorXd::Constant(kRightArmDOF, 50.0);
Eigen::VectorXd Ki = Eigen::VectorXd::Constant(kRightArmDOF, 100);
Eigen::VectorXd Kd = Eigen::VectorXd::Constant(kRightArmDOF, 0);
pid_.setGains(Kp, Ki, Kd);
// 积分上下限
Eigen::VectorXd iLower = Eigen::VectorXd::Constant(kRightArmDOF, -10.0);
Eigen::VectorXd iUpper = Eigen::VectorXd::Constant(kRightArmDOF, 10.0);
pid_.setIntegralLimits(iLower, iUpper);
// 误差死区（小于 0.002 rad 不管）
Eigen::VectorXd deadzone = Eigen::VectorXd::Constant(kRightArmDOF, 0.002);
pid_.setDeadzone(deadzone);
// D 项滤波系数（可根据实际调）
pid_.setDerivativeFilterCoeff(50.0);
std::printf("[MyViewer] initOnce done\n");
}
// 每个仿真步物理线程都会调用这里
void controlCallback(mjModel* m, mjData* d) override
{
// === 1. 组装当前状态 ===
Eigen::VectorXd q(kRightArmDOF);
Eigen::VectorXd dq(kRightArmDOF);
for (int i = 0; i < kRightArmDOF; ++i) {
if (qpos_ids_[i] >= 0)
q[i] = d->qpos[qpos_ids_[i]];
else
q[i] = 0.0;
if (qvel_ids_[i] >= 0)
dq[i] = d->qvel[qvel_ids_[i]];
else
dq[i] = 0.0;
}
ControlInput input;
input.q = q;
input.dq = dq;
// === 2. 组装参考（目标角度） ===
ControlReference ref;
ref.q_d = Eigen::Map<const Eigen::VectorXd>(q_target_.data(), kRightArmDOF);
ref.dq_d = Eigen::VectorXd::Zero(kRightArmDOF);
pid_.setReference(ref);
// === 3. 计算关节力矩 τ ===
Eigen::VectorXd tau = pid_.compute(input, dt_);
// === 4. 写入 MuJoCo ctrl（motor: ctrl = torque） ===
for (int i = 0; i < kRightArmDOF; ++i) {
if (act_ids_[i] < 0) continue;
d->ctrl[act_ids_[i]] = tau[i];
}
// === 5. 记录日志：时间、实际关节角、目标角 ===
double t = d->time;
time_log_.push_back(t);
std::array<double, kRightArmDOF> q_frame{};
std::array<double, kRightArmDOF> q_des_frame{};
for (int i = 0; i < kRightArmDOF; ++i) {
q_frame[i] = q[i];
q_des_frame[i] = ref.q_d[i];
}
q_log_.push_back(q_frame);
q_des_log_.push_back(q_des_frame);
// === 6. 打印误差（比如每 100 步打印一次） ===
if (++print_counter_ % 100 == 0) {
Eigen::VectorXd q_des = ref.q_d;
Eigen::VectorXd err = q_des - q;
std::printf("[Err] ");
for (int i = 0; i < kRightArmDOF; ++i) {
std::printf("J%d=%.4f ", i, err[i]);
}
std::printf(" | ||e||=%.4f\n", err.norm());
}
}
void onReset(mjModel* m, mjData* d) override {
std::printf("[MyViewer] onReset done\n");
time_log_.clear();
q_log_.clear();
q_des_log_.clear();
}
};
// === 画图函数实现：7 个子图，目标 vs 实际 ===
void MyViewer::plotJointResponse(const std::string& filename)
std::filesystem::path findProjectRoot()
{
if (time_log_.empty() || q_log_.empty() || q_des_log_.empty()) {
std::printf("[MyViewer] No log data, skip plotting.\n");
return;
const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
auto current = std::filesystem::current_path();
while (!current.empty()) {
if (std::filesystem::exists(current / marker)) {
return current;
}
const auto parent = current.parent_path();
if (parent == current) {
break;
}
current = parent;
}
const std::vector<double>& t = time_log_;
plt::figure();
for (int j = 0; j < kRightArmDOF; ++j) {
// 为第 j 个关节准备数据
std::vector<double> q_j(t.size());
std::vector<double> qd_j(t.size());
for (size_t k = 0; k < t.size(); ++k) {
q_j[k] = q_log_[k][j];
qd_j[k] = q_des_log_[k][j];
}
// 子图：7 行 1 列，第 j+1 个
plt::subplot(kRightArmDOF, 1, j + 1);
plt::named_plot("q_des", t, qd_j);
plt::named_plot("q", t, q_j);
if (j == 0) {
plt::legend();
}
char title[64];
std::snprintf(title, sizeof(title), "Joint %d", j);
plt::title(title);
if (j == kRightArmDOF - 1) {
plt::xlabel("Time [s]");
}
plt::ylabel("Angle [rad]");
plt::grid(true);
}
// plt::tight_layout();
plt::show();
plt::save(filename);
std::printf("[MyViewer] Joint response figure saved to %s\n", filename.c_str());
return {};
}
// === gtest ===
TEST(mujoco_viewer_test, test_view_and_pid)
std::string defaultModelPath()
{
const auto root = findProjectRoot();
if (root.empty()) {
return "model/xiaoyan_description/dual_arm.xml";
}
return (root / "model/xiaoyan_description/dual_arm.xml").string();
}
MyViewer viewer("/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml");
} // namespace
std::array<double, 7> q = {
M_PI/6, 1.26, M_PI/12,
M_PI/4, 0.22, 0.65, 0.66
};
viewer.moveJ(q);
TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
{
const char* model_path_env = std::getenv("MUJOCO_VIEWER_TEST_MODEL");
const std::string model_path =
model_path_env != nullptr && model_path_env[0] != '\0'
? model_path_env
: defaultModelPath();
auto world = std::make_shared<cmvr::simulate::MujocoWorld>();
cmvr::simulate::MujocoWorld::Options options;
options.model_path = model_path;
options.realtime_factor = 1.0;
ASSERT_TRUE(world->load(options)) << world->lastError();
ASSERT_TRUE(world->start()) << world->lastError();
std::cout << "MujocoWorld loaded: " << model_path << std::endl;
std::cout << "Close the MuJoCo window to exit." << std::endl;
cmvr::MuJocoViewer viewer(world);
viewer.run();
// // 仿真结束后画 7 个关节的响应曲线
viewer.plotJointResponse("right_arm_joint_response.png");
world->stop();
}

247
cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc
View File

@ -1,247 +0,0 @@
// Copyright 2023 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 "simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h"
#include <chrono>
namespace mujoco {
PlatformUIAdapter::PlatformUIAdapter() {
mjr_defaultContext(&con_);
}
void PlatformUIAdapter::FreeMjrContext() {
mjr_freeContext(&con_);
}
bool PlatformUIAdapter::RefreshMjrContext(const mjModel* m, int fontscale) {
if (m != last_model_ || fontscale != last_fontscale_) {
mjr_makeContext(m, &con_, fontscale);
last_model_ = m;
last_fontscale_ = fontscale;
return true;
}
return false;
}
bool PlatformUIAdapter::EnsureContextSize() {
return false;
}
void PlatformUIAdapter::OnFilesDrop(int count, const char** paths) {
state_.type = mjEVENT_FILESDROP;
state_.dropcount = count;
state_.droppaths = paths;
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
// remove paths pointer from mjuiState since we don't own it
state_.dropcount = 0;
state_.droppaths = nullptr;
}
void PlatformUIAdapter::OnKey(int key, int scancode, int act) {
// translate API-specific key code
int mj_key = TranslateKeyCode(key);
// release: nothing to do
if (!IsKeyDownEvent(act)) {
return;
}
// update state
UpdateMjuiState();
// set key info
state_.type = mjEVENT_KEY;
state_.key = mj_key;
state_.keytime = std::chrono::duration<double>(
std::chrono::steady_clock::now().time_since_epoch()).count();
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
last_key_ = mj_key;
}
void PlatformUIAdapter::OnMouseButton(int button, int act) {
// translate API-specific mouse button code
mjtButton mj_button = TranslateMouseButton(button);
// update state
UpdateMjuiState();
// swap left and right if Alt
if (state_.alt) {
if (mj_button == mjBUTTON_LEFT) {
mj_button = mjBUTTON_RIGHT;
} else if (mj_button == mjBUTTON_RIGHT) {
mj_button = mjBUTTON_LEFT;
}
}
// press
if (IsMouseButtonDownEvent(act)) {
double now = std::chrono::duration<double>(
std::chrono::steady_clock::now().time_since_epoch()).count();
// detect doubleclick: 250 ms
if (mj_button == state_.button && now - state_.buttontime < 0.25) {
state_.doubleclick = 1;
} else {
state_.doubleclick = 0;
}
// set info
state_.type = mjEVENT_PRESS;
state_.button = mj_button;
state_.buttontime = now;
// start dragging
if (state_.mouserect) {
state_.dragbutton = state_.button;
state_.dragrect = state_.mouserect;
}
}
// release
else {
state_.type = mjEVENT_RELEASE;
}
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
// stop dragging after application processing
if (state_.type == mjEVENT_RELEASE) {
state_.dragrect = 0;
state_.dragbutton = 0;
}
}
void PlatformUIAdapter::OnMouseMove(double x, double y) {
// no buttons down: nothing to do
if (!state_.left && !state_.right && !state_.middle) {
return;
}
// update state
UpdateMjuiState();
// set move info
state_.type = mjEVENT_MOVE;
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
}
void PlatformUIAdapter::OnScroll(double xoffset, double yoffset) {
// update state
UpdateMjuiState();
// set scroll info, scale by buffer-to-window ratio
const double buffer_window_ratio =
static_cast<double>(GetFramebufferSize().first) / GetWindowSize().first;
state_.type = mjEVENT_SCROLL;
state_.sx = xoffset * buffer_window_ratio;
state_.sy = yoffset * buffer_window_ratio;
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
}
void PlatformUIAdapter::OnWindowRefresh() {
state_.type = mjEVENT_REDRAW;
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
}
void PlatformUIAdapter::OnWindowResize(int width, int height) {
auto [buf_width, buf_height] = GetFramebufferSize();
state_.rect[0].width = buf_width;
state_.rect[0].height = buf_height;
if (state_.nrect < 1) state_.nrect = 1;
// update window layout
if (layout_callback_) {
layout_callback_(&state_);
}
// update state
UpdateMjuiState();
// set resize info
state_.type = mjEVENT_RESIZE;
// stop dragging
state_.dragbutton = 0;
state_.dragrect = 0;
// application-specific processing
if (event_callback_) {
event_callback_(&state_);
}
}
void PlatformUIAdapter::UpdateMjuiState() {
// mouse buttons
state_.left = IsLeftMouseButtonPressed();
state_.right = IsRightMouseButtonPressed();
state_.middle = IsMiddleMouseButtonPressed();
// keyboard modifiers
state_.control = IsCtrlKeyPressed();
state_.shift = IsShiftKeyPressed();
state_.alt = IsAltKeyPressed();
// swap left and right if Alt
if (state_.alt) {
int tmp = state_.left;
state_.left = state_.right;
state_.right = tmp;
}
// get mouse position, scale by buffer-to-window ratio
auto [x, y] = GetCursorPosition();
const double buffer_window_ratio =
static_cast<double>(GetFramebufferSize().first) / GetWindowSize().first;
x *= buffer_window_ratio;
y *= buffer_window_ratio;
// invert y to match OpenGL convention
y = state_.rect[0].height - y;
// save
state_.dx = x - state_.x;
state_.dy = y - state_.y;
state_.x = x;
state_.y = y;
// find mouse rectangle
state_.mouserect = mjr_findRect(mju_round(x), mju_round(y), state_.nrect-1, state_.rect+1) + 1;
}
} // namespace mujoco

2982
cmvr-es/simulate/mujoco/mujoco_viewer/src/simulate.cc
View File

File diff suppressed because it is too large Load Diff

15
cmvr-es/simulate/mujoco/mujoco_world/CMakeLists.txt Normal file
View File

@ -0,0 +1,15 @@
add_library(mujoco_world SHARED
src/mujoco_world.cpp
)
target_include_directories(mujoco_world PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(mujoco_world PUBLIC
cmvr_es::proto
cmvr_es::common
mujoco
pthread
)
add_library(cmvr_es::mujoco_world ALIAS mujoco_world)
install(TARGETS mujoco_world LIBRARY DESTINATION lib)

148
cmvr-es/simulate/mujoco/mujoco_world/include/mujoco_world.h Normal file
View File

@ -0,0 +1,148 @@
#ifndef CMVR_ES_MUJOCO_WORLD_H
#define CMVR_ES_MUJOCO_WORLD_H
#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>
#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h"
#include "devices/abstract_device.h"
#include <mujoco/mujoco.h>
namespace cmvr::simulate {
class MujocoWorld {
public:
struct JointSpec {
std::string joint_name;
bool allow_state_control_fallback{false};
};
struct Options {
std::string model_path;
std::vector<JointSpec> joints;
double timestep_s{0.0};
double realtime_factor{1.0};
bool require_actuator{false};
bool allow_state_control_fallback{false};
};
MujocoWorld() = default;
explicit MujocoWorld(Options options);
~MujocoWorld();
MujocoWorld(const MujocoWorld&) = delete;
MujocoWorld& operator=(const MujocoWorld&) = delete;
bool load(const Options& options);
bool start();
void stop();
void reset();
bool isLoaded() const;
bool isRunning() const;
double timestep() const;
const std::string& lastError() const;
const std::string& modelPath() const;
std::vector<std::string> jointNames() const;
bool hasJoint(const std::string& joint_name) const;
bool getJointPosition(const std::string& joint_name, double& position) const;
bool getJointVelocity(const std::string& joint_name, double& velocity) const;
bool setJointPosition(const std::string& joint_name, double position);
bool setJointVelocity(const std::string& joint_name, double velocity);
bool setJointTargetPosition(const std::string& joint_name, double position);
bool setJointTargetState(const std::string& joint_name,
double position,
double velocity);
bool setJointTargetPositions(const std::vector<std::string>& joint_names,
const std::vector<double>& positions);
bool setJointTargetStates(const std::vector<std::string>& joint_names,
const std::vector<double>& positions,
const std::vector<double>& velocities);
bool setJointTargetVelocity(const std::string& joint_name, double velocity);
mjModel* model();
mjData* data();
const mjModel* model() const;
const mjData* data() const;
std::mutex& mutex() const;
private:
enum class TargetMode {
HoldPosition,
ActuatorPosition,
ActuatorVelocity,
DirectPosition,
DirectVelocity,
DirectState
};
struct JointHandle {
int joint_id{-1};
int qpos_adr{-1};
int qvel_adr{-1};
int actuator_id{-1};
bool allow_state_control_fallback{false};
TargetMode mode{TargetMode::HoldPosition};
double position_reference{0.0};
double target_position{0.0};
double target_velocity{0.0};
};
static mjModel* loadModelFile(const std::string& model_path, std::string& error);
bool buildJointMapLocked(const std::vector<JointSpec>& joint_specs);
bool addJointLocked(const JointSpec& joint_spec);
int findJointActuatorLocked(const std::string& joint_name, int joint_id) const;
bool findJointLocked(const std::string& joint_name, JointHandle*& joint);
bool findJointLocked(const std::string& joint_name, const JointHandle*& joint) const;
void setLastErrorLocked(const std::string& error) const;
void clearModelLocked();
void simulationLoop();
void applyControlLocked();
void applyDirectControlLocked(double dt);
mutable std::mutex mutex_;
Options options_;
mjModel* model_{nullptr};
mjData* data_{nullptr};
std::unordered_map<std::string, JointHandle> joints_;
mutable std::string last_error_;
std::thread simulation_thread_;
std::atomic<bool> stop_requested_{false};
std::atomic<bool> running_{false};
};
class MujocoWorldDevice final : public device::AbstractDevice {
public:
explicit MujocoWorldDevice(config::MujocoWorldConfig config);
~MujocoWorldDevice() override;
device::DeviceKind kind() const noexcept override { return device::DeviceKind::MujocoWorld; }
std::string typeName() const override { return "MujocoWorldDevice"; }
bool init() override;
bool start() override;
bool stop() override;
std::shared_ptr<MujocoWorld> world() const { return world_; }
static std::shared_ptr<MujocoWorld> worldFor(const std::string& id);
private:
config::MujocoWorldConfig config_;
std::shared_ptr<MujocoWorld> world_;
static std::mutex registry_mutex_;
static std::unordered_map<std::string, std::weak_ptr<MujocoWorld>> registry_;
};
} // namespace cmvr::simulate
#endif // CMVR_ES_MUJOCO_WORLD_H

776
cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp Normal file
View File

@ -0,0 +1,776 @@
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
#include <algorithm>
#include <cctype>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <filesystem>
#include <limits>
#include <utility>
#include "common/base/logging/logger.h"
#include "common/config/config_files.h"
namespace cmvr::simulate {
namespace {
constexpr double kDefaultTimestep = 0.001;
bool isScalarJoint(const mjModel* model, const int joint_id)
{
if (model == nullptr || joint_id < 0 || joint_id >= model->njnt) {
return false;
}
return model->jnt_type[joint_id] == mjJNT_HINGE ||
model->jnt_type[joint_id] == mjJNT_SLIDE;
}
std::string lowerCopy(std::string value)
{
std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) {
return static_cast<char>(std::tolower(c));
});
return value;
}
bool containsToken(const std::string& value, const std::string& token)
{
return lowerCopy(value).find(token) != std::string::npos;
}
bool actuatorTargetsJoint(const mjModel* model, const int actuator_id, const int joint_id)
{
if (model == nullptr || actuator_id < 0 || actuator_id >= model->nu) {
return false;
}
return model->actuator_trntype[actuator_id] == mjTRN_JOINT &&
model->actuator_trnid[2 * actuator_id] == joint_id;
}
int actuatorPriority(const mjModel* model,
const int actuator_id,
const std::string& joint_name)
{
const char* actuator_name = mj_id2name(model, mjOBJ_ACTUATOR, actuator_id);
const std::string name = actuator_name == nullptr ? "" : actuator_name;
if (name == joint_name) {
return 0;
}
if (containsToken(name, "pos") || containsToken(name, "position")) {
return 1;
}
if (containsToken(name, "motor") || containsToken(name, "act")) {
return 2;
}
return 3;
}
double clampControl(const mjModel* model, const int actuator_id, const double value)
{
if (model == nullptr || actuator_id < 0 || actuator_id >= model->nu) {
return value;
}
if (!model->actuator_ctrllimited[actuator_id]) {
return value;
}
const double lower = model->actuator_ctrlrange[2 * actuator_id];
const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
return std::clamp(value, lower, upper);
}
} // namespace
std::mutex MujocoWorldDevice::registry_mutex_;
std::unordered_map<std::string, std::weak_ptr<MujocoWorld>> MujocoWorldDevice::registry_;
MujocoWorld::MujocoWorld(Options options)
{
load(options);
}
MujocoWorld::~MujocoWorld()
{
stop();
std::lock_guard<std::mutex> lock(mutex_);
clearModelLocked();
}
bool MujocoWorld::load(const Options& options)
{
stop();
std::lock_guard<std::mutex> lock(mutex_);
clearModelLocked();
options_ = options;
if (options_.model_path.empty()) {
setLastErrorLocked("MujocoWorld model_path is empty");
CMVR_LOG(ERROR) << last_error_;
return false;
}
std::string error;
model_ = loadModelFile(options_.model_path, error);
if (model_ == nullptr) {
setLastErrorLocked(error);
CMVR_LOG(ERROR) << "[MujocoWorld] load model failed: " << last_error_;
return false;
}
if (options_.timestep_s > 0.0) {
model_->opt.timestep = options_.timestep_s;
}
data_ = mj_makeData(model_);
if (data_ == nullptr) {
setLastErrorLocked("mj_makeData failed");
CMVR_LOG(ERROR) << "[MujocoWorld] " << last_error_;
clearModelLocked();
return false;
}
if (!buildJointMapLocked(options_.joints)) {
clearModelLocked();
return false;
}
mj_forward(model_, data_);
setLastErrorLocked("");
CMVR_LOG(INFO) << "[MujocoWorld] loaded model=" << options_.model_path
<< ", joints=" << joints_.size()
<< ", timestep=" << model_->opt.timestep;
return true;
}
bool MujocoWorld::start()
{
if (running_.load()) {
return true;
}
{
std::lock_guard<std::mutex> lock(mutex_);
if (model_ == nullptr || data_ == nullptr) {
setLastErrorLocked("MujocoWorld is not loaded");
CMVR_LOG(ERROR) << "[MujocoWorld] " << last_error_;
return false;
}
}
stop_requested_.store(false);
running_.store(true);
simulation_thread_ = std::thread(&MujocoWorld::simulationLoop, this);
return true;
}
void MujocoWorld::stop()
{
stop_requested_.store(true);
if (simulation_thread_.joinable()) {
simulation_thread_.join();
}
running_.store(false);
}
void MujocoWorld::reset()
{
std::lock_guard<std::mutex> lock(mutex_);
if (model_ == nullptr || data_ == nullptr) {
return;
}
mj_resetData(model_, data_);
mj_forward(model_, data_);
for (auto& [_, joint] : joints_) {
joint.position_reference = data_->qpos[joint.qpos_adr];
joint.target_position = joint.position_reference;
joint.target_velocity = 0.0;
joint.mode = TargetMode::HoldPosition;
}
}
bool MujocoWorld::isLoaded() const
{
std::lock_guard<std::mutex> lock(mutex_);
return model_ != nullptr && data_ != nullptr;
}
bool MujocoWorld::isRunning() const
{
return running_.load();
}
double MujocoWorld::timestep() const
{
std::lock_guard<std::mutex> lock(mutex_);
if (model_ == nullptr || model_->opt.timestep <= 0.0) {
return kDefaultTimestep;
}
return model_->opt.timestep;
}
const std::string& MujocoWorld::lastError() const
{
return last_error_;
}
const std::string& MujocoWorld::modelPath() const
{
return options_.model_path;
}
std::vector<std::string> MujocoWorld::jointNames() const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<std::string> names;
names.reserve(joints_.size());
for (const auto& [name, _] : joints_) {
names.push_back(name);
}
std::sort(names.begin(), names.end());
return names;
}
bool MujocoWorld::hasJoint(const std::string& joint_name) const
{
std::lock_guard<std::mutex> lock(mutex_);
return joints_.find(joint_name) != joints_.end();
}
bool MujocoWorld::getJointPosition(const std::string& joint_name, double& position) const
{
std::lock_guard<std::mutex> lock(mutex_);
const JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
position = data_->qpos[joint->qpos_adr];
return true;
}
bool MujocoWorld::getJointVelocity(const std::string& joint_name, double& velocity) const
{
std::lock_guard<std::mutex> lock(mutex_);
const JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
velocity = data_->qvel[joint->qvel_adr];
return true;
}
bool MujocoWorld::setJointPosition(const std::string& joint_name, const double position)
{
std::lock_guard<std::mutex> lock(mutex_);
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
data_->qpos[joint->qpos_adr] = position;
data_->qvel[joint->qvel_adr] = 0.0;
joint->position_reference = position;
joint->target_position = position;
joint->target_velocity = 0.0;
joint->mode = TargetMode::HoldPosition;
mj_forward(model_, data_);
return true;
}
bool MujocoWorld::setJointVelocity(const std::string& joint_name, const double velocity)
{
std::lock_guard<std::mutex> lock(mutex_);
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
data_->qvel[joint->qvel_adr] = velocity;
joint->position_reference = data_->qpos[joint->qpos_adr];
joint->target_velocity = velocity;
joint->mode = TargetMode::DirectVelocity;
return true;
}
bool MujocoWorld::setJointTargetPosition(const std::string& joint_name, const double position)
{
std::lock_guard<std::mutex> lock(mutex_);
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
if (joint->actuator_id < 0 && !joint->allow_state_control_fallback) {
setLastErrorLocked("position target requires an actuator: " + joint_name);
return false;
}
joint->target_position = position;
joint->position_reference = position;
joint->mode = joint->actuator_id >= 0
? TargetMode::ActuatorPosition
: TargetMode::DirectPosition;
return true;
}
bool MujocoWorld::setJointTargetState(const std::string& joint_name,
const double position,
const double velocity)
{
std::lock_guard<std::mutex> lock(mutex_);
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
joint->target_position = position;
joint->position_reference = position;
joint->target_velocity = velocity;
joint->mode = TargetMode::DirectState;
return true;
}
bool MujocoWorld::setJointTargetPositions(const std::vector<std::string>& joint_names,
const std::vector<double>& positions)
{
if (joint_names.size() != positions.size()) {
std::lock_guard<std::mutex> lock(mutex_);
setLastErrorLocked("joint target batch size mismatch");
return false;
}
std::lock_guard<std::mutex> lock(mutex_);
std::vector<JointHandle*> joints;
joints.reserve(joint_names.size());
for (const auto& joint_name : joint_names) {
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
if (joint->actuator_id < 0 && !joint->allow_state_control_fallback) {
setLastErrorLocked("position target requires an actuator: " + joint_name);
return false;
}
joints.push_back(joint);
}
for (std::size_t i = 0; i < joints.size(); ++i) {
JointHandle* joint = joints[i];
joint->target_position = positions[i];
joint->position_reference = positions[i];
joint->mode = joint->actuator_id >= 0
? TargetMode::ActuatorPosition
: TargetMode::DirectPosition;
}
return true;
}
bool MujocoWorld::setJointTargetStates(const std::vector<std::string>& joint_names,
const std::vector<double>& positions,
const std::vector<double>& velocities)
{
if (joint_names.size() != positions.size() || joint_names.size() != velocities.size()) {
std::lock_guard<std::mutex> lock(mutex_);
setLastErrorLocked("joint target state batch size mismatch");
return false;
}
std::lock_guard<std::mutex> lock(mutex_);
std::vector<JointHandle*> joints;
joints.reserve(joint_names.size());
for (const auto& joint_name : joint_names) {
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
joints.push_back(joint);
}
for (std::size_t i = 0; i < joints.size(); ++i) {
JointHandle* joint = joints[i];
joint->target_position = positions[i];
joint->position_reference = positions[i];
joint->target_velocity = velocities[i];
joint->mode = TargetMode::DirectState;
}
return true;
}
bool MujocoWorld::setJointTargetVelocity(const std::string& joint_name, const double velocity)
{
std::lock_guard<std::mutex> lock(mutex_);
JointHandle* joint = nullptr;
if (!findJointLocked(joint_name, joint)) {
return false;
}
if (joint->mode != TargetMode::DirectVelocity) {
joint->position_reference = data_->qpos[joint->qpos_adr];
}
joint->target_velocity = velocity;
joint->mode = TargetMode::DirectVelocity;
return true;
}
mjModel* MujocoWorld::model()
{
return model_;
}
mjData* MujocoWorld::data()
{
return data_;
}
const mjModel* MujocoWorld::model() const
{
return model_;
}
const mjData* MujocoWorld::data() const
{
return data_;
}
std::mutex& MujocoWorld::mutex() const
{
return mutex_;
}
mjModel* MujocoWorld::loadModelFile(const std::string& model_path, std::string& error)
{
error.clear();
if (model_path.empty()) {
error = "empty model filename";
return nullptr;
}
const std::filesystem::path path(model_path);
if (!std::filesystem::exists(path)) {
error = "model file does not exist: " + model_path;
return nullptr;
}
if (path.extension() == ".mjb") {
mjModel* model = mj_loadModel(model_path.c_str(), nullptr);
if (model == nullptr) {
error = "could not load binary model: " + model_path;
}
return model;
}
char load_error[1024] = {};
mjModel* model = mj_loadXML(model_path.c_str(), nullptr, load_error, sizeof(load_error));
if (model == nullptr) {
error = load_error[0] == '\0' ? "could not load XML model: " + model_path : load_error;
if (!error.empty() && error.back() == '\n') {
error.pop_back();
}
}
return model;
}
bool MujocoWorld::buildJointMapLocked(const std::vector<JointSpec>& joint_specs)
{
joints_.clear();
bool ok = true;
if (joint_specs.empty()) {
for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) {
if (!isScalarJoint(model_, joint_id)) {
continue;
}
const char* name = mj_id2name(model_, mjOBJ_JOINT, joint_id);
if (name == nullptr || name[0] == '\0') {
continue;
}
JointSpec joint_spec;
joint_spec.joint_name = name;
joint_spec.allow_state_control_fallback = options_.allow_state_control_fallback;
ok = addJointLocked(joint_spec) && ok;
}
} else {
for (const auto& joint_spec : joint_specs) {
ok = addJointLocked(joint_spec) && ok;
}
}
if (!ok) {
CMVR_LOG(ERROR) << "[MujocoWorld] build joint map failed: " << last_error_;
}
return ok;
}
bool MujocoWorld::addJointLocked(const JointSpec& joint_spec)
{
const std::string& joint_name = joint_spec.joint_name;
if (joint_name.empty()) {
setLastErrorLocked("empty Mujoco joint name");
return false;
}
const int joint_id = mj_name2id(model_, mjOBJ_JOINT, joint_name.c_str());
if (joint_id < 0) {
setLastErrorLocked("joint not found in Mujoco model: " + joint_name);
return false;
}
if (!isScalarJoint(model_, joint_id)) {
setLastErrorLocked("only hinge/slide scalar joints are supported: " + joint_name);
return false;
}
JointHandle joint;
joint.joint_id = joint_id;
joint.qpos_adr = model_->jnt_qposadr[joint_id];
joint.qvel_adr = model_->jnt_dofadr[joint_id];
joint.actuator_id = findJointActuatorLocked(joint_name, joint_id);
joint.allow_state_control_fallback =
joint_spec.allow_state_control_fallback || options_.allow_state_control_fallback;
joint.position_reference = data_->qpos[joint.qpos_adr];
joint.target_position = joint.position_reference;
if (options_.require_actuator && joint.actuator_id < 0) {
setLastErrorLocked("actuator not found for joint: " + joint_name);
return false;
}
joints_[joint_name] = joint;
return true;
}
int MujocoWorld::findJointActuatorLocked(const std::string& joint_name, const int joint_id) const
{
int best_actuator_id = -1;
int best_priority = std::numeric_limits<int>::max();
int match_count = 0;
for (int actuator_id = 0; actuator_id < model_->nu; ++actuator_id) {
if (!actuatorTargetsJoint(model_, actuator_id, joint_id)) {
continue;
}
++match_count;
const int priority = actuatorPriority(model_, actuator_id, joint_name);
if (priority < best_priority) {
best_priority = priority;
best_actuator_id = actuator_id;
}
}
if (match_count > 1 && best_actuator_id >= 0) {
const char* actuator_name = mj_id2name(model_, mjOBJ_ACTUATOR, best_actuator_id);
CMVR_LOG(DEBUG) << "[MujocoWorld] multiple actuators target joint=" << joint_name
<< ", selected=" << (actuator_name == nullptr ? "" : actuator_name);
}
return best_actuator_id;
}
bool MujocoWorld::findJointLocked(const std::string& joint_name, JointHandle*& joint)
{
auto it = joints_.find(joint_name);
if (it == joints_.end() || model_ == nullptr || data_ == nullptr) {
setLastErrorLocked("joint not available: " + joint_name);
joint = nullptr;
return false;
}
joint = &it->second;
return true;
}
bool MujocoWorld::findJointLocked(const std::string& joint_name, const JointHandle*& joint) const
{
auto it = joints_.find(joint_name);
if (it == joints_.end() || model_ == nullptr || data_ == nullptr) {
setLastErrorLocked("joint not available: " + joint_name);
joint = nullptr;
return false;
}
joint = &it->second;
return true;
}
void MujocoWorld::setLastErrorLocked(const std::string& error) const
{
last_error_ = error;
}
void MujocoWorld::clearModelLocked()
{
joints_.clear();
if (data_ != nullptr) {
mj_deleteData(data_);
data_ = nullptr;
}
if (model_ != nullptr) {
mj_deleteModel(model_);
model_ = nullptr;
}
}
void MujocoWorld::simulationLoop()
{
CMVR_LOG(INFO) << "[MujocoWorld] simulation started";
double dt = kDefaultTimestep;
{
std::lock_guard<std::mutex> lock(mutex_);
if (model_ != nullptr && model_->opt.timestep > 0.0) {
dt = model_->opt.timestep;
}
}
const double realtime_factor = options_.realtime_factor > 0.0 ? options_.realtime_factor : 1.0;
const auto period = std::chrono::duration<double>(dt / realtime_factor);
auto next_tick = std::chrono::steady_clock::now() + period;
while (!stop_requested_.load()) {
{
std::lock_guard<std::mutex> lock(mutex_);
if (model_ == nullptr || data_ == nullptr) {
break;
}
applyControlLocked();
mj_step(model_, data_);
applyDirectControlLocked(dt);
}
std::this_thread::sleep_until(next_tick);
next_tick += period;
const auto now = std::chrono::steady_clock::now();
if (next_tick < now) {
next_tick = now + period;
}
}
running_.store(false);
CMVR_LOG(INFO) << "[MujocoWorld] simulation stopped";
}
void MujocoWorld::applyControlLocked()
{
for (auto& [_, joint] : joints_) {
if (joint.qpos_adr < 0 || joint.qvel_adr < 0) {
continue;
}
if (joint.mode == TargetMode::ActuatorVelocity) {
joint.position_reference += joint.target_velocity * model_->opt.timestep;
data_->ctrl[joint.actuator_id] =
clampControl(model_, joint.actuator_id, joint.position_reference);
} else if (joint.mode == TargetMode::ActuatorPosition) {
joint.position_reference = joint.target_position;
data_->ctrl[joint.actuator_id] =
clampControl(model_, joint.actuator_id, joint.position_reference);
} else if (joint.actuator_id >= 0) {
joint.position_reference = data_->qpos[joint.qpos_adr];
data_->ctrl[joint.actuator_id] =
clampControl(model_, joint.actuator_id, joint.position_reference);
}
}
}
void MujocoWorld::applyDirectControlLocked(const double dt)
{
bool updated = false;
const double step = dt > 0.0 ? dt : model_->opt.timestep;
for (auto& [_, joint] : joints_) {
if (joint.qpos_adr < 0 || joint.qvel_adr < 0) {
continue;
}
if (joint.mode == TargetMode::DirectVelocity) {
joint.position_reference += joint.target_velocity * step;
data_->qpos[joint.qpos_adr] = joint.position_reference;
data_->qvel[joint.qvel_adr] = joint.target_velocity;
updated = true;
} else if (joint.mode == TargetMode::DirectPosition) {
joint.position_reference = joint.target_position;
data_->qpos[joint.qpos_adr] = joint.target_position;
data_->qvel[joint.qvel_adr] = 0.0;
updated = true;
} else if (joint.mode == TargetMode::DirectState) {
joint.position_reference = joint.target_position;
data_->qpos[joint.qpos_adr] = joint.target_position;
data_->qvel[joint.qvel_adr] = joint.target_velocity;
updated = true;
}
}
if (updated) {
mj_forward(model_, data_);
}
}
MujocoWorldDevice::MujocoWorldDevice(config::MujocoWorldConfig config)
: config_(std::move(config))
{
id_ = config_.id();
}
MujocoWorldDevice::~MujocoWorldDevice()
{
stop();
}
bool MujocoWorldDevice::init()
{
if (id_.empty()) {
CMVR_LOG(ERROR) << "[MujocoWorldDevice] id is empty";
return false;
}
if (world_ && world_->isLoaded()) {
return true;
}
MujocoWorld::Options options;
options.model_path = cmvr::ConfigHelper::resolveResourceFile(config_.model_path());
options.timestep_s = config_.timestep_s();
options.realtime_factor = config_.realtime_factor() > 0.0 ? config_.realtime_factor() : 1.0;
options.require_actuator = config_.require_actuator();
options.allow_state_control_fallback = config_.allow_state_control_fallback();
world_ = std::make_shared<MujocoWorld>();
if (!world_->load(options)) {
CMVR_LOG(ERROR) << "[MujocoWorldDevice] load failed, id=" << id_
<< ", error=" << world_->lastError();
world_.reset();
return false;
}
{
std::lock_guard<std::mutex> lock(registry_mutex_);
registry_[id_] = world_;
}
CMVR_LOG(INFO) << "[MujocoWorldDevice] initialized: " << id_;
return true;
}
bool MujocoWorldDevice::start()
{
if (!world_ && !init()) {
return false;
}
if (!world_->start()) {
CMVR_LOG(ERROR) << "[MujocoWorldDevice] start failed, id=" << id_
<< ", error=" << world_->lastError();
return false;
}
return true;
}
bool MujocoWorldDevice::stop()
{
if (world_) {
world_->stop();
}
return true;
}
std::shared_ptr<MujocoWorld> MujocoWorldDevice::worldFor(const std::string& id)
{
std::lock_guard<std::mutex> lock(registry_mutex_);
const auto it = registry_.find(id);
if (it == registry_.end()) {
return nullptr;
}
return it->second.lock();
}
} // namespace cmvr::simulate

17
cmvr-es/test/CMakeLists.txt Normal file
View File

@ -0,0 +1,17 @@
add_executable(mujoco_manual_ui_test
mujoco_manual_ui_test.cpp
)
target_compile_definitions(mujoco_manual_ui_test PRIVATE
CMVR_MANUAL_UI_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/cmvr_es.pb.txt"
)
target_link_libraries(mujoco_manual_ui_test
PRIVATE
gtest
gtest_main
pthread
cmvr_es::runtime
cmvr_es::device::arm
cmvr_es::device::camera
)

237
cmvr-es/test/mujoco_manual_ui_test.cpp Normal file
View File

@ -0,0 +1,237 @@
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cmath>
#include <limits>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <gtest/gtest.h>
#include "common/base/logging/logger.h"
#include "devices/arm/robot_arm.h"
#include "devices/camera/abstract_camera.h"
#include "runtime/include/cmvr_runtime.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
namespace {
std::string manualConfigPath()
{
if (const char* env = std::getenv("CMVR_MANUAL_UI_CONFIG_PATH")) {
return env;
}
return CMVR_MANUAL_UI_CONFIG_PATH;
}
std::string manualArmId()
{
if (const char* env = std::getenv("CMVR_MANUAL_ARM_ID")) {
return env;
}
return "mujoco_right_arm";
}
std::string manualRealArmId()
{
if (const char* env = std::getenv("CMVR_MANUAL_REAL_ARM_ID")) {
return env;
}
return "right_arm";
}
bool shouldRunRealArmTest()
{
const char* env = std::getenv("CMVR_RUN_REAL_ARM_TEST");
return env != nullptr && std::string(env) == "1";
}
struct ManualUiRunResult {
bool ok = true;
std::string error;
};
ManualUiRunResult runManualUiTest(
const std::shared_ptr<cmvr::device::RobotArm>& arm,
const std::string& arm_id,
const std::atomic_bool& stop_requested,
const std::shared_ptr<cmvr::simulate::MujocoWorld>& mujoco_world = nullptr)
{
if (stop_requested.load()) {
return {};
}
std::vector<double> q_target = {0, 1, 1.6, 1.6, -2.5, 0.12, 0.12};
cmvr::device::MotionOptions joint_options;
joint_options.velocity = 5.0;
joint_options.acceleration = 15.0;
joint_options.joint_velocity_limits.assign(7, 2.5);
CMVR_LOG(INFO) << "[MujocoManualUiTest] moveJ " << arm_id;
auto result = arm->moveJ(cmvr::device::JointPositionCommand{q_target}, joint_options);
if (!result.ok()) {
return {false, result.message};
}
if (stop_requested.load()) {
return {};
}
// std::this_thread::sleep_for(std::chrono::seconds(1));
//
// auto move_l_target = arm->getTcpPose(cmvr::device::FrameType::Base);
// // move_l_target.z += 1.50;
// move_l_target.ry += 0.5;
//
// cmvr::device::MotionOptions cartesian_options;
// cartesian_options.velocity = 0.19;
// cartesian_options.acceleration = 100.0;
// cartesian_options.jerk = 500.0;
//
// CMVR_LOG(INFO) << "[MujocoManualUiTest] moveL +X";
// result = arm->moveL(move_l_target, cartesian_options, cmvr::device::FrameType::Base);
// if (!result.ok()) {
// return {false, result.message};
// }
//
// if (stop_requested.load()) {
// return {};
// }
std::this_thread::sleep_for(std::chrono::seconds(1));
cmvr::device::CartesianVelocity speed_l_velocity;
speed_l_velocity.vz = 0.19;
// speed_l_velocity.wy = 0.19;
CMVR_LOG(INFO) << "[MujocoManualUiTest] speedL +X";
result = arm->speedL(speed_l_velocity, 100.0, 1.0, cmvr::device::FrameType::Base);
if (!result.ok()) {
return {false, result.message};
}
// auto last_pose = arm->getTcpPose(cmvr::device::FrameType::Base);
// auto last_time = std::chrono::steady_clock::now();
// auto read_sim_time = [&]() {
// if (!mujoco_world) {
// return std::numeric_limits<double>::quiet_NaN();
// }
// std::lock_guard<std::mutex> lock(mujoco_world->mutex());
// const auto* data = mujoco_world->data();
// return data ? data->time : std::numeric_limits<double>::quiet_NaN();
// };
// double last_sim_time = read_sim_time();
// const auto speed_l_end_time = last_time + std::chrono::seconds(2);
// while (!stop_requested.load() && std::chrono::steady_clock::now() < speed_l_end_time) {
// std::this_thread::sleep_for(std::chrono::milliseconds(50));
//
// const auto now = std::chrono::steady_clock::now();
// const auto pose = arm->getTcpPose(cmvr::device::FrameType::Base);
// const auto command_twist = arm->getSpeedLCommandTwistBase();
// const double command_norm =
// std::sqrt(command_twist.vx * command_twist.vx +
// command_twist.vy * command_twist.vy +
// command_twist.vz * command_twist.vz);
// const double dt = std::chrono::duration<double>(now - last_time).count();
// const double sim_time = read_sim_time();
// const double sim_dt =
// std::isfinite(sim_time) && std::isfinite(last_sim_time)
// ? sim_time - last_sim_time
// : std::numeric_limits<double>::quiet_NaN();
// const double sim_realtime_ratio =
// dt > 1e-6 && std::isfinite(sim_dt)
// ? sim_dt / dt
// : std::numeric_limits<double>::quiet_NaN();
// if (dt > 1e-6) {
// const double vx = (pose.x - last_pose.x) / dt;
// const double vy = (pose.y - last_pose.y) / dt;
// const double vz = (pose.z - last_pose.z) / dt;
// const double v_norm = std::sqrt(vx * vx + vy * vy + vz * vz);
// CMVR_LOG(INFO) << "[MujocoManualUiTest] tcp velocity base: vx="
// << vx << ", vy=" << vy << ", vz=" << vz
// << ", norm=" << v_norm
// << ", command_vx=" << command_twist.vx
// << ", command_vy=" << command_twist.vy
// << ", command_vz=" << command_twist.vz
// << ", command_norm=" << command_norm
// << ", wall_dt=" << dt;
// if (mujoco_world) {
// CMVR_LOG(INFO) << "[MujocoManualUiTest] sim timing: sim_dt="
// << sim_dt
// << ", sim_realtime_ratio=" << sim_realtime_ratio;
// }
// }
//
// last_pose = pose;
// last_time = now;
// last_sim_time = sim_time;
// }
// (void)arm->stopL(10);
return {};
}
} // namespace
TEST(MujocoManualUiTest, RunMujocoArm)
{
cmvr::Runtime runtime;
ASSERT_TRUE(runtime.init(manualConfigPath()));
ASSERT_TRUE(runtime.startTasks());
auto& devices = runtime.deviceManager();
const std::string arm_id = manualArmId();
auto arm = devices.getDevice<cmvr::device::RobotArm>(arm_id);
auto mujoco_camera = devices.getDevice<cmvr::device::AbstractCamera>("mujoco_hand_cam");
auto viewer = runtime.mujocoViewer("mujoco_viewer");
auto mujoco_world = cmvr::simulate::MujocoWorldDevice::worldFor("mujoco_world");
ASSERT_TRUE(arm);
ASSERT_TRUE(viewer);
std::atomic_bool stop_requested{false};
ManualUiRunResult worker_result;
std::thread worker([&]() {
std::this_thread::sleep_for(std::chrono::seconds(1));
worker_result = runManualUiTest(arm, arm_id, stop_requested, mujoco_world);
if (mujoco_camera && !stop_requested.load()) {
mujoco_camera->start();
}
});
viewer->runOnMainThread();
stop_requested.store(true);
if (worker.joinable()) {
worker.join();
}
EXPECT_TRUE(worker_result.ok) << worker_result.error;
runtime.shutdown();
}
TEST(MujocoManualUiTest, RunRealArm)
{
if (!shouldRunRealArmTest()) {
GTEST_SKIP() << "Set CMVR_RUN_REAL_ARM_TEST=1 to run the real arm manual UI test.";
}
cmvr::Runtime runtime;
ASSERT_TRUE(runtime.init(manualConfigPath()));
ASSERT_TRUE(runtime.startTasks());
const std::string arm_id = manualRealArmId();
auto arm = runtime.deviceManager().getDevice<cmvr::device::RobotArm>(arm_id);
ASSERT_TRUE(arm);
std::atomic_bool stop_requested{false};
const ManualUiRunResult result = runManualUiTest(arm, arm_id, stop_requested);
EXPECT_TRUE(result.ok) << result.error;
runtime.shutdown();
}

27
protos/cmvr/config/mujoco_config/mujoco_world_config.proto Normal file
View File

@ -0,0 +1,27 @@
syntax = "proto3";
package cmvr.config;
message MujocoWorldConfig {
string id = 1;
string model_path = 2;
double timestep_s = 3;
double realtime_factor = 4;
bool require_actuator = 5;
bool allow_state_control_fallback = 6;
}
message MujocoWorldRootConfig {
repeated MujocoWorldConfig worlds = 1;
}
message MujocoViewerConfig {
string id = 1;
string world_id = 2;
double camera_distance = 3;
double camera_azimuth = 4;
double camera_elevation = 5;
}
message MujocoViewerRootConfig {
repeated MujocoViewerConfig viewers = 1;
}