diff --git a/CHANGELOG.md b/CHANGELOG.md index bb49d6a..128579c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ The default is `min(#available_threads - 1, 4)`. * Added profiling of plugin load and callback execution times. For each callback an EMA with sensitivity of 1000 steps is computed. In case a plugin has lower frequency than simulation step size, the callback should set `skip_ema_ = true` when skipping computations. Loading and reset times are reported in the server debug log. All plugin stats can be retrieved by the `get_plugin_stats` service call. * Added ros laser plugin. +* Added dynamic reconfigure for all MuJoCo settings for fast model testing. ### Fixed * Added missing call to render callbacks in viewer. While the callbacks were still being run for offscreen rendering, the viewer did not render additional geoms added by plugins. diff --git a/mujoco_ros/CMakeLists.txt b/mujoco_ros/CMakeLists.txt index 63cee17..d809cfb 100644 --- a/mujoco_ros/CMakeLists.txt +++ b/mujoco_ros/CMakeLists.txt @@ -36,6 +36,7 @@ find_package(catkin REQUIRED COMPONENTS image_transport camera_info_manager sensor_msgs + dynamic_reconfigure ) # cmake-format: off @@ -75,6 +76,10 @@ find_library(GLFW libglfw.so.3) # # * uncomment the "generate_dynamic_reconfigure_options" section below # # and list every .cfg file to be processed +## Dynamic Reconfigure +generate_dynamic_reconfigure_options( + cfg/SimParams.cfg +) # ################################## # # catkin specific configuration ## @@ -98,6 +103,7 @@ catkin_package( image_transport camera_info_manager sensor_msgs + dynamic_reconfigure INCLUDE_DIRS include LIBRARIES @@ -108,6 +114,9 @@ catkin_package( add_subdirectory(src) +# Depend on gencfg to ensure build before lib +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) + # ############ # # Install ## # ############ diff --git a/mujoco_ros/cfg/SimParams.cfg b/mujoco_ros/cfg/SimParams.cfg new file mode 100755 index 0000000..3912412 --- /dev/null +++ b/mujoco_ros/cfg/SimParams.cfg @@ -0,0 +1,201 @@ +#!/usr/bin/env python +PACKAGE = "mujoco_ros" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("running", bool_t, 0, "Pause/Resume simulation", True) +gen.add("admin_hash", str_t, 0, "Admin hash for evaluation mode", "") + +physics = gen.add_group("Physics Parameters", type="tab") +alg_params = gen.add_group("Algorithmic Parameters", type="tab") +phy_params = gen.add_group("Physical Parameters", type="tab") +disable_flags = gen.add_group("Disable Flags", type="tab") +enable_flags = gen.add_group("Enable Flags", type="tab") +contact = gen.add_group("Contact Override", type="tab") + +integrator_enum = gen.enum( + [ + gen.const( + "Implicitfast", + int_t, + 3, + "Implicit-in-velocity Euler with Cholesky. The implicitfast integrator has similar computational cost to Euler, yet provides increased stability, and is therefore a strict improvement. It is the recommended integrator for most models.", + ), + gen.const( + "Implicit", + int_t, + 2, + "Implicit-in-velocity Euler. The benefit over implicitfast is the implicit integration of Coriolis and centripetal forces, including gyroscopic forces. The most common case where integrating such forces implicitly leads to noticable improvement is when free objects with assymetric inertia are spinning quickly.", + ), + gen.const( + "RK4", + int_t, + 1, + "Runge-Kutta 4th order multi-step. This integrator is best for systems which are energy conserving, or almost energy-conserving.", + ), + gen.const( + "Euler", + int_t, + 0, + "Semi-implicit Euler with joint damping. Use Euler for compatibillity with older models.", + ), + ], "Enum to set the numerical integrator." +) + +friction_enum = gen.enum( + [ + gen.const( + "Elliptic", + int_t, + 1, + "Model friction as a smooth, continuous force constraint within an elliptical base within physically realistic bounds.", + ), + gen.const( + "Pyramidal", + int_t, + 0, + "Approximate the friction constraint using a pyramidal base, which simplifies computation but may introduce discontinuities or inaccuracies", + ), + ], "Enum to set the friction cone type." +) + +jacobian_enum = gen.enum( + [ + gen.const("Dense", int_t, 0, "Dense Jacobian."), + gen.const("Sparse", int_t, 1, "Sparse Jacobian"), + gen.const("Auto", int_t, 2, "Dense up to 60 DoF, and sparse over 60 DoF"), + ], "Enum to set the Jacobian type." +) + +solver_enum = gen.enum( + [ + gen.const( + "Newton", + int_t, + 2, + "This algorithm implements the exact Newton method, with analytical second-order derivatives and Cholesky factorization of the Hessian. The line-search is the same as in the CG method. It is the default solver.", + ), + gen.const( + "PGS", + int_t, + 0, + "PGS uses the dual formulation. Unlike gradient-based methods which improve the solution along oblique directions, Gauss-Seidel works on one scalar component at a time, and sets it to its optimal value given the current values of all other components. One sweep of PGS has the computational complexity of one matrix-vector multiplication (although the constants are larger). It has first-order convergence but nevertheless makes rapid progress in a few iterations.", + ), + gen.const( + "CG", + int_t, + 1, + "This algorithm uses the non-linear conjugate gradient method with the Polak-Ribiere-Plus formula. Line-search is exact, using Newton’s method in one dimension, with analytical second derivatives.", + ), + ], "Enum to set the constraint solver." +) + +physics.add( + "integrator", + int_t, + 0, + "Numerical integrator for forward dynamics in continuous time.", + 0, + 0, + 3, + edit_method=integrator_enum, +) + +physics.add( + "cone", + int_t, + 0, + "Type of contact friction cone.", + 1, + 0, + 1, + edit_method=friction_enum, +) + +physics.add( + "jacobian", + int_t, + 0, + "Type Jacobian and matrices computed from it.", + 2, + 0, + 2, + edit_method=jacobian_enum, +) + +physics.add( + "solver", + int_t, + 0, + "Type of solver to use for constraint forces.", + 2, + 0, + 2, + edit_method=solver_enum, +) + +### Algorithmic params + +alg_params.add("timestep", double_t, 0, "Simulation step in seconds", 1e-3, 1e-10) +alg_params.add( + "iterations", + int_t, + 0, + "Maximum number of iterations of the constraint solver. When the warmstart attribute of flag is enabled (which is the default), accurate results are obtained with fewer iterations. Larger and more complex systems with many interacting constraints require more iterations. Note that mjData.solver contains statistics about solver convergence, also shown in the profiler.", + 100, + 1, +) +alg_params.add("tolerance", double_t, 0, "Tolerance threshold used for early termination of the iterative solver. For PGS, the threshold is applied to the cost improvement between two iterations. For CG and Newton, it is applied to the smaller of the cost improvement and the gradient norm. Set the tolerance to 0 to disable early termination.", 1e-8, 0) +alg_params.add("ls_iter", int_t, 0, "Maximum number of linesearch iterations performed by CG/Newton constraint solvers. Ensures that at most iterations times ls_iterations linesearch iterations are performed during each constraint solve.", 50, 1) +alg_params.add("ls_tol", double_t, 0, "Tolerance threshold used for early termination of the linesearch algorithm.", 0.01, 0) +alg_params.add("noslip_iter", int_t, 0, "Maximum number of iterations of the Noslip solver. This is a post-processing step executed after the main solver. It uses a modified PGS method to suppress slip/drift in friction dimensions resulting from the soft-constraint model. The default setting 0 disables this post-processing step.", 0, 0) +alg_params.add("noslip_tol", double_t, 0, "Tolerance threshold used for early termination of the Noslip solver.", 1e-6, 0) +alg_params.add("mpr_iter", int_t, 0, "Maximum number of iterations of the algorithm used for convex collisions (CCD). This rarely needs to be adjusted, except in situations where some geoms have very large aspect ratios.", 50, 1) +alg_params.add("mpr_tol", double_t, 0, "Tolerance threshold used for early termination of the convex collision algorithm.", 1e-6, 0) +alg_params.add("sdf_iter", int_t, 0, "Number of iterations used for Signed Distance Field collisions (per initial point).", 10, 1) +alg_params.add("sdf_init", int_t, 0, "Number of starting points used for finding contacts with Signed Distance Field collisions.", 40, 1) + +#### Physical Params +phy_params.add("gravity", str_t, 0, "Gravitational acceleration vector. In the default world orientation the Z-axis points up.", "0 0 -9.81") +phy_params.add("wind", str_t, 0, "Velocity vector of the medium (i.e., wind). This vector is subtracted from the 3D translational velocity of each body, and the result is used to compute viscous, lift and drag forces acting on the body.", "0 0 0") +phy_params.add("magnetic", str_t, 0, "Global magnetic flux. This vector is used by magnetometer sensors, which return the magnetic flux at their pose expressed in their frame.", "0 -0.5 0") +phy_params.add("density", double_t, 0, "Density of the medium, not to be confused with the geom density used to infer masses and inertias. This parameter is used to simulate lift and drag forces, which scale quadratically with velocity. In SI units the density of air is around 1.2 while the density of water is around 1000 depending on temperature. Setting density to 0 disables lift and drag forces.", 0.) +phy_params.add("viscosity", double_t, 0, "Viscosity of the medium. This parameter is used to simulate viscous forces, which scale linearly with velocity. In SI units the visocsity of air is around 0.00002 while the viscosity of water is around 0.00009 depending on temperature. Setting viscosity to 0 disables viscous forces. Note that the default Euler integrator handles damping in the joints implicitly - which improves stability and accuracy. It does not presently do this with body viscosity. Therefore, if the goal is merely to create a damped simulation (as opposed to modeling the specific effects of viscosity), we revommend using joint damping rather thad body viscosity, or switching to the implicit or implicitfast integrators.", 0.) +phy_params.add("impratio", double_t, 0, "This attribute determines the ratio of frictional-to-normal constraint impedance for elliptic ", 1.) + + +### Disable Flags +disable_flags.add("constraint_disabled", bool_t, 0, "", False) +disable_flags.add("equality_disabled", bool_t, 0, "", False) +disable_flags.add("frictionloss_disabled", bool_t, 0, "", False) +disable_flags.add("limit_disabled", bool_t, 0, "", False) +disable_flags.add("contact_disabled", bool_t, 0, "", False) +disable_flags.add("passive_disabled", bool_t, 0, "", False) +disable_flags.add("gravity_disabled", bool_t, 0, "This flag causes the gravitational acceleration vector in mjOption to be replaced with (0 0 0) at runtime, without changing the value in mjOption. Once the flag is re-enabled, the value in mjOption is used.", False) +disable_flags.add("clampctrl_disabled", bool_t, 0, "", False) +disable_flags.add("warmstart_disabled", bool_t, 0, "", False) +disable_flags.add("filterparent_disabled", bool_t, 0, "", False) +disable_flags.add("actuation_disabled", bool_t, 0, "", False) +disable_flags.add("refsafe_disabled", bool_t, 0, "", False) +disable_flags.add("sensor_disabled", bool_t, 0, "", False) +disable_flags.add("midphase_disabled", bool_t, 0, "", False) +disable_flags.add("eulerdamp_disabled", bool_t, 0, "", False) + +### Enable Flags +enable_flags.add("override_contacts", bool_t, 0, "", False) +enable_flags.add("energy", bool_t, 0, "", False) +enable_flags.add("fwd_inv", bool_t, 0, "", False) +enable_flags.add("inv_discrete", bool_t, 0, "", False) +enable_flags.add("multiccd", bool_t, 0, "", False) +enable_flags.add("island", bool_t, 0, "", False) + +### Contact Override +contact.add("margin", double_t, 0, "", 0.) +contact.add("solimp", str_t, 0, "", "0.9 0.95 0.00") +contact.add("solref", str_t, 0, "", "0.02 1.0") +contact.add("friction", str_t, 0, "", "1 1 0.05 0.") + + +exit(gen.generate(PACKAGE, "mujoco_server", "SimParams")) diff --git a/mujoco_ros/include/mujoco_ros/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 6bfcfae..0f02a63 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -85,6 +85,9 @@ #include #include +#include +#include + #include #include @@ -193,6 +196,8 @@ class MujocoEnv std::atomic_int speed_changed = { 0 }; std::atomic_int env_steps_request = { 0 }; + std::atomic_int settings_changed = { 0 }; + // Must be set to true before loading a new model from python std::atomic_int is_python_request = { 0 }; } settings_; @@ -318,6 +323,12 @@ class MujocoEnv void notifyGeomChanged(const int geom_id); + dynamic_reconfigure::Server *param_server_; + mujoco_ros::SimParamsConfig sim_params_; + boost::recursive_mutex sim_params_mutex_; + void dynparamCallback(mujoco_ros::SimParamsConfig &config, uint32_t level); + void updateDynamicParams(); + std::vector service_servers_; std::unique_ptr> action_step_; diff --git a/mujoco_ros/package.xml b/mujoco_ros/package.xml index 7913dc8..1c5ae3e 100644 --- a/mujoco_ros/package.xml +++ b/mujoco_ros/package.xml @@ -14,6 +14,7 @@ catkin roscpp + dynamic_reconfigure actionlib pluginlib mujoco_ros_msgs diff --git a/mujoco_ros/src/callbacks.cpp b/mujoco_ros/src/callbacks.cpp index 49eba8c..718f1e4 100644 --- a/mujoco_ros/src/callbacks.cpp +++ b/mujoco_ros/src/callbacks.cpp @@ -76,7 +76,7 @@ void MujocoEnv::setupServices() service_servers_.emplace_back(nh_->advertiseService( "load_initial_joint_states", [&](auto /*&req*/, auto /*&res*/) { - std::lock_guard lock(physics_thread_mutex_); + boost::lock_guard lock(physics_thread_mutex_); loadInitialJointStates(); return true; })); @@ -90,6 +90,208 @@ void MujocoEnv::setupServices() action_step_ = std::make_unique>( *nh_, "step", boost::bind(&MujocoEnv::onStepGoal, this, boost::placeholders::_1), false); action_step_->start(); + + param_server_ = new dynamic_reconfigure::Server(sim_params_mutex_, *nh_); + param_server_->setCallback( + boost::bind(&MujocoEnv::dynparamCallback, this, boost::placeholders::_1, boost::placeholders::_2)); +} + +// Helper function to convert a double array to a space-delimited string +void arr_to_string(const mjtNum *arr, int size, std::string &str) +{ + str.clear(); + for (int i = 0; i < size; ++i) { + str += std::to_string(arr[i]); + if (i < size - 1) { + str += " "; + } + } +} + +// TODO(dleins): once changes from python bindings are merged, use MujocoEnvSettings struct instead of passing all these +// arguments +void readSimParams(SimParamsConfig &config, mjModel *model_, bool is_running, const std::string &admin_hash) +{ + if (model_ != nullptr) { + config.running = is_running; + ROS_WARN_STREAM("admin_hash: " << admin_hash); + config.admin_hash = admin_hash; + ROS_WARN_STREAM("admin_hash: " << config.admin_hash); + + config.integrator = model_->opt.integrator; + config.cone = model_->opt.cone; + config.jacobian = model_->opt.jacobian; + config.solver = model_->opt.solver; + + config.timestep = model_->opt.timestep; + config.iterations = model_->opt.iterations; + config.tolerance = model_->opt.tolerance; + config.ls_iter = model_->opt.ls_iterations; + config.ls_tol = model_->opt.ls_tolerance; + config.noslip_iter = model_->opt.noslip_iterations; + config.noslip_tol = model_->opt.noslip_tolerance; + config.mpr_iter = model_->opt.mpr_iterations; + config.mpr_tol = model_->opt.mpr_tolerance; + config.sdf_iter = model_->opt.sdf_iterations; + config.sdf_init = model_->opt.sdf_initpoints; + + std::string gravity; + arr_to_string(model_->opt.gravity, 3, gravity); + config.gravity = gravity; + + std::string wind; + arr_to_string(model_->opt.wind, 3, wind); + config.wind = wind; + + std::string magnetic; + arr_to_string(model_->opt.magnetic, 3, magnetic); + config.magnetic = magnetic; + + config.density = model_->opt.density; + config.viscosity = model_->opt.viscosity; + config.impratio = model_->opt.impratio; + + config.constraint_disabled = ((model_->opt.disableflags & (1 << 0)) != 0); + config.equality_disabled = ((model_->opt.disableflags & (1 << 1)) != 0); + config.frictionloss_disabled = ((model_->opt.disableflags & (1 << 2)) != 0); + config.limit_disabled = ((model_->opt.disableflags & (1 << 3)) != 0); + config.contact_disabled = ((model_->opt.disableflags & (1 << 4)) != 0); + config.passive_disabled = ((model_->opt.disableflags & (1 << 5)) != 0); + config.gravity_disabled = ((model_->opt.disableflags & (1 << 6)) != 0); + config.clampctrl_disabled = ((model_->opt.disableflags & (1 << 7)) != 0); + config.warmstart_disabled = ((model_->opt.disableflags & (1 << 8)) != 0); + config.filterparent_disabled = ((model_->opt.disableflags & (1 << 9)) != 0); + config.actuation_disabled = ((model_->opt.disableflags & (1 << 10)) != 0); + config.refsafe_disabled = ((model_->opt.disableflags & (1 << 11)) != 0); + config.sensor_disabled = ((model_->opt.disableflags & (1 << 12)) != 0); + config.midphase_disabled = ((model_->opt.disableflags & (1 << 13)) != 0); + config.eulerdamp_disabled = ((model_->opt.disableflags & (1 << 14)) != 0); + + config.override_contacts = ((model_->opt.enableflags & (1 << 0)) != 0); + config.energy = ((model_->opt.enableflags & (1 << 1)) != 0); + config.fwd_inv = ((model_->opt.enableflags & (1 << 2)) != 0); + config.inv_discrete = ((model_->opt.enableflags & (1 << 3)) != 0); + config.multiccd = ((model_->opt.enableflags & (1 << 4)) != 0); + config.island = ((model_->opt.enableflags & (1 << 5)) != 0); + + config.margin = model_->opt.o_margin; + + std::string solimp; + arr_to_string(model_->opt.o_solimp, mjNIMP, solimp); + config.solimp = solimp; + + std::string solref; + arr_to_string(model_->opt.o_solref, mjNREF, solref); + config.solref = solref; + + std::string friction; + arr_to_string(model_->opt.o_friction, 5, friction); + config.friction = friction; + } +} + +void MujocoEnv::updateDynamicParams() +{ + boost::recursive_mutex::scoped_lock lk(sim_params_mutex_); + SimParamsConfig config; + readSimParams(config, model_.get(), settings_.run.load(), std::string(settings_.admin_hash)); + param_server_->updateConfig(config); +} + +// Helper function to set a bit in a flags int +inline void bit_set_to(int &flags, int bit, bool value) +{ + if (value) { + flags |= (1 << bit); + } else { + flags &= ~(1 << bit); + } +} + +// Helper function to read size values from a space-delimited string +inline void set_from_string(mjtNum *vec, std::string str, uint8_t size) +{ + uint8_t count = 0; + char *pch = strtok(&str[0], " "); + while (pch != nullptr) { + if (count < size) { + vec[count] = std::stod(pch); + count++; + } else { + ROS_WARN_STREAM("Too many values in string '" << str << "' expected " << size << ". Ignoring the rest."); + } + pch = strtok(nullptr, " "); + } + if (count < size - 1) { + ROS_WARN_STREAM("Too few values in string '" << str << "' expected " << size << ". Filling with zeros."); + for (uint8_t i = count; i < size; i++) { + vec[i] = 0; + } + } +} + +void MujocoEnv::dynparamCallback(mujoco_ros::SimParamsConfig &config, uint32_t level) +{ + boost::recursive_mutex::scoped_lock lk(sim_params_mutex_); + if (level == 0xFFFFFFFF) { + // First call on init -> Set params from model + readSimParams(config, model_.get(), settings_.run.load(), std::string(settings_.admin_hash)); + return; + } + settings_.run.store(config.running); + mju::strcpy_arr(settings_.admin_hash, config.admin_hash.c_str()); + + model_->opt.integrator = config.integrator; + model_->opt.cone = config.cone; + model_->opt.jacobian = config.jacobian; + model_->opt.solver = config.solver; + + model_->opt.timestep = config.timestep; + model_->opt.iterations = config.iterations; + model_->opt.tolerance = config.tolerance; + model_->opt.ls_iterations = config.ls_iter; + model_->opt.ls_tolerance = config.ls_tol; + model_->opt.noslip_iterations = config.noslip_iter; + model_->opt.noslip_tolerance = config.noslip_tol; + model_->opt.mpr_iterations = config.mpr_iter; + model_->opt.mpr_tolerance = config.mpr_tol; + model_->opt.sdf_iterations = config.sdf_iter; + model_->opt.sdf_initpoints = config.sdf_init; + + set_from_string(model_->opt.gravity, config.gravity, 3); + set_from_string(model_->opt.wind, config.wind, 3); + set_from_string(model_->opt.magnetic, config.magnetic, 3); + model_->opt.density = config.density; + model_->opt.viscosity = config.viscosity; + model_->opt.impratio = config.impratio; + + bit_set_to(model_->opt.disableflags, 0, config.constraint_disabled); + bit_set_to(model_->opt.disableflags, 1, config.equality_disabled); + bit_set_to(model_->opt.disableflags, 2, config.frictionloss_disabled); + bit_set_to(model_->opt.disableflags, 3, config.limit_disabled); + bit_set_to(model_->opt.disableflags, 4, config.contact_disabled); + bit_set_to(model_->opt.disableflags, 5, config.passive_disabled); + bit_set_to(model_->opt.disableflags, 6, config.gravity_disabled); + bit_set_to(model_->opt.disableflags, 7, config.clampctrl_disabled); + bit_set_to(model_->opt.disableflags, 8, config.warmstart_disabled); + bit_set_to(model_->opt.disableflags, 9, config.filterparent_disabled); + bit_set_to(model_->opt.disableflags, 10, config.actuation_disabled); + bit_set_to(model_->opt.disableflags, 11, config.refsafe_disabled); + bit_set_to(model_->opt.disableflags, 12, config.sensor_disabled); + bit_set_to(model_->opt.disableflags, 13, config.midphase_disabled); + bit_set_to(model_->opt.disableflags, 14, config.eulerdamp_disabled); + + bit_set_to(model_->opt.enableflags, 0, config.override_contacts); + bit_set_to(model_->opt.enableflags, 1, config.energy); + bit_set_to(model_->opt.enableflags, 2, config.fwd_inv); + bit_set_to(model_->opt.enableflags, 3, config.inv_discrete); + bit_set_to(model_->opt.enableflags, 4, config.multiccd); + bit_set_to(model_->opt.enableflags, 5, config.island); + + model_->opt.o_margin = config.margin; + set_from_string(model_->opt.o_solimp, config.solimp, mjNIMP); + set_from_string(model_->opt.o_solref, config.solref, mjNREF); + set_from_string(model_->opt.o_friction, config.friction, 5); } void MujocoEnv::onStepGoal(const mujoco_ros_msgs::StepGoalConstPtr &goal) diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index 2b5b5ed..28fca40 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -217,6 +217,11 @@ void MujocoEnv::eventLoop() std::unique_lock lock(physics_thread_mutex_); now = Clock::now(); + if (settings_.settings_changed.load()) { + settings_.settings_changed.store(0); + updateDynamicParams(); + } + if (settings_.load_request.load() == 1) { ROS_DEBUG("Load request received"); loadWithModelAndData(); @@ -426,6 +431,7 @@ void MujocoEnv::completeEnvSetup() mju_zero(ctrlnoise_, model_->nu); loadPlugins(); + updateDynamicParams(); ROS_DEBUG("Env setup complete"); } @@ -539,6 +545,7 @@ bool MujocoEnv::togglePaused(bool paused, const std::string &admin_hash /*= std: } ROS_DEBUG("Request valid. Handling request"); } + settings_.settings_changed.store(1); settings_.run.store(!paused); if (settings_.run.load()) settings_.env_steps_request.store(0); @@ -779,6 +786,7 @@ void MujocoEnv::prepareReload() MujocoEnv::~MujocoEnv() { ROS_DEBUG("Destructor called"); + delete param_server_; connected_viewers_.clear(); free(this->ctrlnoise_); this->cb_ready_plugins_.clear(); diff --git a/mujoco_ros/src/viewer.cpp b/mujoco_ros/src/viewer.cpp index 8d43779..6f2158b 100644 --- a/mujoco_ros/src/viewer.cpp +++ b/mujoco_ros/src/viewer.cpp @@ -1421,6 +1421,7 @@ void UiEvent(mjuiState *state) // Update flags in env if (!viewer->is_passive_) { viewer->env_->UpdateModelFlags(opt); + viewer->env_->settings_.settings_changed.store(true); } } @@ -1883,6 +1884,7 @@ void Viewer::Sync() this->run = env_->settings_.run.load(); if (pending_.ui_update_run) { env_->settings_.run.store(1 - env_->settings_.run.load()); + env_->settings_.settings_changed.store(true); this->run = env_->settings_.run.load(); pending_.ui_update_run = false; } diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index 9320a7e..3934484 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include "test_util.h" using namespace mujoco_ros; @@ -50,6 +52,7 @@ class MujocoEnvTestWrapper : public MujocoEnv mjModel *getModelPtr() { return model_.get(); } mjData *getDataPtr() { return data_.get(); } MujocoEnvMutex *getMutexPtr() { return &physics_thread_mutex_; } + dynamic_reconfigure::Server *getParamServer() { return param_server_; } int getPendingSteps() { return num_steps_until_exit_; } void setEvalMode(bool eval_mode) { settings_.eval_mode = eval_mode; } diff --git a/mujoco_ros/test/ros_interface_test.cpp b/mujoco_ros/test/ros_interface_test.cpp index 75c7098..c951e8a 100644 --- a/mujoco_ros/test/ros_interface_test.cpp +++ b/mujoco_ros/test/ros_interface_test.cpp @@ -51,6 +51,8 @@ #include "mujoco_env_fixture.h" +#include + #include #include #include @@ -2316,3 +2318,404 @@ TEST_F(PendulumEnvFixture, GetSimInfo_RTSettingChanges) EXPECT_FLOAT_EQ(sim_info_srv.response.state.rt_setting, 1.5f) << "RT setting should change when RT factor is changed!"; } + +TEST_F(BaseEnvFixture, DynParamEnumsMatchMJEnums) +{ + // Integrators + ASSERT_EQ(mujoco_ros::SimParams_Euler, mjINT_EULER); + ASSERT_EQ(mujoco_ros::SimParams_RK4, mjINT_RK4); + ASSERT_EQ(mujoco_ros::SimParams_Implicit, mjINT_IMPLICIT); + ASSERT_EQ(mujoco_ros::SimParams_Implicitfast, mjINT_IMPLICITFAST); + + // Cones + ASSERT_EQ(mujoco_ros::SimParams_Elliptic, mjCONE_ELLIPTIC); + ASSERT_EQ(mujoco_ros::SimParams_Pyramidal, mjCONE_PYRAMIDAL); + + // Jacobians + ASSERT_EQ(mujoco_ros::SimParams_Dense, mjJAC_DENSE); + ASSERT_EQ(mujoco_ros::SimParams_Sparse, mjJAC_SPARSE); + ASSERT_EQ(mujoco_ros::SimParams_Auto, mjJAC_AUTO); + + // Solvers + ASSERT_EQ(mujoco_ros::SimParams_PGS, mjSOL_PGS); + ASSERT_EQ(mujoco_ros::SimParams_CG, mjSOL_CG); + ASSERT_EQ(mujoco_ros::SimParams_Newton, mjSOL_NEWTON); +} + +TEST_F(PendulumEnvFixture, DynamicReconfigureServiceExists) +{ + EXPECT_NE(env_ptr->getParamServer(), nullptr) << "Parameter server should be initialized!"; + EXPECT_TRUE(ros::service::waitForService(env_ptr->getHandleNamespace() + "/set_parameters", 1000)) + << "Service should be available!"; +} + +TEST_F(PendulumEnvFixture, DynamicReconfigureSingleParam) +{ + // Test dynamic reconfigure + dynamic_reconfigure::ReconfigureRequest req; + dynamic_reconfigure::ReconfigureResponse res; + dynamic_reconfigure::DoubleParameter param; + dynamic_reconfigure::Config conf; + + param.name = "timestep"; + param.value = 0.002; + conf.doubles.emplace_back(param); + req.config = conf; + + EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_parameters", req, res)) + << "Service call should not fail!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.timestep, 0.002) << "Timestep should have been updated!"; +} + +TEST_F(PendulumEnvFixture, DynamicReconfigureAllParams) +{ + // Test dynamic reconfigure with all parameters + dynamic_reconfigure::ReconfigureRequest req; + dynamic_reconfigure::ReconfigureResponse res; + dynamic_reconfigure::StrParameter string_param; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::IntParameter int_param; + dynamic_reconfigure::BoolParameter bool_param; + dynamic_reconfigure::Config conf; + + string_param.name = "admin_hash"; + string_param.value = "new_hash"; + conf.strs.emplace_back(string_param); + + // Physics + int_param.name = "integrator"; + int_param.value = 2; + conf.ints.emplace_back(int_param); + + int_param.name = "cone"; + int_param.value = 0; + conf.ints.emplace_back(int_param); + + int_param.name = "jacobian"; + int_param.value = 1; + conf.ints.emplace_back(int_param); + + int_param.name = "solver"; + int_param.value = 1; + conf.ints.emplace_back(int_param); + + // alg_params + double_param.name = "timestep"; + double_param.value = 0.002; + conf.doubles.emplace_back(double_param); + + int_param.name = "iterations"; + int_param.value = 200; + conf.ints.emplace_back(int_param); + + double_param.name = "tolerance"; + double_param.value = 1e-7; + conf.doubles.emplace_back(double_param); + + int_param.name = "ls_iter"; + int_param.value = 60; + conf.ints.emplace_back(int_param); + + double_param.name = "ls_tol"; + double_param.value = 0.02; + conf.doubles.emplace_back(double_param); + + int_param.name = "noslip_iter"; + int_param.value = 10; + conf.ints.emplace_back(int_param); + + double_param.name = "noslip_tol"; + double_param.value = 1e-5; + conf.doubles.emplace_back(double_param); + + int_param.name = "mpr_iter"; + int_param.value = 60; + conf.ints.emplace_back(int_param); + + double_param.name = "mpr_tol"; + double_param.value = 1e-5; + conf.doubles.emplace_back(double_param); + + int_param.name = "sdf_iter"; + int_param.value = 15; + conf.ints.emplace_back(int_param); + + int_param.name = "sdf_init"; + int_param.value = 50; + conf.ints.emplace_back(int_param); + + // phy_params + string_param.name = "gravity"; + string_param.value = "9.81 1 2"; + conf.strs.emplace_back(string_param); + + string_param.name = "wind"; + string_param.value = "1 1 1"; + conf.strs.emplace_back(string_param); + + string_param.name = "magnetic"; + string_param.value = "0.1 0.1 0.1"; + conf.strs.emplace_back(string_param); + + double_param.name = "density"; + double_param.value = 1.2; + conf.doubles.emplace_back(double_param); + + double_param.name = "viscosity"; + double_param.value = 0.00003; + conf.doubles.emplace_back(double_param); + + double_param.name = "impratio"; + double_param.value = 1.5; + conf.doubles.emplace_back(double_param); + + // disable flags + bool_param.name = "constraint_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "equality_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "frictionloss_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "limit_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "contact_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "passive_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "gravity_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "clampctrl_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "warmstart_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "filterparent_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "actuation_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "refsafe_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "sensor_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "midphase_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "eulerdamp_disabled"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + // enbale flags + bool_param.name = "override_contacts"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "energy"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "fwd_inv"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "inv_discrete"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "multiccd"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + bool_param.name = "island"; + bool_param.value = true; + conf.bools.emplace_back(bool_param); + + // contact override + double_param.name = "margin"; + double_param.value = 0.5; + conf.doubles.emplace_back(double_param); + + string_param.name = "solimp"; + string_param.value = "0.8 0.9 0.1"; + conf.strs.emplace_back(string_param); + + string_param.name = "solref"; + string_param.value = "0.03 1.1"; + conf.strs.emplace_back(string_param); + + string_param.name = "friction"; + string_param.value = "0.5 0.5 0.1 0.1"; + conf.strs.emplace_back(string_param); + + req.config = conf; + + EXPECT_NE(env_ptr->getModelPtr()->opt.integrator, 2) << "Integrator is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.cone, 0) << "Cone is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.jacobian, 1) << "Jacobian is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.solver, 1) << "Solver is already set to the target value!"; + + EXPECT_NE(env_ptr->getModelPtr()->opt.timestep, 0.002) << "Timestep is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.iterations, 200) << "Iterations are already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.tolerance, 1e-7) << "Tolerance is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.ls_iterations, 60) << "LS iterations are already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.ls_tolerance, 0.02) << "LS tolerance is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.noslip_iterations, 10) + << "No-slip iterations are already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.noslip_tolerance, 1e-5) + << "No-slip tolerance is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.mpr_iterations, 60) << "MPR iterations are already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.mpr_tolerance, 1e-5) << "MPR tolerance is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.sdf_iterations, 15) << "SDF iterations are already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.sdf_initpoints, 50) << "SDF init is already set to the target value!"; + + EXPECT_NE(env_ptr->getModelPtr()->opt.gravity[0], 9.81) << "Gravity is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.gravity[1], 1) << "Gravity is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.gravity[2], 2) << "Gravity is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.wind[0], 1) << "Wind is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.wind[1], 1) << "Wind is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.wind[2], 1) << "Wind is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.magnetic[0], 0.1) << "Magnetic is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.magnetic[1], 0.1) << "Magnetic is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.magnetic[2], 0.1) << "Magnetic is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.density, 1.2) << "Density is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.viscosity, 0.00003) << "Viscosity is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.impratio, 1.5) << "Imp ratio is already set to the target value!"; + + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CONSTRAINT) << "Constraint is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_EQUALITY) << "Equality is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_FRICTIONLOSS) << "Friction loss is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_LIMIT) << "Limit is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CONTACT) << "Contact is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_PASSIVE) << "Passive is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_GRAVITY) << "Gravity is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CLAMPCTRL) << "Clamp control is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_WARMSTART) << "Warm start is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_FILTERPARENT) << "Filter parent is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_ACTUATION) << "Actuation is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_REFSAFE) << "Ref safe is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_SENSOR) << "Sensor is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_MIDPHASE) << "Midphase is already disabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_EULERDAMP) << "Euler damp is already disabled!"; + + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_OVERRIDE) << "Override contacts are already enabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_ENERGY) << "Energy is already enabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_FWDINV) << "Forward inverse is already enabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_INVDISCRETE) << "Inverse discrete is already enabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_MULTICCD) << "Multi CCD is already enabled!"; + EXPECT_FALSE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_ISLAND) << "Island is already enabled!"; + + EXPECT_NE(env_ptr->getModelPtr()->opt.o_margin, 0.5) << "Margin is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_solimp[0], 0.8) << "Solimp is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_solimp[1], 0.9) << "Solimp is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_solimp[2], 0.1) << "Solimp is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_solref[0], 0.03) << "Solref is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_solref[1], 1.1) << "Solref is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_friction[0], 0.5) << "Friction is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_friction[1], 0.5) << "Friction is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_friction[2], 0.1) << "Friction is already set to the target value!"; + EXPECT_NE(env_ptr->getModelPtr()->opt.o_friction[3], 0.1) << "Friction is already set to the target value!"; + + EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_parameters", req, res)); + + // Wait for physics thread to update + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::lock_guard lock(env_ptr->physics_thread_mutex_); + + EXPECT_STREQ(env_ptr->settings_.admin_hash, std::string("new_hash").c_str()) + << "Eval mode should have been updated!"; + + EXPECT_EQ(env_ptr->getModelPtr()->opt.integrator, 2) << "Integrator should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.cone, 0) << "Cone should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.jacobian, 1) << "Jacobian should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.solver, 1) << "Solver should have been updated!"; + + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.timestep, 0.002) << "Timestep should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.iterations, 200) << "Iterations should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.tolerance, 1e-7) << "Tolerance should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.ls_iterations, 60) << "LS iterations should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.ls_tolerance, 0.02) << "LS tolerance should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.noslip_iterations, 10) << "No-slip iterations should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.noslip_tolerance, 1e-5) + << "No-slip tolerance should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.mpr_iterations, 60) << "MPR iterations should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.mpr_tolerance, 1e-5) << "MPR tolerance should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.sdf_iterations, 15) << "SDF iterations should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.sdf_initpoints, 50) << "SDF init should have been updated!"; + + EXPECT_EQ(env_ptr->getModelPtr()->opt.gravity[0], 9.81) << "Gravity should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.gravity[1], 1) << "Gravity should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.gravity[2], 2) << "Gravity should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.wind[0], 1) << "Wind should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.wind[1], 1) << "Wind should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.wind[2], 1) << "Wind should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.magnetic[0], 0.1) << "Magnetic should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.magnetic[1], 0.1) << "Magnetic should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.magnetic[2], 0.1) << "Magnetic should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.density, 1.2) << "Density should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.viscosity, 0.00003) << "Viscosity should have been updated!"; + EXPECT_DOUBLE_EQ(env_ptr->getModelPtr()->opt.impratio, 1.5) << "Imp ratio should have been updated!"; + + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CONSTRAINT) << "Constraint should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_EQUALITY) << "Equality should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_FRICTIONLOSS) + << "Friction loss should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_LIMIT) << "Limit should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CONTACT) << "Contact should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_PASSIVE) << "Passive should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_GRAVITY) << "Gravity should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_CLAMPCTRL) + << "Clamp control should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_WARMSTART) << "Warm start should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_FILTERPARENT) + << "Filter parent should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_ACTUATION) << "Actuation should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_REFSAFE) << "Ref safe should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_SENSOR) << "Sensor should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_MIDPHASE) << "Midphase should have been disabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.disableflags & mjDSBL_EULERDAMP) << "Euler damp should have been disabled!"; + + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_OVERRIDE) + << "Override contacts should have been enabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_ENERGY) << "Energy should have been enabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_FWDINV) << "Forward inverse should have been enabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_INVDISCRETE) + << "Inverse discrete should have been enabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_MULTICCD) << "Multi CCD should have been enabled!"; + EXPECT_TRUE(env_ptr->getModelPtr()->opt.enableflags & mjENBL_ISLAND) << "Island should have been enabled!"; + + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_margin, 0.5) << "Margin should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_solimp[0], 0.8) << "Solimp should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_solimp[1], 0.9) << "Solimp should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_solimp[2], 0.1) << "Solimp should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_solref[0], 0.03) << "Solref should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_solref[1], 1.1) << "Solref should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_friction[0], 0.5) << "Friction should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_friction[1], 0.5) << "Friction should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_friction[2], 0.1) << "Friction should have been updated!"; + EXPECT_EQ(env_ptr->getModelPtr()->opt.o_friction[3], 0.1) << "Friction should have been updated!"; +}