Introduce new logging API, fixes #858

PiperOrigin-RevId: 930744288
Change-Id: I6ec1203b55c031390f3eef23192e2337508ce886

src/engine/engine_crossplatform.h

@@ -33,6 +33,12 @@
   #include <strings.h>
 #endif
 
+// Environment variable handling.
+#ifdef _WIN32
+  #define setenv(name, value, overwrite) _putenv_s(name, value)
+  #define unsetenv(name) _putenv_s(name, "")
+#endif
+
 // Switch-case fallthrough annotation.
 #if defined(__cplusplus)
   #define mjFALLTHROUGH [[fallthrough]]

src/engine/engine_io.c

@@ -1069,6 +1069,9 @@ void mj_makeRawData(mjData** dest, const mjModel* m) {
     mjERROR("could not allocate mjData");
   }
 
+  // prevent spurious timing print from mj_resetData before _resetData zeroes the struct
+  d->timer[mjTIMER_STEP].number = 0;
+
   // compute buffer size
   d->nbuffer = 0;
   d->buffer = d->arena = NULL;
@@ -1563,12 +1566,85 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
 }
 
 
+// emit step timing diagnostics
+static void mj_logTimingDiagnostics(const mjData* d) {
+  int nstep = d->timer[mjTIMER_STEP].number;
+  if (nstep <= 0) {
+    return;
+  }
+
+  mjtNum tstep = d->timer[mjTIMER_STEP].duration / nstep;
+  if (tstep <= 0) {
+    return;
+  }
+
+  char buf[2048];
+  int pos = 0;
+  mjtNum components = 0;
+
+  for (int i = mjTIMER_POSITION; i <= mjTIMER_ADVANCE; i++) {
+    if (d->timer[i].number > 0) {
+      mjtNum istep = d->timer[i].duration / d->timer[i].number;
+      components += istep;
+      pos += snprintf(buf + pos, sizeof(buf) - pos,
+                      "%s  %-15s %8.1f  (%5.1f%%)",
+                      pos > 0 ? "\n" : "",
+                      mjTIMERSTRING[i], istep * 1000, 100 * istep / tstep);
+
+      // position sub-breakdown
+      if (i == mjTIMER_POSITION) {
+        for (int p = mjTIMER_POS_KINEMATICS; p <= mjTIMER_POS_PROJECT; p++) {
+          if (d->timer[p].number > 0) {
+            mjtNum pstep = d->timer[p].duration / d->timer[p].number;
+            pos += snprintf(buf + pos, sizeof(buf) - pos,
+                            "\n    %-13s %8.1f  (%5.1f%%)",
+                            mjTIMERSTRING[p] + 4, pstep * 1000, 100 * pstep / tstep);
+
+            // collision sub-breakdown
+            if (p == mjTIMER_POS_COLLISION) {
+              for (int c = mjTIMER_COL_BROAD; c <= mjTIMER_COL_NARROW; c++) {
+                if (d->timer[c].number > 0) {
+                  mjtNum cstep = d->timer[c].duration / d->timer[c].number;
+                  pos += snprintf(buf + pos, sizeof(buf) - pos,
+                                  "\n      %-11s %8.1f  (%5.1f%%)",
+                                  mjTIMERSTRING[c] + 4, cstep * 1000, 100 * cstep / tstep);
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  mjtNum other = tstep - components;
+  pos += snprintf(buf + pos, sizeof(buf) - pos,
+                  "%s  %-15s %8.1f  (%5.1f%%)",
+                  pos > 0 ? "\n" : "",
+                  "other", other * 1000, 100 * other / tstep);
+
+  pos += snprintf(buf + pos, sizeof(buf) - pos,
+                  "%s  %-15s %8.1f",
+                  pos > 0 ? "\n" : "",
+                  "total", tstep * 1000);
+
+  mjLogMessage msg = {.level = mjLOG_INFO, .topic = mjTOPIC_TIME_STP, .body = buf};
+  snprintf(msg.subject, sizeof(msg.subject),
+           "average time per step (%d steps, units: \u00B5s)", nstep);
+  mju_message(&msg);
+}
+
+
 // clear data, set data->qpos = model->qpos0
 void mj_resetData(const mjModel* m, mjData* d) {
+  // emit step timing diagnostics before timers are cleared
+  mj_logTimingDiagnostics(d);
+
   _resetData(m, d, 0);
 }
 
 
+
 // clear data, set data->qpos = model->qpos0, fill with debug_value
 void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value) {
   _resetData(m, d, debug_value);

src/engine/engine_sleep.c

@@ -24,8 +24,6 @@
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
 
-// uncomment to print sleep/wake events
-// #define MJ_DEBUG_SLEEP
 
 //-------------------------------- update ----------------------------------------------------------
 
@@ -187,8 +185,14 @@ int mj_sleepCycle(const int* tree_asleep, int ntree, int i) {
 
 //-------------------------------- wake ------------------------------------------------------------
 
+// helper for pluralizing in log messages
+static inline const char* plural(int n) {
+  return n > 1 ? "s" : "";
+}
+
+
 // wake tree i and its associated cycle, return number of woke trees
-int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval) {
+int mj_wakeIsland(int* tree_asleep, int ntree, int i, int wakeval, const char* reason, mjtNum time) {
   int nwoke = 0;
 
   // i is invalid; SHOULD NOT OCCUR
@@ -207,6 +211,7 @@ int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval) {
   // tree i asleep: wake up tree and its island cycle
   else {
     int current = i;
+    int woke_trees[1024];  // buffer for woke tree indices
     do {
       // get the index of the next tree in the cycle
       int next = tree_asleep[current];
@@ -217,8 +222,9 @@ int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval) {
         return 0;
       }
 
-      // wake the current tree, increment count, advance to next
+      // wake the current tree, record index, increment and advance to next
       tree_asleep[current] = wakeval;
+      if (nwoke < 1024) woke_trees[nwoke] = current;
       nwoke++;
       current = next;
     } while (current != i && nwoke < ntree);
@@ -228,6 +234,21 @@ int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval) {
       mjERROR("tree %d is not in a cycle", i);
       return 0;
     }
+
+#ifndef MJ_DISABLE_DEBUG_TRACING
+    if (reason && mju_isTopicEnabled(mjTOPIC_SLEEP)) {
+      int nprint = mjMIN(nwoke, 1024);
+      char buf[1024];
+      int pos = snprintf(buf, sizeof(buf), "t=%6.3g, woke due to %s tree%s ", time, reason, plural(nprint));
+      for (int j = 0; j < nprint; j++) {
+        pos += snprintf(buf + pos, sizeof(buf) - pos, "%d%s", woke_trees[j],
+                        (j == nprint - 1) ? "" : " ");
+      }
+      mjLogMessage msg = {.level = mjLOG_DEBUG, .topic = mjTOPIC_SLEEP, .func = __func__};
+      mju_strncpy(msg.subject, buf, sizeof(msg.subject));
+      mju_message(&msg);
+    }
+#endif
   }
 
   return nwoke;
@@ -260,14 +281,7 @@ int mj_wake(const mjModel* m, mjData* d) {
 
     // if qpos mismatch or cannot sleep: wake up
     if (d->tree_awake[i] || !treeCanSleep(m, d, i, 0)) {
-      int woke = mj_wakeTree(d->tree_asleep, ntree, i, kAwake);
-      if (woke) {
-        nwoke += woke;
-
-        #ifdef MJ_DEBUG_SLEEP
-        printf("woke tree %d due to perturbation at t=%g\n", i, d->time);
-        #endif
-      }
+      nwoke += mj_wakeIsland(d->tree_asleep, ntree, i, kAwake, "perturbation", d->time);
     }
   }
 
@@ -340,11 +354,7 @@ int mj_wakeCollision(const mjModel* m, mjData* d) {
     // wake sleeping tree
     int sleeping_tree = awake1 ? tree2 : tree1;
     int wakeval = awake1 ? d->tree_asleep[tree1] : d->tree_asleep[tree2];
-    nwoke += mj_wakeTree(d->tree_asleep, ntree, sleeping_tree, wakeval);
-
-    #ifdef MJ_DEBUG_SLEEP
-    printf("woke tree %d due to contact at t=%g\n", sleeping_tree, d->time);
-    #endif
+    nwoke += mj_wakeIsland(d->tree_asleep, ntree, sleeping_tree, wakeval, "contact", d->time);
   }
 
   return nwoke;
@@ -372,11 +382,8 @@ int mj_wakeTendon(const mjModel* m, mjData* d) {
     if (awake1 != awake2) {
       int sleeping_tree = awake1 ? tree2 : tree1;
       int wakeval = awake1 ? d->tree_asleep[tree1] : d->tree_asleep[tree2];
-      nwoke += mj_wakeTree(d->tree_asleep, m->ntree, sleeping_tree, wakeval);
-
-      #ifdef MJ_DEBUG_SLEEP
-      printf("woke tree %d due to tendon constraint at t=%g\n", sleeping_tree, d->time);
-      #endif
+      nwoke += mj_wakeIsland(d->tree_asleep, m->ntree, sleeping_tree, wakeval,
+                             "tendon constraint", d->time);
     }
   }
 
@@ -455,10 +462,8 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
         for (int j = 0; j < num; j++) {
           int treeid = m->body_treeid[bodyid[adr+j]];
           if (treeid >= 0 && !d->tree_awake[treeid]) {
-            nwoke += mj_wakeTree(d->tree_asleep, m->ntree, treeid, wakeval);
-            #ifdef MJ_DEBUG_SLEEP
-            printf("woke tree %d due to flex equality %d at t=%g\n", treeid, i, d->time);
-            #endif
+            nwoke += mj_wakeIsland(d->tree_asleep, m->ntree, treeid, wakeval,
+                                   "flex equality", d->time);
             break;
           }
         }
@@ -494,12 +499,8 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
       int cycle1 = mj_sleepCycle(d->tree_asleep, m->ntree, tree1);
       int cycle2 = mj_sleepCycle(d->tree_asleep, m->ntree, tree2);
       if (cycle1 != cycle2) {
-        int nwoke1 = mj_wakeTree(d->tree_asleep, m->ntree, tree1, kAwake);
-        int nwoke2 = mj_wakeTree(d->tree_asleep, m->ntree, tree2, kAwake);
-
-        #ifdef MJ_DEBUG_SLEEP
-        printf("woke trees %d, %d due to equality %d at t=%g\n", tree1, tree2, i, d->time);
-        #endif
+        int nwoke1 = mj_wakeIsland(d->tree_asleep, m->ntree, tree1, kAwake, "equality", d->time);
+        int nwoke2 = mj_wakeIsland(d->tree_asleep, m->ntree, tree2, kAwake, "equality", d->time);
 
         nwoke += nwoke1 + nwoke2;
       }
@@ -508,11 +509,7 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
 
     // one is asleep and one is awake, wake the sleeping tree
     int sleeping_tree = s1 == mjS_ASLEEP ? tree1 : tree2;
-    nwoke += mj_wakeTree(d->tree_asleep, m->ntree, sleeping_tree, kAwake);
-
-    #ifdef MJ_DEBUG_SLEEP
-    printf("woke tree %d due to equality %d at t=%g\n", sleeping_tree, i, d->time);
-    #endif
+    nwoke += mj_wakeIsland(d->tree_asleep, m->ntree, sleeping_tree, kAwake, "equality", d->time);
   }
 
   return nwoke;
@@ -522,7 +519,7 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
 //-------------------------------- sleep -----------------------------------------------------------
 
 // put n trees to sleep (create cycle), set their velocity and acceleration to zero
-static inline void sleepTrees(const mjModel* m, mjData* d, const int* tree, int n) {
+static inline void mj_sleepTrees(const mjModel* m, mjData* d, const int* tree, int n) {
   for (int i=0; i < n; i++) {
     // create cycle
     int current = tree[i];
@@ -545,17 +542,18 @@ static inline void sleepTrees(const mjModel* m, mjData* d, const int* tree, int
     mju_zero(d->qacc+adr, num);
   }
 
-  #ifdef MJ_DEBUG_SLEEP
-  if (n == 1) {
-    printf("tree %d put to sleep at t=%g\n", tree[0], d->time);
-  } else if (n > 1) {
-    printf("trees ");
+#ifndef MJ_DISABLE_DEBUG_TRACING
+  if (mju_isTopicEnabled(mjTOPIC_SLEEP)) {
+    char buf[1024];
+    int pos = snprintf(buf, sizeof(buf), "t=%6.2g, slept tree%s ", d->time, plural(n));
     for (int i = 0; i < n; i++) {
-      printf("%d%s", tree[i], (i == n - 1) ? "" : ", ");
+      pos += snprintf(buf + pos, sizeof(buf) - pos, "%d%s", tree[i], (i == n - 1) ? "" : " ");
     }
-    printf(" put to sleep at t=%g\n", d->time);
+    mjLogMessage msg = {.level = mjLOG_DEBUG, .topic = mjTOPIC_SLEEP, .func = __func__};
+    mju_strncpy(msg.subject, buf, sizeof(msg.subject));
+    mju_message(&msg);
   }
-  #endif
+#endif
 }
 
 
@@ -611,7 +609,7 @@ int mj_sleep(const mjModel* m, mjData* d) {
     if (can_sleep) {
       const int* tree = d->map_itree2tree + start;
       int n = d->island_ntree[i];
-      sleepTrees(m, d, tree, n);
+      mj_sleepTrees(m, d, tree, n);
       nslept += n;
     }
   }
@@ -621,7 +619,7 @@ int mj_sleep(const mjModel* m, mjData* d) {
   for (int j=start; j < ntree; j++) {
     int i = nisland ? d->map_itree2tree[j] : j;
     if (d->tree_asleep[i] == -1) {
-      sleepTrees(m, d, &i, 1);
+      mj_sleepTrees(m, d, &i, 1);
       nslept++;
     }
   }
@@ -853,8 +851,3 @@ mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int
     return mjS_AWAKE;
   }
 }
-
-
-#ifdef MJ_DEBUG_SLEEP
-  #undef MJ_DEBUG_SLEEP
-#endif

src/engine/engine_sleep.h

@@ -18,6 +18,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjtype.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -32,11 +33,9 @@ MJAPI void mj_updateSleep(const mjModel* m, mjData* d);
 // return the first tree in the sleep cycle that starts at i, -1 if error
 int mj_sleepCycle(const int* tree_asleep, int ntree, int i);
 
-// return the first tree in the sleep cycle that starts at i, -1 if error
-int mj_sleepCycle(const int* tree_asleep, int ntree, int i);
-
 // wake tree i and its related island cycle, return number of woke trees
-MJAPI int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval);
+MJAPI int mj_wakeIsland(int* tree_asleep, int ntree, int i, int wakeval,
+                        const char* reason, mjtNum time);
 
 // wake trees with nonzero velocity or external forces, return number of woke trees
 int mj_wake(const mjModel* m, mjData* d);

src/engine/engine_support.c

@@ -102,6 +102,14 @@ const char* mjTIMERSTRING[mjNTIMER]= {
 };
 
 
+// names of log topics (index i corresponds to topic i+1)
+const char* mjTOPICSTRING[mjNTOPIC] = {
+  "Step timing",
+  "Compile timing",
+  "Sleep/wake"
+};
+
+
 // size of contact data fields
 const int mjCONDATA_SIZE[mjNCONDATA] = {
   1,  // mjCONDATA_FOUND

src/engine/engine_support.h

@@ -28,6 +28,7 @@ extern "C" {
 MJAPI extern const char* mjDISABLESTRING[mjNDISABLE];
 MJAPI extern const char* mjENABLESTRING[mjNENABLE];
 MJAPI extern const char* mjTIMERSTRING[mjNTIMER];
+MJAPI extern const char* mjTOPICSTRING[mjNTOPIC];
 
 // arrays
 MJAPI extern const int mjCONDATA_SIZE[mjNCONDATA];  // TODO(tassa): expose in public header?

src/engine/engine_util_errmem.c

@@ -18,17 +18,29 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdarg.h>
+#include <string.h>
 #include <time.h>
 
 #if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
 #include <unistd.h>
 #endif
 
-#include "engine/engine_array_safety.h"
+#include "engine/engine_crossplatform.h"  // IWYU pragma: keep
 #include "engine/engine_macro.h"
 
 //------------------------- cross-platform aligned malloc/free -------------------------------------
 
+// forward declaration for active log handler dispatch
+static void mju_defaultLogHandler(const mjLogMessage* msg);
+
+
+//------------------------------ malloc and free ---------------------------------------------------
+
+// user memory handlers
+void* (*mju_user_malloc) (size_t) = 0;
+void (*mju_user_free) (void*) = 0;
+
+// cross-platform aligned malloc
 static inline void* mju_alignedMalloc(size_t size, size_t align) {
 #ifdef _WIN32
   return _aligned_malloc(size, align);
@@ -37,6 +49,7 @@ static inline void* mju_alignedMalloc(size_t size, size_t align) {
 #endif
 }
 
+// cross-platform aligned free
 static inline void mju_alignedFree(void* ptr) {
 #ifdef _WIN32
   _aligned_free(ptr);
@@ -45,98 +58,262 @@ static inline void mju_alignedFree(void* ptr) {
 #endif
 }
 
+// allocate memory; byte-align on 64; pad size to multiple of 64
+void* mju_malloc(size_t size) {
+  void* ptr = 0;
 
-//------------------------- default user handlers --------------------------------------------------
+  if (mju_user_malloc) {
+    ptr = mju_user_malloc(size);
+  } else {
+    if (size > 0 && (size % 64)) {
+      size += 64 - (size % 64);
+    }
+    if (size > 0) {
+      ptr = mju_alignedMalloc(size, 64);
+    }
+  }
 
-// define and clear handlers
+  if (!ptr && size > 0) {
+    mju_error("Could not allocate memory");
+  }
+  return ptr;
+}
+
+// free memory
+void mju_free(void* ptr) {
+  if (!ptr) return;
+  if (mju_user_free) {
+    mju_user_free(ptr);
+  } else {
+    mju_alignedFree(ptr);
+  }
+}
+
+
+//------------------------------ logging configuration and handlers --------------------------------
+
+// global log handler
+static mjfLogHandler global_log_handler = mju_defaultLogHandler;
+
+// legacy error/warning handlers (deprecated)
 void (*mju_user_error) (const char*) = 0;
 void (*mju_user_warning) (const char*) = 0;
-void* (*mju_user_malloc) (size_t) = 0;
-void (*mju_user_free) (void*) = 0;
 
+// default handler configuration
+static mjLogConfig log_config = {.logto_console = true,
+                                 .logto_file = true,
+                                 .logfile = "MUJOCO_LOG.TXT",
+                                 .topics = 0};
+static mjtBool env_checked = 0;
+
+// parse MUJOCO_LOG_TOPICS env var to seed initial topic bitmask
+// example: MUJOCO_LOG_TOPICS="time_stp,sleep"
+static void mju_initLogTopicsFromEnv(void) {
+  const char* env = getenv("MUJOCO_LOG_TOPICS");
+  if (!env) return;
+
+  // mjTOPIC_X enum names with the mjTOPIC_ prefix stripped and lowercased; keep in sync with mjtLogTopic
+  static const char* topic_names[mjNTOPIC] = {"time_stp", "time_cmp", "sleep"};
+  char buf[256];
+  strncpy(buf, env, sizeof(buf) - 1);
+  buf[sizeof(buf) - 1] = '\0';
+
+  char* token = buf;
+  while (*token) {
+    while (*token == ' ' || *token == ',') token++;
+    if (!*token) break;
+
+    char* end = token;
+    while (*end && *end != ',') end++;
+    bool trailing_comma = (*end == ',');
+
+    char* tEnd = end - 1;
+    while (tEnd > token && *tEnd == ' ') tEnd--;
+    *(tEnd + 1) = '\0';
+
+    for (int i = 0; i < mjNTOPIC; i++) {
+      if (strcasecmp(token, topic_names[i]) == 0) {
+        log_config.topics |= (1 << i);
+        break;
+      }
+    }
+    token = trailing_comma ? end + 1 : end;
+  }
+}
+
+// private pointer getter encapsulates lazy init with zero copy overhead
+static const mjLogConfig* mju_getLogConfigPtr(void) {
+  if (!env_checked) {
+    mju_initLogTopicsFromEnv();
+    env_checked = 1;
+  }
+  return &log_config;
+}
+
+// check whether an info topic is enabled
+mjtBool mju_isTopicEnabled(int topic) {
+  if (!topic) return 1;
+  const mjLogConfig* cfg = mju_getLogConfigPtr();
+  return ((cfg->topics & (1 << (topic - 1))) != 0);
+}
+
+// set the active log handler; return the previous handler
+mjfLogHandler mju_setLogHandler(mjfLogHandler handler) {
+  mjfLogHandler prev = global_log_handler;
+  global_log_handler = handler ? handler : mju_defaultLogHandler;
+  return prev;
+}
+
+// get default handler configuration
+mjLogConfig mju_getLogConfig(void) {
+  return *mju_getLogConfigPtr();
+}
+
+// set default handler configuration
+void mju_setLogConfig(mjLogConfig config) {
+  env_checked = 1;
+  log_config = config;
+}
 
 // restore default processing
 void mju_clearHandlers(void) {
+  global_log_handler = mju_defaultLogHandler;
+  log_config = (mjLogConfig){.logto_console = true,
+                             .logto_file = true,
+                             .logfile = "MUJOCO_LOG.TXT",
+                             .topics = 0};
+  env_checked = 1;
+  mju_initLogTopicsFromEnv();
+
   mju_user_error = 0;
   mju_user_warning = 0;
   mju_user_malloc = 0;
   mju_user_free = 0;
 }
 
-//------------------------- internal-only handlers -------------------------------------------------
-
-typedef void (*callback_fn)(const char*);
-
-static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_error_fn = NULL;
-static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_warning_fn = NULL;
-
-callback_fn _mjPRIVATE__get_tls_error_fn(void) {
-  return _mjPRIVATE_tls_error_fn;
-}
-
-void _mjPRIVATE__set_tls_error_fn(callback_fn h) {
-  _mjPRIVATE_tls_error_fn = h;
-}
-
-callback_fn _mjPRIVATE__get_tls_warning_fn(void) {
-  return _mjPRIVATE_tls_warning_fn;
-}
-
-void _mjPRIVATE__set_tls_warning_fn(callback_fn h) {
-  _mjPRIVATE_tls_warning_fn = h;
-}
-
-//------------------------------ error hadling -----------------------------------------------------
-
-// write datetime, type: message to MUJOCO_LOG.TXT
-void mju_writeLog(const char* type, const char* msg) {
+// fill buffer with formatted local time string (thread-safe)
+static void mju_localTimeStr(char* buf, int buf_sz) {
   time_t rawtime;
   struct tm timeinfo;
-  FILE* fp = fopen("MUJOCO_LOG.TXT", "a+t");
-  if (fp) {
-    // get time
-    time(&rawtime);
+  time(&rawtime);
 
 #if defined(_POSIX_C_SOURCE) || defined(__APPLE__) || defined(__STDC_VERSION_TIME_H__) || defined(__EMSCRIPTEN__)
-    localtime_r(&rawtime, &timeinfo);
+  localtime_r(&rawtime, &timeinfo);
 #elif defined(_WIN32)
-    localtime_s(&timeinfo, &rawtime);
+  localtime_s(&timeinfo, &rawtime);
 #elif __STDC_LIB_EXT1__
-    localtime_s(&rawtime, &timeinfo);
+  localtime_s(&rawtime, &timeinfo);
 #else
-    #error "Thread-safe version of `localtime` is not present in the standard C library"
+  #error "Thread-safe version of `localtime` is not present in the standard C library"
 #endif
 
-    // write to log file
-    fprintf(fp, "%s%s: %s\n\n", asctime(&timeinfo), type, msg);
-    fclose(fp);
-  }
+  strftime(buf, buf_sz, "%c", &timeinfo);
 }
 
+// write formatted message to stream
+static void mju_fprint_message(FILE* stream, const char* timestr,
+                               const mjLogMessage* msg) {
+  const char* type = msg->level == mjLOG_ERROR   ? "ERROR" :
+                     msg->level == mjLOG_WARNING ? "WARNING" :
+                     msg->level == mjLOG_INFO    ? "INFO" : "DEBUG";
+  fprintf(stream, "%s", type);
+  if (msg->func) fprintf(stream, " %s", msg->func);
+  if (msg->file && msg->line) fprintf(stream, " (%s:%d)", BaseName(msg->file), msg->line);
+  if (timestr[0]) fprintf(stream, " %s", timestr);
+  fprintf(stream, ": %s\n", msg->subject);
+  if (msg->body) fprintf(stream, "%s\n", msg->body);
 
-void mju_error_raw(const char* msg) {
-  if (_mjPRIVATE_tls_error_fn) {
-    _mjPRIVATE_tls_error_fn(msg);
-  } else if (mju_user_error) {
-    mju_user_error(msg);
-  } else {
-    // write to log and console
-    mju_writeLog("ERROR", msg);
-    printf("ERROR: %s\n\n", msg);
+  // add blank line after message except for DEBUG, for compactness
+  if (msg->level != mjLOG_DEBUG) fprintf(stream, "\n");
+}
 
-    // exit
+// format legacy adapter string: "func: subject" or just "subject"
+static const char* mju_legacy_text(const mjLogMessage* msg, char* buf, int bufsz) {
+  if (msg->func) {
+    snprintf(buf, bufsz, "%s: %s", msg->func, msg->subject);
+    return buf;
+  }
+  return msg->subject;
+}
+
+// default log handler: topic filtering, console/file output, legacy compat, exit on error
+static void mju_defaultLogHandler(const mjLogMessage* msg) {
+  const mjLogConfig* cfg = mju_getLogConfigPtr();
+
+  if ((msg->level == mjLOG_INFO || msg->level == mjLOG_DEBUG) && !mju_isTopicEnabled(msg->topic)) {
+    return;
+  }
+
+  if (msg->level == mjLOG_ERROR && mju_user_error) {
+    char buf[1024];
+    mju_user_error(mju_legacy_text(msg, buf, sizeof(buf)));
+    return;
+  }
+
+  if (msg->level == mjLOG_WARNING && mju_user_warning) {
+    char buf[1024];
+    mju_user_warning(mju_legacy_text(msg, buf, sizeof(buf)));
+    return;
+  }
+
+  char timestr[64] = "";
+  if (msg->timestamp || (cfg->logto_file && cfg->logfile[0])) {
+    mju_localTimeStr(timestr, sizeof(timestr));
+  }
+
+  if (cfg->logto_file && cfg->logfile[0]) {
+    FILE* fp = fopen(cfg->logfile, "a+t");
+    if (fp) {
+      mju_fprint_message(fp, timestr, msg);
+      fclose(fp);
+    }
+  }
+
+  if (cfg->logto_console) {
+    FILE* stream = (msg->level >= mjLOG_WARNING) ? stderr : stdout;
+    mju_fprint_message(stream, msg->timestamp ? timestr : "", msg);
+  }
+
+  if (msg->level == mjLOG_ERROR) {
     exit(EXIT_FAILURE);
   }
 }
 
 
-void mju_error_v(const char* msg, va_list args) {
-  // Format msg into errmsg
-  char errmsg[1024];
-  vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args);
-  mju_error_raw(errmsg);
+//------------------------------ public message logging --------------------------------------------
+
+// thread-local log handler override
+static mjTHREADLOCAL mjfLogHandler _mjPRIVATE_tls_log_handler = NULL;
+
+// recursion guard for log handler
+static mjTHREADLOCAL bool in_log = false;
+
+// dispatch to active handler (TLS > global)
+static inline mjfLogHandler mju_activeHandler(void) {
+  return _mjPRIVATE_tls_log_handler ? _mjPRIVATE_tls_log_handler : global_log_handler;
 }
 
+// dispatch a structured log message to the active handler
+void mju_message(const mjLogMessage* msg) {
+  // recursion guard: silently drop messages dispatched from within a handler
+  if (in_log) return;
+
+  // error handlers are expected to not return (longjmp or exit); we cannot set in_log
+  // around the call because longjmp would leave it permanently true on this thread
+  if (msg->level != mjLOG_ERROR) {
+    in_log = true;
+    mju_activeHandler()(msg);
+    in_log = false;
+  } else {
+    mju_activeHandler()(msg);
+  }
+}
+
+void mju_error_v(const char* msg, va_list args) {
+  mjLogMessage m = {.level = mjLOG_ERROR};
+  vsnprintf(m.subject, sizeof(m.subject), msg, args);
+  mju_message(&m);
+}
 
 // write message to logfile and console, pause and exit
 void mju_error(const char* msg, ...) {
@@ -146,73 +323,50 @@ void mju_error(const char* msg, ...) {
   va_end(args);
 }
 
-
 // write message to logfile and console
 void mju_warning(const char* msg, ...) {
-  char wrnmsg[1024];
-
-  // Format msg into wrnmsg
+  mjLogMessage m = {.level = mjLOG_WARNING};
   va_list args;
   va_start(args, msg);
-  vsnprintf(wrnmsg, mjSIZEOFARRAY(wrnmsg), msg, args);
+  vsnprintf(m.subject, sizeof(m.subject), msg, args);
   va_end(args);
+  mju_message(&m);
+}
 
-  if (_mjPRIVATE_tls_warning_fn) {
-    _mjPRIVATE_tls_warning_fn(wrnmsg);
-  } else if (mju_user_warning) {
-    mju_user_warning(wrnmsg);
-  } else {
-    // write to log file and console
-    mju_writeLog("WARNING", wrnmsg);
-    printf("WARNING: %s\n\n", wrnmsg);
+// log an info message
+void mju_info(int topic, const char* msg, ...) {
+  mjLogMessage m = {.level = mjLOG_INFO, .topic = topic};
+  va_list args;
+  va_start(args, msg);
+  vsnprintf(m.subject, sizeof(m.subject), msg, args);
+  va_end(args);
+  mju_message(&m);
+}
+
+// (deprecated) write datetime, type: message to MUJOCO_LOG.TXT
+void mju_writeLog(const char* type, const char* msg) {
+  char timestr[64];
+  mju_localTimeStr(timestr, sizeof(timestr));
+
+  FILE* fp = fopen("MUJOCO_LOG.TXT", "a+t");
+  if (fp) {
+    fprintf(fp, "%s\n%s: %s\n\n", timestr, type, msg);
+    fclose(fp);
   }
 }
 
 
-//------------------------------ malloc and free ---------------------------------------------------
+//------------------------------ internal helpers --------------------------------------------------
 
-// allocate memory; byte-align on 64; pad size to multiple of 64
-void* mju_malloc(size_t size) {
-  void* ptr = 0;
-
-  // user allocator
-  if (mju_user_malloc) {
-    ptr = mju_user_malloc(size);
-  }
-
-  // default allocator
-  else {
-    // pad size to multiple of 64
-    if (size > 0 && (size % 64)) {
-      size += 64 - (size % 64);
-    }
-
-    // allocate
-    if (size > 0) {
-      ptr = mju_alignedMalloc(size, 64);
-    }
-  }
-
-  // error if null pointer
-  if (!ptr && size > 0) {
-    mju_error("Could not allocate memory");
-  }
-
-  return ptr;
+// set thread-local log handler; return previous
+mjfLogHandler _mjPRIVATE_setTlsLogHandler(mjfLogHandler handler) {
+  mjfLogHandler prev = _mjPRIVATE_tls_log_handler;
+  _mjPRIVATE_tls_log_handler = handler;
+  return prev;
 }
 
 
-// free memory
-void mju_free(void* ptr) {
-  // return if null
-  if (!ptr) {
-    return;
-  }
-
-  // free with user or built-in function
-  if (mju_user_free) {
-    mju_user_free(ptr);
-  } else {
-    mju_alignedFree(ptr);
-  }
+// get the currently active global log handler (read-only, no modification)
+mjfLogHandler _mjPRIVATE_getGlobalLogHandler(void) {
+  return global_log_handler;
 }

src/engine/engine_util_errmem.h

@@ -22,6 +22,7 @@
 
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmacro.h>
+#include <mujoco/mjtype.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -36,48 +37,6 @@ extern "C" {
 #endif  // mjPRINTFLIKE
 
 
-//------------------------------ user handlers -----------------------------------------------------
-
-MJAPI extern void (*mju_user_error)(const char*);
-MJAPI extern void (*mju_user_warning)(const char*);
-MJAPI extern void* (*mju_user_malloc)(size_t);
-MJAPI extern void (*mju_user_free)(void*);
-
-// clear user handlers; restore default processing
-MJAPI void mju_clearHandlers(void);
-
-// gets/sets thread-local error/warning handlers for internal use
-MJAPI void (*_mjPRIVATE__get_tls_error_fn(void))(const char*);
-MJAPI void _mjPRIVATE__set_tls_error_fn(void (*h)(const char*));
-MJAPI void (*_mjPRIVATE__get_tls_warning_fn(void))(const char*);
-MJAPI void _mjPRIVATE__set_tls_warning_fn(void (*h)(const char*));
-
-//------------------------------ errors and warnings -----------------------------------------------
-
-// errors
-MJAPI void mju_error_raw(const char* msg);
-MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2);
-MJAPI void mju_error_v(const char* msg, va_list args);
-
-// warnings
-MJAPI void mju_warning(const char* msg, ...) mjPRINTFLIKE(1, 2);
-
-// write [datetime, type: message] to MUJOCO_LOG.TXT
-MJAPI void mju_writeLog(const char* type, const char* msg);
-
-//------------------------------ internal error macros --------------------------------------------
-
-// internal macro to prepend the calling function name to the error message
-#pragma warning(disable : 4996)  // needed to use strncpy with Visual Studio
-#define mjERROR(...)                                                          \
-{                                                                             \
-  char _errbuf[1024];                                                         \
-  size_t _funclen = strlen(__func__);                                         \
-  strncpy(_errbuf, __func__, sizeof(_errbuf));                                \
-  snprintf(_errbuf + _funclen, sizeof(_errbuf) - _funclen, ": " __VA_ARGS__); \
-  mju_error_raw(_errbuf);                                                     \
-}
-
 //------------------------------ malloc and free ---------------------------------------------------
 
 // allocate memory; byte-align on 8; pad size to multiple of 8
@@ -86,6 +45,94 @@ MJAPI void* mju_malloc(size_t size);
 // free memory with free() by default
 MJAPI void mju_free(void* ptr);
 
+// user memory handlers
+MJAPI extern void* (*mju_user_malloc)(size_t);
+MJAPI extern void (*mju_user_free)(void*);
+
+
+//------------------------------ logging configuration and handlers --------------------------------
+
+// set the active log handler, return the previous handler
+// if handler is NULL, restore the default handler
+MJAPI mjfLogHandler mju_setLogHandler(mjfLogHandler handler);
+
+// set/get default handler configuration
+MJAPI mjLogConfig mju_getLogConfig(void);
+MJAPI void mju_setLogConfig(mjLogConfig config);
+
+// clear user handlers; restore default processing
+MJAPI void mju_clearHandlers(void);
+
+// legacy error/warning handlers (deprecated: prefer mju_setLogHandler)
+MJAPI extern void (*mju_user_error)(const char*);
+MJAPI extern void (*mju_user_warning)(const char*);
+
+
+//------------------------------ public message logging --------------------------------------------
+
+// log a fatal error message, write to logfile and console, pause and exit
+MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2);
+MJAPI void mju_error_v(const char* msg, va_list args);
+
+// log a warning message, write to logfile and console
+MJAPI void mju_warning(const char* msg, ...) mjPRINTFLIKE(1, 2);
+
+// log an info message with optional topic filtering
+MJAPI void mju_info(int topic, const char* msg, ...) mjPRINTFLIKE(2, 3);
+
+// dispatch a structured log message to the active handler
+MJAPI void mju_message(const mjLogMessage* msg);
+
+// (deprecated) write [datetime, type: message] to MUJOCO_LOG.TXT
+MJAPI void mju_writeLog(const char* type, const char* msg);
+
+
+//------------------------------ internal helpers and macros ---------------------------------------
+
+// set thread-local log handler; return previous thread-local handler
+MJAPI mjfLogHandler _mjPRIVATE_setTlsLogHandler(mjfLogHandler handler);
+
+// get the currently active global log handler (read-only, no modification)
+MJAPI mjfLogHandler _mjPRIVATE_getGlobalLogHandler(void);
+
+// check whether an info topic is enabled
+MJAPI mjtBool mju_isTopicEnabled(int topic);
+
+// strip directory from __FILE__ (cross-platform)
+static inline const char* BaseName(const char* path) {
+  const char* slash = strrchr(path, '/');
+  const char* bslash = strrchr(path, '\\');
+  if (slash && bslash) return (slash > bslash ? slash : bslash) + 1;
+  if (slash) return slash + 1;
+  if (bslash) return bslash + 1;
+  return path;
+}
+
+// internal macro to emit a structured error with source location
+#define mjERROR(...)                                           \
+  {                                                            \
+    mjLogMessage _msg = {.level = mjLOG_ERROR,                 \
+                         .func = __func__,                     \
+                         .file = __FILE__,                     \
+                         .line = __LINE__};                    \
+    snprintf(_msg.subject, sizeof(_msg.subject), __VA_ARGS__); \
+    mju_message(&_msg);                                        \
+  }
+
+// internal macro to emit a structured debug trace with fast producer-side topic filtering
+#ifndef MJ_DISABLE_DEBUG_TRACING
+#define mjDEBUG(_topic, ...)                                   \
+  if (mju_isTopicEnabled(_topic)) {                            \
+    mjLogMessage _msg = {.level = mjLOG_DEBUG,                 \
+                         .topic = _topic,                      \
+                         .func = __func__};                    \
+    snprintf(_msg.subject, sizeof(_msg.subject), __VA_ARGS__); \
+    mju_message(&_msg);                                        \
+  }
+#else
+#define mjDEBUG(_topic, ...) ((void)0)
+#endif
+
 #ifdef __cplusplus
 }
 #endif