Skip to content

Commit e06c769

Browse files
committed
Visualize virtual gantry in MuJoCo viewer (#7)
* Visualize virtual gantry in MuJoCo viewer * Make virtual gantry visualization more visible * Fix virtual gantry visualization rendering * Move gantry visualization behind torso * Render gantry at physical mount point * Test gantry visualization uses configured mount * Tune gantry visualization marker size * Keep gantry visualization state out of interface header * Address gantry visualization review comments * Move MuJoCo simulate changes to patch file
1 parent 8d613e3 commit e06c769

10 files changed

Lines changed: 375 additions & 1 deletion

File tree

‎mujoco_ros2_control/CMakeLists.txt‎

Lines changed: 19 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -142,9 +142,27 @@ else()
142142
OUTPUT_NAME simulate
143143
POSITION_INDEPENDENT_CODE ON
144144
)
145+
set(MUJOCO_SIMULATE_PATCHED_SOURCE "${CMAKE_CURRENT_BINARY_DIR}/generated/simulate.cc")
146+
set(MUJOCO_SIMULATE_PATCH_FILE "${CMAKE_CURRENT_SOURCE_DIR}/patches/mujoco_simulate_managed_user_scn.patch")
147+
find_program(PATCH_EXECUTABLE patch REQUIRED)
148+
file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated")
149+
configure_file("${MUJOCO_SIMULATE_DIR}/simulate.cc" "${MUJOCO_SIMULATE_PATCHED_SOURCE}" COPYONLY)
150+
execute_process(
151+
COMMAND "${PATCH_EXECUTABLE}" -p1 --forward --batch --input "${MUJOCO_SIMULATE_PATCH_FILE}"
152+
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated"
153+
RESULT_VARIABLE MUJOCO_SIMULATE_PATCH_RESULT
154+
OUTPUT_VARIABLE MUJOCO_SIMULATE_PATCH_OUTPUT
155+
ERROR_VARIABLE MUJOCO_SIMULATE_PATCH_ERROR
156+
)
157+
if(NOT MUJOCO_SIMULATE_PATCH_RESULT EQUAL 0)
158+
message(FATAL_ERROR
159+
"Could not patch MuJoCo Simulate for managed-mode user geoms:\n"
160+
"${MUJOCO_SIMULATE_PATCH_OUTPUT}\n${MUJOCO_SIMULATE_PATCH_ERROR}"
161+
)
162+
endif()
145163
target_sources(mujoco_simulate
146164
PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.h
147-
PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.cc ${MUJOCO_SIMULATE_DIR}/array_safety.h
165+
PRIVATE ${MUJOCO_SIMULATE_PATCHED_SOURCE} ${MUJOCO_SIMULATE_DIR}/array_safety.h
148166
)
149167
target_include_directories(mujoco_simulate PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR})
150168
target_link_libraries(mujoco_simulate PRIVATE lodepng mujoco::platform_ui_adapter ${MUJOCO_LIB})

‎mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ class MujocoSimulation
110110
*/
111111
using ResetCallback = std::function<void(bool fill_initial_state)>;
112112
using KeyCallback = std::function<bool(int key, int scancode, int action, int mods)>;
113+
using VisualizationCallback = std::function<void(const mjModel* model, const mjData* data, mjvScene* scene)>;
113114

114115
/**
115116
* @brief Construct a new Mujoco Simulation object. This is a no-op until initialization.
@@ -151,6 +152,9 @@ class MujocoSimulation
151152
/** Register a callback for viewer key events. An empty callback disables dispatch. */
152153
void set_key_callback(KeyCallback callback);
153154

155+
/** Register a callback that appends plugin-owned viewer geoms. */
156+
void set_visualization_callback(VisualizationCallback callback);
157+
154158
/**
155159
* @brief Start the physics thread. Must be called after load_model().
156160
*/
@@ -521,6 +525,9 @@ class MujocoSimulation
521525
ResetCallback reset_callback_;
522526
std::mutex key_callback_mutex_;
523527
KeyCallback key_callback_;
528+
VisualizationCallback visualization_callback_;
529+
mjvScene plugin_visualization_scene_{};
530+
bool plugin_visualization_scene_initialized_{ false };
524531
};
525532

526533
} // namespace mujoco_ros2_control
Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
--- a/simulate.cc
2+
+++ b/simulate.cc
3+
@@ -2161,6 +2161,27 @@ void Simulate::Sync(bool state_only) {
4+
// update scene or sync data from user in passive mode
5+
if (!is_passive_) {
6+
mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
7+
+
8+
+ // append geoms from user_scn to scratch space
9+
+ if (user_scn) {
10+
+ user_scn_geoms_.clear();
11+
+ user_scn_geoms_.reserve(user_scn->ngeom);
12+
+ for (int i = 0; i < user_scn->ngeom; ++i) {
13+
+ user_scn_geoms_.push_back(user_scn->geoms[i]);
14+
+ }
15+
+ }
16+
+
17+
+ // pick up rendering flags changed via user_scn
18+
+ if (user_scn) {
19+
+ for (int i = 0; i < mjNRNDFLAG; ++i) {
20+
+ if (user_scn->flags[i] != user_scn_flags_prev_[i]) {
21+
+ scn.flags[i] = user_scn->flags[i];
22+
+ pending_.ui_update_rendering = true;
23+
+ }
24+
+ }
25+
+ Copy(user_scn->flags, scn.flags);
26+
+ Copy(user_scn_flags_prev_, user_scn->flags);
27+
+ }
28+
} else {
29+
if (state_only) {
30+
int state_size = mj_stateSize(m_, mjSTATE_INTEGRATION);
31+
@@ -2400,6 +2421,27 @@ void Simulate::LoadOnRenderThread() {
32+
// update scene in managed mode, in passive mode copy data from user (update in RenderLoop)
33+
if (!is_passive_) {
34+
mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
35+
+
36+
+ // append geoms from user_scn to scratch space
37+
+ if (user_scn) {
38+
+ user_scn_geoms_.clear();
39+
+ user_scn_geoms_.reserve(user_scn->ngeom);
40+
+ for (int i = 0; i < user_scn->ngeom; ++i) {
41+
+ user_scn_geoms_.push_back(user_scn->geoms[i]);
42+
+ }
43+
+ }
44+
+
45+
+ // pick up rendering flags changed via user_scn
46+
+ if (user_scn) {
47+
+ for (int i = 0; i < mjNRNDFLAG; ++i) {
48+
+ if (user_scn->flags[i] != user_scn_flags_prev_[i]) {
49+
+ scn.flags[i] = user_scn->flags[i];
50+
+ pending_.ui_update_rendering = true;
51+
+ }
52+
+ }
53+
+ Copy(user_scn->flags, scn.flags);
54+
+ Copy(user_scn_flags_prev_, user_scn->flags);
55+
+ }
56+
} else {
57+
mjopt_prev_ = m_->opt;
58+
opt_prev_ = opt;
59+
@@ -2864,6 +2906,19 @@ void Simulate::RenderLoop() {
60+
// update scene, doing a full sync if in fully managed mode
61+
if (!is_passive_) {
62+
Sync();
63+
+
64+
+ // add user geoms to scene
65+
+ int nusergeom = user_scn_geoms_.size();
66+
+ int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom);
67+
+ if (ngeom < nusergeom) {
68+
+ mj_warning(this->d_, mjWARN_VGEOMFULL, this->scn.maxgeom);
69+
+ }
70+
+ std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(),
71+
+ ngeom * sizeof(mjvGeom));
72+
+ for (int i = 0; i < ngeom; ++i) {
73+
+ this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i;
74+
+ }
75+
+ this->scn.ngeom += ngeom;
76+
} else if (m_passive_ && d_passive_) {
77+
// the user has called Sync() in their code
78+
mjv_updateScene(m_passive_, d_passive_,
79+
@@ -2877,6 +2932,9 @@ void Simulate::RenderLoop() {
80+
}
81+
std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(),
82+
ngeom * sizeof(mjvGeom));
83+
+ for (int i = 0; i < ngeom; ++i) {
84+
+ this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i;
85+
+ }
86+
this->scn.ngeom += ngeom;
87+
}
88+
} // MutexLock (unblocks simulation thread)

‎mujoco_ros2_control/src/mujoco_simulation.cpp‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -516,6 +516,16 @@ MujocoSimulation::~MujocoSimulation()
516516
{
517517
shutdown();
518518

519+
if (plugin_visualization_scene_initialized_)
520+
{
521+
if (sim_)
522+
{
523+
sim_->user_scn = nullptr;
524+
}
525+
mjv_freeScene(&plugin_visualization_scene_);
526+
plugin_visualization_scene_initialized_ = false;
527+
}
528+
519529
// Cleanup data and the model, if they haven't been
520530
if (mj_data_)
521531
{
@@ -734,6 +744,14 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin
734744
publish_control_state();
735745
}
736746
}
747+
if (!headless_)
748+
{
749+
mjv_defaultScene(&plugin_visualization_scene_);
750+
mjv_makeScene(mj_model_, &plugin_visualization_scene_, 32);
751+
plugin_visualization_scene_initialized_ = true;
752+
sim_->user_scn = &plugin_visualization_scene_;
753+
}
754+
737755
if (!mj_data_ || !snapshot_write_ || !snapshot_read_)
738756
{
739757
RCLCPP_FATAL(get_logger(), "Could not allocate mjData for '%s'", model_path_.c_str());
@@ -780,6 +798,12 @@ void MujocoSimulation::set_key_callback(KeyCallback callback)
780798
key_callback_ = std::move(callback);
781799
}
782800

801+
void MujocoSimulation::set_visualization_callback(VisualizationCallback callback)
802+
{
803+
std::lock_guard<std::mutex> lock(key_callback_mutex_);
804+
visualization_callback_ = std::move(callback);
805+
}
806+
783807
void MujocoSimulation::start_physics_thread()
784808
{
785809
// Disable the rangefinder flag at startup so that we don't get the yellow lines.
@@ -1619,6 +1643,17 @@ void MujocoSimulation::update_sim_display()
16191643
// holding sim_->mtx (Simulate::Render runs outside the lock). newtextrequest is
16201644
// the single-producer/single-consumer flag: 0 = physics may write, 1 = render
16211645
// may consume. Skip if the last update hasn't been consumed yet.
1646+
VisualizationCallback visualization_callback;
1647+
{
1648+
std::lock_guard<std::mutex> lock(key_callback_mutex_);
1649+
visualization_callback = visualization_callback_;
1650+
}
1651+
if (visualization_callback && plugin_visualization_scene_initialized_)
1652+
{
1653+
plugin_visualization_scene_.ngeom = 0;
1654+
visualization_callback(mj_model_, mj_data_, &plugin_visualization_scene_);
1655+
}
1656+
16221657
if (sim_->newtextrequest.load(std::memory_order_acquire) != 0)
16231658
{
16241659
return;

‎mujoco_ros2_control/src/mujoco_system_interface.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -254,6 +254,7 @@ MujocoSystemInterface::~MujocoSystemInterface()
254254
if (simulation_)
255255
{
256256
simulation_->set_key_callback({});
257+
simulation_->set_visualization_callback({});
257258
}
258259

259260
for (auto& plugin : plugin_instances_)
@@ -2364,6 +2365,13 @@ void MujocoSystemInterface::load_mujoco_plugins()
23642365
}
23652366
return consumed;
23662367
});
2368+
2369+
simulation_->set_visualization_callback([this](const mjModel* model, const mjData* data, mjvScene* scene) {
2370+
for (auto& plugin : plugin_instances_)
2371+
{
2372+
plugin->update_visualization(model, data, scene);
2373+
}
2374+
});
23672375
}
23682376
catch (const pluginlib::PluginlibException& ex)
23692377
{

‎mujoco_ros2_control_plugins/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,10 @@ if(BUILD_TESTING)
166166
"$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
167167
"LINKER:--pop-state"
168168
)
169+
ament_add_gtest(test_plugin_visualization test/test_plugin_visualization.cpp)
170+
target_link_libraries(test_plugin_visualization
171+
mujoco_ros2_control_plugins_impl
172+
)
169173
endif()
170174

171175
# Install include directory

‎mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,18 @@ class MuJoCoROS2ControlPluginBase
7373
{
7474
}
7575

76+
/**
77+
* @brief Called when the MuJoCo viewer display is refreshed.
78+
* @param model Pointer to the MuJoCo model
79+
* @param data Pointer to the MuJoCo data
80+
* @param scene Scene where the plugin can append viewer-only geoms
81+
* @note Override to add lightweight viewer visualization. This must not mutate
82+
* physics state.
83+
*/
84+
virtual void update_visualization(const mjModel* /*model*/, const mjData* /*data*/, mjvScene* /*scene*/)
85+
{
86+
}
87+
7688
/**
7789
* @brief Called from the GLFW UI thread on every key event.
7890
* @return true if the event was consumed (caller should not forward to the MuJoCo viewer).

‎mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,12 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase
3838
void update(const mjModel* model, mjData* data) override;
3939
void reset() override;
4040
void cleanup() override;
41+
void update_visualization(const mjModel* model, const mjData* data, mjvScene* scene) override;
4142
bool on_key(int key, int scancode, int action, int mods) override;
4243

4344
private:
45+
friend class VirtualGantryPluginTestAccessor;
46+
4447
rclcpp::Node::SharedPtr node_;
4548

4649
// Target body (attachment point on the robot).

‎mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -219,6 +219,46 @@ void VirtualGantryPlugin::reset()
219219
last_update_time_ = -1.0;
220220
}
221221

222+
void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const mjData* data, mjvScene* scene)
223+
{
224+
std::lock_guard<std::mutex> lock(state_mutex_);
225+
226+
if (!enabled_ || !spawn_pos_captured_ || body_id_ < 0 || !data || !data->xpos || !data->xmat || !scene ||
227+
!scene->geoms || scene->ngeom >= scene->maxgeom)
228+
{
229+
return;
230+
}
231+
232+
const double* xpos = &data->xpos[body_id_ * 3];
233+
const double* xmat = &data->xmat[body_id_ * 9];
234+
235+
mjtNum attach_pos[3];
236+
for (int i = 0; i < 3; ++i)
237+
{
238+
attach_pos[i] = xpos[i] + xmat[i * 3 + 0] * body_offset_[0] + xmat[i * 3 + 1] * body_offset_[1] +
239+
xmat[i * 3 + 2] * body_offset_[2];
240+
}
241+
242+
const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1], anchor_pos_[2] };
243+
const float rope_rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f };
244+
245+
mjvGeom* geom = &scene->geoms[scene->ngeom++];
246+
mjv_initGeom(geom, mjGEOM_CAPSULE, nullptr, nullptr, nullptr, rope_rgba);
247+
geom->category = mjCAT_DECOR;
248+
mjv_connector(geom, mjGEOM_CAPSULE, 0.0125, anchor_pos, attach_pos);
249+
250+
if (scene->ngeom >= scene->maxgeom)
251+
{
252+
return;
253+
}
254+
255+
const mjtNum anchor_size[3] = { 0.04, 0.0, 0.0 };
256+
const float anchor_rgba[4] = { 1.0f, 0.0f, 0.0f, 1.0f };
257+
mjvGeom* anchor_geom = &scene->geoms[scene->ngeom++];
258+
mjv_initGeom(anchor_geom, mjGEOM_SPHERE, anchor_size, anchor_pos, nullptr, anchor_rgba);
259+
anchor_geom->category = mjCAT_DECOR;
260+
}
261+
222262
bool VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/)
223263
{
224264
// GLFW constants (stable values matching glfw3.h; avoids adding glfw as a plugin dependency).

0 commit comments

Comments
 (0)