feat: adds dynamic reconfigure for engine settings

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.

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 ##
 # ############

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"))

mujoco_ros/include/mujoco_ros/mujoco_env.h

@@ -85,6 +85,9 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <dynamic_reconfigure/server.h>
+#include <mujoco_ros/SimParamsConfig.h>
+
 #include <rosgraph_msgs/Clock.h>
 
 #include <mujoco_ros/glfw_adapter.h>
@@ -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<mujoco_ros::SimParamsConfig> *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<ros::ServiceServer> service_servers_;
 	std::unique_ptr<actionlib::SimpleActionServer<mujoco_ros_msgs::StepAction>> action_step_;
 

mujoco_ros/package.xml

@@ -14,6 +14,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roscpp</depend>
+  <depend>dynamic_reconfigure</depend>
   <depend>actionlib</depend>
   <depend>pluginlib</depend>
   <depend>mujoco_ros_msgs</depend>