Implement threading for island constraint solving.

Humanoids22 No threads:
 Benchmark Execution Time: 9.475905812s

Humanoids22 with 10 Threads:
 Benchmark Execution Time: 4.871352s

PiperOrigin-RevId: 571214307
Change-Id: I1f4f2c761b4ae6bc8fac1f28c6c695f6c499f339

src/thread/thread_pool.cc

@@ -206,7 +206,10 @@ mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id) {
   // align the end of the shard to be mjStackInfo.
   misalignment = result % alignof(mjStackInfo);
   result -= misalignment;
-
+#ifdef ADDRESS_SANITIZER
+  // Ensure StackInfo is always accessible
+  ASAN_UNPOISON_MEMORY_REGION((void*)result, sizeof(mjStackInfo));
+#endif
   return (mjStackInfo*) result;
 }
 
@@ -253,7 +256,7 @@ static void  ConfigureMultiThreadedStack(mjData* d) {
 }
 
 // adds a thread pool to mjData and configures it for multi-threaded use.
-void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool) {
+void mju_bindThreadPool(mjData* d, void* thread_pool) {
   if (d->threadpool) {
     mju_error("Thread Pool already bound to mjData");
   }

src/thread/thread_pool.h

@@ -54,7 +54,7 @@ MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
 mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id);
 
 // Adds a thread pool to mjData and configures it for multi-threaded use.
-MJAPI void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool);
+MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool);
 
 // Gets the number of running threads in the thread pool.
 MJAPI size_t mju_threadPoolNumberOfThreads(mjThreadPool* thread_pool);

src/thread/thread_task.h

@@ -15,14 +15,13 @@
 #ifndef MUJOCO_SRC_THREAD_THREAD_TASK_H_
 #define MUJOCO_SRC_THREAD_THREAD_TASK_H_
 
-#include <atomic>
-#include <new>
-#include <type_traits>
-
 #include <mujoco/mjexport.h>
 #include <mujoco/mjthread.h>
 
 #ifdef __cplusplus
+#include <atomic>
+#include <new>
+#include <type_traits>
 namespace mujoco {
 extern "C" {
 #endif