feat: update viewer to match 3.2.0 simulate UI

ubi-agni · Jul 31, 2024 · 6b3dc06 · 6b3dc06
1 parent e5d6e81
commit 6b3dc06
Show file tree

Hide file tree

Showing 4 changed files with 124 additions and 68 deletions.
diff --git a/mujoco_ros/include/mujoco_ros/viewer.h b/mujoco_ros/include/mujoco_ros/viewer.h
@@ -316,7 +316,7 @@ class Viewer
 
 	// 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", 1, nullptr, "AO" },
+	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" },
@@ -332,7 +332,7 @@ class Viewer
 		                              { mjITEM_END } };
 
 	// simulation section of UI
-	const mjuiDef def_simulation[14] = { { mjITEM_SECTION, "Simulation", 1, nullptr, "AS" },
+	const mjuiDef def_simulation[14] = { { mjITEM_SECTION, "Simulation", mjPRESERVE, nullptr, "AS" },
 		                                  { mjITEM_RADIO, "", 5, &this->pending_.ui_update_run, "Pause\nRun" },
 		                                  { mjITEM_BUTTON, "Reset", 2, nullptr, " #259" },
 		                                  { mjITEM_BUTTON, "Reload", 5, nullptr, "CL" },
@@ -341,15 +341,14 @@ class Viewer
 		                                  { 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 2" },
-		                                  { mjITEM_SLIDERNUM, "Noise rate", 5, &this->ctrl_noise_rate, "0 2" },
-
+		                                  { 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", 0, nullptr, "AW" },
+	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, " " },

diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp
@@ -615,6 +615,7 @@ bool MujocoEnv::initModelFromQueue()
 		ROS_DEBUG("\tSaved string content to VFS");
 	}
 
+	auto load_start = Viewer::Clock::now();
 	if (is_mjb) {
 		ROS_DEBUG("\tLoading mjb file");
 		mnew = mj_loadModel(queued_filename_, nullptr);
@@ -628,6 +629,16 @@ bool MujocoEnv::initModelFromQueue()
 		}
 	}
 
+	auto load_interval  = Viewer::Clock::now() - load_start;
+	double load_seconds = Seconds(load_interval).count();
+
+	if (!load_error_[0]) {
+		ROS_INFO_STREAM("Model loaded in " << load_seconds << " seconds");
+		if (load_seconds > 0.25) {
+			mju::sprintf_arr(load_error_, "Model loaded in %.2g seconds", load_seconds);
+		}
+	}
+
 	for (const auto viewer : connected_viewers_) {
 		mju::strcpy_arr(viewer->load_error, load_error_);
 	}

diff --git a/mujoco_ros/src/physics.cpp b/mujoco_ros/src/physics.cpp
@@ -150,7 +150,7 @@ void MujocoEnv::simUnpausedPhysics(mjtNum &syncSim, std::chrono::time_point<mujo
 	double slowdown = 100 / percentRealTime[settings_.real_time_index];
 
 	// Misalignment condition: distance from target sim time is bigger than syncsimalign
-	bool misaligned = mju_abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > syncMisalign;
+	bool misaligned = std::abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > syncMisalign;
 
 	// Out-of-sync (for any reason): reset sync times, step
 	if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || misaligned ||