mjbatch-uni 0.2.4__tar.gz → 0.2.5__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -17,45 +17,21 @@ execute_process(
17
17
  OUTPUT_VARIABLE MUJOCO_PKG_DIR
18
18
  COMMAND_ERROR_IS_FATAL ANY)
19
19
  if(WIN32)
20
- set(MUJOCO_DLL "${MUJOCO_PKG_DIR}/mujoco.dll")
21
- if(NOT EXISTS "${MUJOCO_DLL}")
22
- message(FATAL_ERROR "Could not find mujoco.dll in ${MUJOCO_PKG_DIR}")
23
- endif()
24
-
25
- # The MuJoCo wheel exports the C API from mujoco.dll but does not ship an
26
- # MSVC import library. Generate one at build time; it is not installed.
27
- get_filename_component(MSVC_TOOL_BIN_DIR "${CMAKE_CXX_COMPILER}" DIRECTORY)
28
- find_program(MSVC_LIB_EXECUTABLE NAMES lib HINTS "${MSVC_TOOL_BIN_DIR}")
29
- if(NOT MSVC_LIB_EXECUTABLE)
30
- message(FATAL_ERROR "Could not find MSVC lib.exe")
31
- endif()
32
- find_program(MSVC_DUMPBIN_EXECUTABLE NAMES dumpbin HINTS "${MSVC_TOOL_BIN_DIR}")
33
- if(NOT MSVC_DUMPBIN_EXECUTABLE)
34
- message(FATAL_ERROR "Could not find MSVC dumpbin.exe")
35
- endif()
36
-
20
+ # The Windows wheel ships mujoco.dll without an import library, so make one from
21
+ # the DLL's export table.
37
22
  execute_process(
38
- COMMAND "${MSVC_DUMPBIN_EXECUTABLE}" "/EXPORTS" "${MUJOCO_DLL}"
23
+ COMMAND "${CMAKE_LINKER}" /dump /exports "${MUJOCO_PKG_DIR}/mujoco.dll"
39
24
  OUTPUT_VARIABLE MUJOCO_EXPORTS
40
25
  COMMAND_ERROR_IS_FATAL ANY)
41
- string(REGEX MATCHALL
42
- " +[0-9]+ +[0-9A-Fa-f]+ +[0-9A-Fa-f]+ +([^ \r\n]+)"
43
- MUJOCO_EXPORT_MATCHES "${MUJOCO_EXPORTS}")
44
- if(NOT MUJOCO_EXPORT_MATCHES)
45
- message(FATAL_ERROR "Could not parse exports from ${MUJOCO_DLL}")
46
- endif()
47
-
48
- set(MUJOCO_DEF_FILE "${CMAKE_CURRENT_BINARY_DIR}/mujoco.def")
49
- file(WRITE "${MUJOCO_DEF_FILE}" "LIBRARY mujoco.dll\nEXPORTS\n")
50
- foreach(export_match IN LISTS MUJOCO_EXPORT_MATCHES)
51
- string(REGEX REPLACE ".* +([^ \r\n]+)$" "\\1" export_symbol "${export_match}")
52
- file(APPEND "${MUJOCO_DEF_FILE}" " ${export_symbol}\n")
53
- endforeach()
54
-
26
+ string(REGEX MATCHALL "[0-9]+ +[0-9A-F]+ +[0-9A-F]+ +[A-Za-z_][A-Za-z0-9_]*"
27
+ MUJOCO_EXPORTS "${MUJOCO_EXPORTS}")
28
+ list(TRANSFORM MUJOCO_EXPORTS REPLACE "^.* " "")
29
+ list(JOIN MUJOCO_EXPORTS "\n" MUJOCO_EXPORTS)
30
+ file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/mujoco.def" "EXPORTS\n${MUJOCO_EXPORTS}\n")
55
31
  set(MUJOCO_LIBRARY "${CMAKE_CURRENT_BINARY_DIR}/mujoco.lib")
56
32
  execute_process(
57
- COMMAND "${MSVC_LIB_EXECUTABLE}" "/def:${MUJOCO_DEF_FILE}" "/machine:x64"
58
- "/out:${MUJOCO_LIBRARY}"
33
+ COMMAND "${CMAKE_AR}" /nologo /machine:x64 /name:mujoco.dll
34
+ "/def:${CMAKE_CURRENT_BINARY_DIR}/mujoco.def" "/out:${MUJOCO_LIBRARY}"
59
35
  COMMAND_ERROR_IS_FATAL ANY)
60
36
  else()
61
37
  file(GLOB MUJOCO_LIBRARY "${MUJOCO_PKG_DIR}/libmujoco*${CMAKE_SHARED_LIBRARY_SUFFIX}*")
@@ -66,17 +42,16 @@ else()
66
42
  endif()
67
43
 
68
44
  nanobind_add_module(_bindings FREE_THREADED NOMINSIZE src/mjbatch/csrc/bindings.cpp)
69
- if(WIN32)
70
- # Windows headers are nested one directory deeper than the Unix wheels.
71
- target_include_directories(_bindings PRIVATE "${MUJOCO_PKG_DIR}/include/mujoco")
72
- else()
73
- target_include_directories(_bindings PRIVATE "${MUJOCO_PKG_DIR}/include")
74
- endif()
45
+ # The Windows wheel nests the headers one directory deeper.
46
+ find_path(MUJOCO_INCLUDE_DIR mujoco/mujoco.h REQUIRED NO_DEFAULT_PATH
47
+ PATHS "${MUJOCO_PKG_DIR}/include" "${MUJOCO_PKG_DIR}/include/mujoco")
48
+ target_include_directories(_bindings PRIVATE "${MUJOCO_INCLUDE_DIR}")
75
49
  target_link_libraries(_bindings PRIVATE "${MUJOCO_LIBRARY}")
76
50
 
77
51
  # Find libmujoco next to the installed extension, <site-packages>/mujoco. The
78
52
  # macOS wheel's install name is a framework path that does not exist, so point
79
- # our load command at the plain file name instead.
53
+ # our load command at the plain file name instead. Windows has no rpath: the
54
+ # package adds that directory to the DLL search path before importing us.
80
55
  if(APPLE)
81
56
  get_filename_component(MUJOCO_LIB_NAME "${MUJOCO_LIBRARY}" NAME)
82
57
  execute_process(COMMAND otool -D "${MUJOCO_LIBRARY}" OUTPUT_VARIABLE _otool)
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: mjbatch-uni
3
- Version: 0.2.4
3
+ Version: 0.2.5
4
4
  Summary: A Python library for running thousands of MuJoCo simulations in parallel on CPU (unilabsim fork).
5
5
  Keywords: mujoco,physics,simulation,batched,reinforcement-learning
6
6
  Author-Email: Kevin Zakka <zakka@berkeley.edu>, Yufei Jia <jyf23@mails.tsinghua.edu.cn>
@@ -42,7 +42,7 @@ Features include:
42
42
  * Per-simulation model parameters, with `expand` for MjModel fields and `set_const` to recompute derived constants.
43
43
  * Same-layout compiler-coherent mesh variants, with `VariantPack.from_specs()` and `Batch.from_variant_pack()`.
44
44
  * Explicit topology-affine groups, with `ModelAffineBatch` routing global ids to incompatible layouts.
45
- * Batched queries beyond stepping: site Jacobians with `jac_site` and heightfield sampling with `sample_hfield` (world/yaw grid alignment); query ops return caller-allocated results without refreshing the bound views.
45
+ * Batched queries beyond stepping: ray casting with `rays`, body-point Jacobians with `jac`, and heightfield sampling with `sample_hfield` (world/yaw grid alignment); query ops return caller-allocated results without refreshing the bound views.
46
46
  * Per-substep control from Python with `step(..., callback=...)`.
47
47
  * Opt-in split substeps with fresh position/velocity sensor views for body wrench control.
48
48
  * Optional per-worker CPU pinning on Linux, with `cpu_ids` binding pool worker `i` to `cpu_ids[i]`.
@@ -12,7 +12,7 @@ Features include:
12
12
  * Per-simulation model parameters, with `expand` for MjModel fields and `set_const` to recompute derived constants.
13
13
  * Same-layout compiler-coherent mesh variants, with `VariantPack.from_specs()` and `Batch.from_variant_pack()`.
14
14
  * Explicit topology-affine groups, with `ModelAffineBatch` routing global ids to incompatible layouts.
15
- * Batched queries beyond stepping: site Jacobians with `jac_site` and heightfield sampling with `sample_hfield` (world/yaw grid alignment); query ops return caller-allocated results without refreshing the bound views.
15
+ * Batched queries beyond stepping: ray casting with `rays`, body-point Jacobians with `jac`, and heightfield sampling with `sample_hfield` (world/yaw grid alignment); query ops return caller-allocated results without refreshing the bound views.
16
16
  * Per-substep control from Python with `step(..., callback=...)`.
17
17
  * Opt-in split substeps with fresh position/velocity sensor views for body wrench control.
18
18
  * Optional per-worker CPU pinning on Linux, with `cpu_ids` binding pool worker `i` to `cpu_ids[i]`.
@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
4
4
 
5
5
  [project]
6
6
  name = "mjbatch-uni"
7
- version = "0.2.4"
7
+ version = "0.2.5"
8
8
  description = "A Python library for running thousands of MuJoCo simulations in parallel on CPU (unilabsim fork)."
9
9
  readme = "README.md"
10
10
  license = "Apache-2.0"
@@ -80,7 +80,7 @@ testpaths = ["tests"]
80
80
 
81
81
  [tool.cibuildwheel]
82
82
  build = "cp310-* cp311-* cp312-* cp313-*"
83
- skip = "*-musllinux_* *_i686"
83
+ skip = "*-musllinux_* *-win32 *_i686"
84
84
  test-requires = "pytest"
85
85
  test-command = "pytest {project}/tests"
86
86
 
@@ -1,15 +1,22 @@
1
1
  # SPDX-License-Identifier: Apache-2.0
2
2
 
3
+ import os
4
+ import sys
3
5
  from collections.abc import Sequence
4
6
  from contextlib import contextmanager
5
7
  from dataclasses import dataclass
6
8
  from functools import cached_property
9
+ from pathlib import Path
7
10
  from typing import Any, Iterator, Mapping
8
11
 
9
12
  import mujoco
10
13
  import numpy as np
11
14
 
12
- from mjbatch._bindings import Batch as _Batch
15
+ if sys.platform == "win32":
16
+ # The extension links mujoco.dll, which lives in the mujoco package.
17
+ os.add_dll_directory(str(Path(mujoco.__file__).parent))
18
+
19
+ from mjbatch._bindings import Batch as _Batch # noqa: E402
13
20
  from mjbatch.groups import ModelAffineBatch as ModelAffineBatch
14
21
  from mjbatch.groups import TopologyGroup as TopologyGroup
15
22
  from mjbatch.model import ModelFieldSpec as ModelFieldSpec
@@ -196,20 +203,6 @@ class Batch(_Batch):
196
203
  ids = np.asarray(ids)
197
204
  return int(ids.sum()) if ids.dtype == bool else len(ids)
198
205
 
199
- def jac_site( # pyright: ignore[reportIncompatibleMethodOverride] # name-based allocating wrapper
200
- self, name: str, ids: Any = None
201
- ) -> tuple[np.ndarray, np.ndarray]:
202
- """World-frame position/rotation Jacobians of a site, per selected simulation.
203
-
204
- Returns (jacp, jacr) with shape (nsel, 3, nv). Runs kinematics and comPos
205
- only, not mj_forward, and does not refresh the bound views."""
206
- i = self.model.site(name).id
207
- n = self._nsel(ids)
208
- jacp = np.zeros((n, 3, self.model.nv))
209
- jacr = np.zeros((n, 3, self.model.nv))
210
- super().jac_site(i, jacp, jacr, ids)
211
- return jacp, jacr
212
-
213
206
  def sample_hfield( # pyright: ignore[reportIncompatibleMethodOverride] # name-based allocating wrapper
214
207
  self,
215
208
  geom: str,
@@ -1,6 +1,7 @@
1
1
  from collections.abc import Callable, Sequence
2
2
  from typing import Annotated
3
3
 
4
+ import numpy
4
5
  from numpy.typing import NDArray
5
6
 
6
7
 
@@ -18,19 +19,23 @@ class Batch:
18
19
  bind("state") returns the (N, nstate) integration states themselves, in
19
20
  mj_getState's mjSTATE_INTEGRATION order: opaque rows for copying, saving and
20
21
  restoring simulations, native dtype only. A row written is the simulation's
21
- state at its next call, with pending field writes applied on top; the field
22
- views are stale until then. reset discards it, as it discards a field write.
22
+ state at its next call, with pending writes to copied fields (below) applied on
23
+ top. reset applies it element by element where it changed, like a field write.
23
24
 
24
25
  bind(field) returns an (N, ...) array over an mjData field in MjData's layout.
25
- Input fields (the mjtState components and warning) are copied in before
26
- a physics call, element by element, where they changed since we last wrote
27
- them; every bound field is copied out after, and a derived field bound between
28
- calls is filled for a simulation by its next call. reset applies pending writes
29
- after mj_resetData and then runs mj_forward, so derived fields are valid. After
30
- step, sensordata and every derived field are one substep behind qpos and qvel,
31
- as with mj_step itself, unless the batch was made with forward=True, which ends
32
- every step with mj_forward so they are current, at the cost of one forward per
33
- simulation per call.
26
+ An mjtNum state component bound in its native dtype (qpos, qvel, ctrl,
27
+ mocap_pos, ...) is a strided view into the state rows, so a write through it and
28
+ a write to the row are one write, the later winning. The other input fields
29
+ (eq_active, warning, and float32 bindings) are copies, copied in before a
30
+ physics call, element by element, where they changed since we last wrote them;
31
+ after a state write they are stale until the next call, and a write of the
32
+ stale value goes unseen. Every bound field is copied out after a call, and a
33
+ derived field bound between calls is filled for a simulation by its next call.
34
+ reset applies pending writes after mj_resetData and then runs mj_forward, so
35
+ derived fields are valid. After step, sensordata and every derived field are
36
+ one substep behind qpos and qvel, as with mj_step itself, unless the batch was
37
+ made with forward=True, which ends every step with mj_forward so they are
38
+ current, at the cost of one forward per simulation per call.
34
39
 
35
40
  expand(field) returns (N, ...) per-simulation values of an mjModel field, seeded
36
41
  from the model and applied before every physics call for that simulation.
@@ -49,6 +54,11 @@ class Batch:
49
54
  the first failing simulation; the others still ran, and the failing one keeps the
50
55
  state it had before the call, its writes still pending. The trap is a MuJoCo log
51
56
  handler installed at import; installing another handler later disables it.
57
+
58
+ rays and jac are queries: mj_ray and mj_jac for each simulation against its own
59
+ geometry, which is its state with pending writes on top and its expanded model
60
+ fields. A query runs only the part of the pipeline it needs, consumes no pending
61
+ write, and updates no bound field.
52
62
  """
53
63
 
54
64
  def __init__(self, model: object, num_sims: int, num_threads: int = 0, forward: bool = False, cpu_ids: Sequence[int] | None = None) -> None:
@@ -95,14 +105,19 @@ class Batch:
95
105
  mj_resetData, or mj_resetDataKeyframe when keyframe >= 0, then mj_forward.
96
106
  """
97
107
 
98
- def jac_site(self, site: int, jacp: NDArray | None = None, jacr: NDArray | None = None, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None) -> None:
99
- """
100
- mj_jacSite per selected simulation into caller-allocated (sel, 3, nv) rows; either output may be None. Runs mj_kinematics and mj_comPos only, not mj_forward, and does not refresh the bound views: the outputs are the caller-allocated rows.
101
- """
102
-
103
108
  def sample_hfield(self, geom: int, body: int, offsets: NDArray, out: NDArray, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None, alignment: str = 'world') -> None:
104
109
  """
105
110
  Bilinear hfield sampling per selected simulation at XY offsets around a frame body's origin, into caller-allocated (sel, npoint) rows: the world z of the sampled hfield surface (the local elevation for an unrotated geom at the origin). alignment rotates the sampling grid: "world" keeps offsets in world axes, "yaw" rotates them by the frame body's yaw about world z. Runs mj_kinematics only, not mj_forward, and does not refresh the bound views. All simulations sample the template's hfield; a per-sim geom_pos or geom_quat moves the sampling frame.
106
111
  """
107
112
 
108
113
  def set_const(self, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None) -> None: ...
114
+
115
+ def rays(self, pnt: NDArray, vec: NDArray, dist: NDArray, geomid: NDArray | None = None, normal: NDArray | None = None, geomgroup: Annotated[NDArray[numpy.uint8], dict(shape=(6), order='C', writable=False)] | None = None, flg_static: bool = True, bodyexclude: Annotated[NDArray[numpy.int32], dict(shape=(None,), order='C', writable=False)] | None = None, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None) -> None:
116
+ """
117
+ mj_ray for every ray of every simulation. pnt and vec are (num_sims, nray, 3) origins and directions in the world frame, float32 or the native dtype. dist (num_sims, nray), and the optional geomid (int32) and normal (num_sims, nray, 3), are caller-allocated and filled in place: dist is -1 and normal zero where a ray hits nothing. geomgroup is mj_ray's six-entry uint8 mask, bodyexclude one int32 body id per ray (-1 for none), and ids restricts which rows are computed.
118
+ """
119
+
120
+ def jac(self, jacp: NDArray | None, jacr: NDArray | None, point: NDArray, body: Annotated[NDArray[numpy.int32], dict(shape=(None,), order='C', writable=False)], ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None) -> None:
121
+ """
122
+ mj_jac for one point per simulation. point is (num_sims, 3) in the world frame, float32 or the native dtype, and body (num_sims,) int32 is the body it moves with. jacp and jacr are caller-allocated (num_sims, 3, nv) arrays filled in place with the translational and rotational Jacobians; either may be None. ids restricts which rows are computed.
123
+ """
@@ -17,6 +17,8 @@
17
17
  #include <cstdlib>
18
18
  #include <cstring>
19
19
  #include <exception>
20
+ #include <functional>
21
+ #include <initializer_list>
20
22
  #include <memory>
21
23
  #include <mutex>
22
24
  #include <optional>
@@ -162,6 +164,29 @@ inline bool IsInput(std::string_view name) {
162
164
  return false;
163
165
  }
164
166
 
167
+ // The mjtState component a data field is stored as, or 0.
168
+ inline int StateBit(std::string_view name) {
169
+ static constexpr std::pair<std::string_view, int> bits[] = {
170
+ {"time", mjSTATE_TIME},
171
+ {"qpos", mjSTATE_QPOS},
172
+ {"qvel", mjSTATE_QVEL},
173
+ {"act", mjSTATE_ACT},
174
+ {"history", mjSTATE_HISTORY},
175
+ {"qacc_warmstart", mjSTATE_WARMSTART},
176
+ {"ctrl", mjSTATE_CTRL},
177
+ {"qfrc_applied", mjSTATE_QFRC_APPLIED},
178
+ {"xfrc_applied", mjSTATE_XFRC_APPLIED},
179
+ {"eq_active", mjSTATE_EQ_ACTIVE},
180
+ {"mocap_pos", mjSTATE_MOCAP_POS},
181
+ {"mocap_quat", mjSTATE_MOCAP_QUAT},
182
+ {"userdata", mjSTATE_USERDATA},
183
+ {"plugin_state", mjSTATE_PLUGIN}};
184
+ for (auto [n, bit] : bits) {
185
+ if (name == n) return bit;
186
+ }
187
+ return 0;
188
+ }
189
+
165
190
  inline bool IsAsset(std::string_view name) {
166
191
  for (std::string_view p : {"mesh_", "hfield_", "tex_", "skin_", "bvh_", "oct_"}) {
167
192
  if (name.substr(0, p.size()) == p) return true;
@@ -247,6 +272,7 @@ struct Slot {
247
272
  size_t row; // bytes per sim
248
273
  std::unique_ptr<uint8_t[]> buf;
249
274
  std::unique_ptr<uint8_t[]> mirror; // buf as last written by us; input data fields only
275
+ int offset = -1; // into each state row when the view is the state itself (no buf)
250
276
  };
251
277
 
252
278
  inline void ToBuf(uint8_t* dst, const void* src, const Slot& s) {
@@ -308,12 +334,9 @@ struct Scalars {
308
334
  // BatchEnvPool.sample_hfield_height convention, which this op replaces.
309
335
  enum class HfieldAlign { World, Yaw };
310
336
 
311
- // Per-call context for the query ops (JacSite, SampleHfield): outputs are
312
- // caller-allocated arrays, one row per selected sim, like step's history.
337
+ // Per-call context for the query ops (SampleHfield): outputs are caller-
338
+ // allocated arrays, one row per selected sim, like step's history.
313
339
  struct QueryCtx {
314
- int site = -1; // JacSite: site id
315
- mjtNum* jacp = nullptr; // (sel, 3, nv) rows, either may be null
316
- mjtNum* jacr = nullptr;
317
340
  int geom = -1, body = -1; // SampleHfield: hfield geom id, frame body id
318
341
  const mjtNum* offsets = nullptr; // (npoint, 2) grid-frame XY around the body origin
319
342
  int npoint = 0;
@@ -471,8 +494,8 @@ class Batch {
471
494
  Reset,
472
495
  RefreshSensor,
473
496
  SetConst,
474
- JacSite,
475
- SampleHfield
497
+ SampleHfield,
498
+ Query
476
499
  };
477
500
  using Ids = nb::ndarray<nb::ndim<1>, nb::c_contig>;
478
501
 
@@ -493,10 +516,12 @@ class Batch {
493
516
  for (int t = 0; t < pool_->size(); ++t) data_.push_back(mj_makeData(template_));
494
517
  nstate_ = mj_stateSize(template_, mjSTATE_INTEGRATION);
495
518
  states_.resize(static_cast<size_t>(num_sims) * nstate_);
519
+ written_.resize(states_.size());
496
520
  warnings_.resize(static_cast<size_t>(num_sims) * mjNWARNING);
497
521
  for (int i = 0; i < num_sims; ++i) {
498
522
  mj_getState(template_, data_[0], State(i), mjSTATE_INTEGRATION);
499
523
  }
524
+ written_ = states_;
500
525
  }
501
526
 
502
527
  ~Batch() {
@@ -526,6 +551,19 @@ class Batch {
526
551
  for (auto& s : bound_) {
527
552
  if (s->info == &f) return Matching(*s, f32);
528
553
  }
554
+ // An mjtNum state component is a strided view into the state rows, so a
555
+ // write through either lands in the same place and none can go stale.
556
+ if (int bit = StateBit(f.name); bit && f.elem == Elem::Num && !f32) {
557
+ auto s = std::make_unique<Slot>();
558
+ s->info = &f;
559
+ s->f32 = false;
560
+ s->offset = mj_stateSize(template_, mjSTATE_INTEGRATION & (bit - 1));
561
+ if (mj_stateSize(template_, bit) != f.nr * f.nc) {
562
+ throw std::runtime_error(std::string(f.name) + " does not match its state component");
563
+ }
564
+ bound_.push_back(std::move(s));
565
+ return View(*bound_.back());
566
+ }
529
567
  return View(BoundOrAdd(f, f32));
530
568
  }
531
569
 
@@ -639,25 +677,6 @@ class Batch {
639
677
  if (!error_.empty()) throw std::runtime_error(error_);
640
678
  }
641
679
 
642
- // mj_jacSite per selected sim into caller-allocated (sel, 3, nv) rows; either
643
- // output may be omitted. Runs kinematics and comPos only, not mj_forward, and
644
- // does not refresh the bound views: the outputs are the caller-allocated rows.
645
- void jac_site(int site, std::optional<nb::ndarray<>> jacp,
646
- std::optional<nb::ndarray<>> jacr, std::optional<Ids> ids) {
647
- ReentryGuard();
648
- if (site < 0 || site >= template_->nsite) throw nb::value_error("site out of range");
649
- auto sel = Parse(ids);
650
- int nsel = sel ? static_cast<int>(sel->size()) : num_sims_;
651
- QueryCtx ctx;
652
- ctx.site = site;
653
- ctx.jacp = jacp ? OutPtr(*jacp, nsel, 3, template_->nv, "jacp") : nullptr;
654
- ctx.jacr = jacr ? OutPtr(*jacr, nsel, 3, template_->nv, "jacr") : nullptr;
655
- if (!ctx.jacp && !ctx.jacr) {
656
- throw nb::value_error("jacp and jacr cannot both be None");
657
- }
658
- Run(Op::JacSite, std::move(sel), 0, nullptr, &ctx);
659
- }
660
-
661
680
  // Bilinear hfield sampling per selected sim at XY offsets around a frame
662
681
  // body's origin, into caller-allocated (sel, npoint) rows: the world z of
663
682
  // the sampled hfield surface. Runs kinematics only, not mj_forward, and does
@@ -692,7 +711,145 @@ class Batch {
692
711
  Run(Op::SampleHfield, std::move(sel), 0, nullptr, &ctx);
693
712
  }
694
713
 
714
+ // Queries read a sim's state with its pending writes on top, and its expanded model
715
+ // fields, and write nothing back. Each runs the part of the pipeline it needs.
716
+
717
+ // mj_ray for (N, R) rays given in the world frame.
718
+ void rays(nb::ndarray<> pnt, nb::ndarray<> vec, nb::ndarray<> dist,
719
+ std::optional<nb::ndarray<>> geomid, std::optional<nb::ndarray<>> normal,
720
+ std::optional<nb::ndarray<const uint8_t, nb::shape<mjNGROUP>, nb::c_contig>> geomgroup,
721
+ bool flg_static,
722
+ std::optional<nb::ndarray<const int32_t, nb::ndim<1>, nb::c_contig>> bodyexclude,
723
+ std::optional<Ids> ids) {
724
+ if (pnt.ndim() != 3) throw nb::value_error("pnt must have shape (num_sims, nray, 3)");
725
+ const size_t n = num_sims_, nray = pnt.shape(1);
726
+ const Elem real = RealOf(pnt);
727
+ const bool f32 = real == Elem::Float;
728
+ const void* origins = Array(pnt, "pnt", real, {n, nray, 3});
729
+ const void* directions = Array(vec, "vec", real, {n, nray, 3});
730
+ void* dists = Array(dist, "dist", real, {n, nray});
731
+ int* geomids =
732
+ geomid ? static_cast<int*>(Array(*geomid, "geomid", Elem::Int, {n, nray})) : nullptr;
733
+ void* normals = normal ? Array(*normal, "normal", real, {n, nray, 3}) : nullptr;
734
+ const mjtByte* group = geomgroup ? geomgroup->data() : nullptr;
735
+ if (bodyexclude && bodyexclude->shape(0) != nray) {
736
+ throw nb::value_error("bodyexclude must have nray entries");
737
+ }
738
+ const int* exclude = bodyexclude ? bodyexclude->data() : nullptr;
739
+ Query(ids, [=](const mjModel* m, mjData* d, int i) {
740
+ mj_kinematics(m, d);
741
+ if (m->nflex) mj_flex(m, d);
742
+ for (size_t k = 0; k < nray; ++k) {
743
+ const size_t at = i * nray + k;
744
+ mjtNum origin[3], direction[3], hit_normal[3];
745
+ for (int c = 0; c < 3; ++c) {
746
+ origin[c] = Get(origins, f32, 3 * at + c);
747
+ direction[c] = Get(directions, f32, 3 * at + c);
748
+ }
749
+ int hit = -1;
750
+ const mjtNum x = mj_ray(m, d, origin, direction, group, flg_static,
751
+ exclude ? exclude[k] : -1, &hit, normals ? hit_normal : nullptr);
752
+ Put(dists, f32, at, x);
753
+ if (geomids) geomids[at] = hit;
754
+ for (int c = 0; normals && c < 3; ++c)
755
+ Put(normals, f32, 3 * at + c, x < 0 ? 0 : hit_normal[c]);
756
+ }
757
+ });
758
+ }
759
+
760
+ // mj_jac: the Jacobians of a world-frame point moving with a body.
761
+ void jac(std::optional<nb::ndarray<>> jacp, std::optional<nb::ndarray<>> jacr,
762
+ nb::ndarray<> point, nb::ndarray<const int32_t, nb::ndim<1>, nb::c_contig> body,
763
+ std::optional<Ids> ids) {
764
+ const size_t n = num_sims_, row = 3 * static_cast<size_t>(template_->nv);
765
+ const Elem real = RealOf(point);
766
+ const bool f32 = real == Elem::Float;
767
+ const void* points = Array(point, "point", real, {n, 3});
768
+ void* positional = jacp ? Array(*jacp, "jacp", real, {n, 3, row / 3}) : nullptr;
769
+ void* rotational = jacr ? Array(*jacr, "jacr", real, {n, 3, row / 3}) : nullptr;
770
+ if (body.shape(0) != n) throw nb::value_error("body must have num_sims entries");
771
+ const int* bodies = body.data();
772
+ for (size_t i = 0; i < n; ++i) {
773
+ if (bodies[i] < 0 || bodies[i] >= template_->nbody) {
774
+ throw nb::value_error("body ids must be in [0, nbody)");
775
+ }
776
+ }
777
+ Query(ids, [=](const mjModel* m, mjData* d, int i) {
778
+ mj_kinematics(m, d);
779
+ mj_comPos(m, d);
780
+ const mjtNum p[3] = {Get(points, f32, 3 * i), Get(points, f32, 3 * i + 1),
781
+ Get(points, f32, 3 * i + 2)};
782
+ if (!f32) {
783
+ auto at = [=](void* a) { return a ? static_cast<mjtNum*>(a) + i * row : nullptr; };
784
+ mj_jac(m, d, at(positional), at(rotational), p, bodies[i]);
785
+ return;
786
+ }
787
+ mj_markStack(d);
788
+ mjtNum* jp = positional ? mj_stackAllocNum(d, row) : nullptr;
789
+ mjtNum* jr = rotational ? mj_stackAllocNum(d, row) : nullptr;
790
+ mj_jac(m, d, jp, jr, p, bodies[i]);
791
+ for (size_t k = 0; k < row; ++k) {
792
+ if (jp) Put(positional, true, i * row + k, jp[k]);
793
+ if (jr) Put(rotational, true, i * row + k, jr[k]);
794
+ }
795
+ mj_freeStack(d);
796
+ });
797
+ }
798
+
695
799
  private:
800
+ using QueryFn = std::function<void(const mjModel*, mjData*, int)>;
801
+
802
+ void Query(const std::optional<Ids>& ids, const QueryFn& fn) {
803
+ ReentryGuard();
804
+ auto sel = Parse(ids);
805
+ nb::gil_scoped_release release;
806
+ std::lock_guard<std::mutex> lock(mu_);
807
+ error_.clear();
808
+ query_ = &fn;
809
+ RunLocked(Op::Query, sel, 0);
810
+ query_ = nullptr;
811
+ if (!error_.empty()) throw std::runtime_error(error_);
812
+ }
813
+
814
+ // float32 or mjtNum, whichever the array that sets a query's precision holds.
815
+ static Elem RealOf(const nb::ndarray<>& a) {
816
+ return a.dtype() == nb::dtype<float>() && sizeof(mjtNum) != sizeof(float) ? Elem::Float
817
+ : Elem::Num;
818
+ }
819
+
820
+ static void* Array(const nb::ndarray<>& a, const char* name, Elem elem,
821
+ std::initializer_list<size_t> shape) {
822
+ const auto dtype = elem == Elem::Int ? nb::dtype<int>()
823
+ : elem == Elem::Float ? nb::dtype<float>()
824
+ : nb::dtype<mjtNum>();
825
+ bool ok = a.ndim() == shape.size() && a.dtype() == dtype &&
826
+ a.device_type() == nb::device::cpu::value && IsCContig(a);
827
+ std::string want;
828
+ size_t k = 0;
829
+ for (size_t extent : shape) {
830
+ ok = ok && a.shape(k++) == extent;
831
+ want += (want.empty() ? "" : ", ") + std::to_string(extent);
832
+ }
833
+ if (!ok) {
834
+ throw nb::value_error((std::string(name) + " must be a C-contiguous CPU " + DtypeName(elem) +
835
+ " array of shape (" + want + ")")
836
+ .c_str());
837
+ }
838
+ return a.data();
839
+ }
840
+
841
+ static mjtNum Get(const void* a, bool f32, size_t k) {
842
+ return f32 ? static_cast<const float*>(a)[k] : static_cast<const mjtNum*>(a)[k];
843
+ }
844
+
845
+ static void Put(void* a, bool f32, size_t k, mjtNum v) {
846
+ if (f32) {
847
+ static_cast<float*>(a)[k] = static_cast<float>(v);
848
+ } else {
849
+ static_cast<mjtNum*>(a)[k] = v;
850
+ }
851
+ }
852
+
696
853
  const FieldInfo& Field(const FieldTable& table, const std::string& name) {
697
854
  auto it = table.find(name);
698
855
  if (it == table.end()) {
@@ -840,6 +997,11 @@ class Batch {
840
997
  const FieldInfo& f = *s.info;
841
998
  size_t shape[3] = {static_cast<size_t>(num_sims_), static_cast<size_t>(f.nr),
842
999
  static_cast<size_t>(f.nc)};
1000
+ if (s.offset >= 0) {
1001
+ int64_t strides[3] = {nstate_, f.ndim == 2 ? f.nc : 1, 1};
1002
+ return nb::ndarray<nb::numpy>(states_.data() + s.offset, f.ndim + 1, shape, nb::find(this),
1003
+ strides, nb::dtype<mjtNum>());
1004
+ }
843
1005
  nb::dlpack::dtype dt;
844
1006
  if (s.f32 || f.elem == Elem::Float) {
845
1007
  dt = nb::dtype<float>();
@@ -856,6 +1018,7 @@ class Batch {
856
1018
  }
857
1019
 
858
1020
  void CopyOut(Slot& s, mjData* d, int i) {
1021
+ if (s.offset >= 0) return; // the state row itself
859
1022
  uint8_t* row = s.buf.get() + i * s.row;
860
1023
  ToBuf(row, s.info->get(d), s);
861
1024
  if (s.mirror) std::memcpy(s.mirror.get() + i * s.row, row, s.row);
@@ -883,6 +1046,7 @@ class Batch {
883
1046
  }
884
1047
 
885
1048
  mjtNum* State(int i) { return states_.data() + static_cast<size_t>(i) * nstate_; }
1049
+ mjtNum* Written(int i) { return written_.data() + static_cast<size_t>(i) * nstate_; }
886
1050
  mjWarningStat* Warning(int i) { return warnings_.data() + static_cast<size_t>(i) * mjNWARNING; }
887
1051
 
888
1052
  void RunSim(int t, int i, Op op, int arg, mjtNum* hist, const QueryCtx* ctx,
@@ -911,6 +1075,20 @@ class Batch {
911
1075
  } else {
912
1076
  TrapCall(mj_resetData, m, d);
913
1077
  }
1078
+ // Pending writes to the state row, through it or an aliasing field view, land
1079
+ // on top of the reset element by element, like pending field writes below.
1080
+ const mjtNum* row = State(i);
1081
+ const mjtNum* last = Written(i);
1082
+ if (std::memcmp(row, last, nstate_ * sizeof(mjtNum)) != 0) {
1083
+ mj_markStack(d);
1084
+ mjtNum* merged = mj_stackAllocNum(d, nstate_);
1085
+ mj_getState(m, d, merged, mjSTATE_INTEGRATION);
1086
+ for (int k = 0; k < nstate_; ++k) {
1087
+ if (std::memcmp(row + k, last + k, sizeof(mjtNum)) != 0) merged[k] = row[k];
1088
+ }
1089
+ mj_setState(m, d, merged, mjSTATE_INTEGRATION);
1090
+ mj_freeStack(d);
1091
+ }
914
1092
  } else {
915
1093
  TrapCall(mj_setState, m, d, State(i), mjSTATE_INTEGRATION);
916
1094
  std::memcpy(d->warning, Warning(i), sizeof(d->warning));
@@ -1009,24 +1187,20 @@ class Batch {
1009
1187
  (*ctx->sensor_ranges)[j + 1] - (*ctx->sensor_ranges)[j]);
1010
1188
  }
1011
1189
  return;
1012
- // The query ops run kinematics only: they do not change the integration
1013
- // state, so the copy-out tail below is skipped. mjData is shared by every
1014
- // sim a worker serves, and copying out would leak another sim's stale
1015
- // derived fields into the bound views; the outputs are the
1016
- // caller-allocated rows.
1017
- case Op::JacSite:
1018
- TrapCall(mj_kinematics, m, d);
1019
- TrapCall(mj_comPos, m, d);
1020
- TrapCall(mj_jacSite, m, d, ctx->jacp, ctx->jacr, ctx->site);
1021
- return;
1190
+ // Queries do not change the integration state, so the copy-out tail
1191
+ // below is skipped; their outputs are the caller-allocated rows.
1022
1192
  case Op::SampleHfield:
1023
1193
  TrapCall(mj_kinematics, m, d);
1024
1194
  SampleHfield(m, d, *ctx);
1025
1195
  return;
1196
+ case Op::Query:
1197
+ (*query_)(m, d, i);
1198
+ return;
1026
1199
  case Op::SetConst:
1027
1200
  break;
1028
1201
  }
1029
1202
  TrapCall(mj_getState, m, d, State(i), mjSTATE_INTEGRATION);
1203
+ std::memcpy(Written(i), State(i), nstate_ * sizeof(mjtNum));
1030
1204
  std::memcpy(Warning(i), d->warning, sizeof(d->warning));
1031
1205
  for (auto& s : bound_) CopyOut(*s, d, i);
1032
1206
  }
@@ -1135,8 +1309,6 @@ class Batch {
1135
1309
  const QueryCtx* c = nullptr;
1136
1310
  if (ctx) {
1137
1311
  row_ctx = *ctx;
1138
- if (row_ctx.jacp) row_ctx.jacp += static_cast<size_t>(j) * 3 * template_->nv;
1139
- if (row_ctx.jacr) row_ctx.jacr += static_cast<size_t>(j) * 3 * template_->nv;
1140
1312
  if (row_ctx.out) row_ctx.out += static_cast<size_t>(j) * row_ctx.npoint;
1141
1313
  c = &row_ctx;
1142
1314
  }
@@ -1145,11 +1317,7 @@ class Batch {
1145
1317
  return;
1146
1318
  }
1147
1319
  };
1148
- if (pool_->size() == 1) {
1149
- for (int j = 0; j < n; ++j) fn(0, j);
1150
- } else {
1151
- pool_->Run(n, fn);
1152
- }
1320
+ pool_->Run(n, fn);
1153
1321
  }
1154
1322
 
1155
1323
  // One substep for every selected sim that has not run its copy-out tail yet.
@@ -1170,11 +1338,7 @@ class Batch {
1170
1338
  row.done = done + j;
1171
1339
  if (!Guarded(t, p ? p[j] : j, op, k, nullptr, nullptr, &row)) return;
1172
1340
  };
1173
- if (pool_->size() == 1) {
1174
- for (int j = 0; j < n; ++j) fn(0, j);
1175
- } else {
1176
- pool_->Run(n, fn);
1177
- }
1341
+ pool_->Run(n, fn);
1178
1342
  }
1179
1343
 
1180
1344
  // A step with a Python control callback: the calling thread invokes the
@@ -1263,6 +1427,7 @@ class Batch {
1263
1427
  std::vector<mjModel*> models_; // per worker, once anything is expanded
1264
1428
  int nstate_;
1265
1429
  std::vector<mjtNum> states_; // per sim, mjSTATE_INTEGRATION
1430
+ std::vector<mjtNum> written_; // states_ as last written by us, for reset
1266
1431
  std::vector<mjWarningStat> warnings_; // per sim
1267
1432
  std::vector<Scalars> scalars_; // per sim
1268
1433
  std::vector<std::unique_ptr<Slot>> bound_;
@@ -1270,6 +1435,7 @@ class Batch {
1270
1435
  std::set<const FieldInfo*> expanded_set_;
1271
1436
  const Slot* enableflags_ = nullptr; // scanned for mjENBL_SLEEP before every call
1272
1437
  std::set<const FieldInfo*> changed_; // set_const outputs not yet expanded
1438
+ const QueryFn* query_ = nullptr; // the query in flight, for workers
1273
1439
  std::unique_ptr<ThreadPool> pool_;
1274
1440
  std::mutex mu_; // serializes calls; held with the GIL released in Run
1275
1441
  std::mutex changed_mu_; // changed_ and error_ from workers
@@ -30,19 +30,23 @@ outside mjtState.
30
30
  bind("state") returns the (N, nstate) integration states themselves, in
31
31
  mj_getState's mjSTATE_INTEGRATION order: opaque rows for copying, saving and
32
32
  restoring simulations, native dtype only. A row written is the simulation's
33
- state at its next call, with pending field writes applied on top; the field
34
- views are stale until then. reset discards it, as it discards a field write.
33
+ state at its next call, with pending writes to copied fields (below) applied on
34
+ top. reset applies it element by element where it changed, like a field write.
35
35
 
36
36
  bind(field) returns an (N, ...) array over an mjData field in MjData's layout.
37
- Input fields (the mjtState components and warning) are copied in before
38
- a physics call, element by element, where they changed since we last wrote
39
- them; every bound field is copied out after, and a derived field bound between
40
- calls is filled for a simulation by its next call. reset applies pending writes
41
- after mj_resetData and then runs mj_forward, so derived fields are valid. After
42
- step, sensordata and every derived field are one substep behind qpos and qvel,
43
- as with mj_step itself, unless the batch was made with forward=True, which ends
44
- every step with mj_forward so they are current, at the cost of one forward per
45
- simulation per call.
37
+ An mjtNum state component bound in its native dtype (qpos, qvel, ctrl,
38
+ mocap_pos, ...) is a strided view into the state rows, so a write through it and
39
+ a write to the row are one write, the later winning. The other input fields
40
+ (eq_active, warning, and float32 bindings) are copies, copied in before a
41
+ physics call, element by element, where they changed since we last wrote them;
42
+ after a state write they are stale until the next call, and a write of the
43
+ stale value goes unseen. Every bound field is copied out after a call, and a
44
+ derived field bound between calls is filled for a simulation by its next call.
45
+ reset applies pending writes after mj_resetData and then runs mj_forward, so
46
+ derived fields are valid. After step, sensordata and every derived field are
47
+ one substep behind qpos and qvel, as with mj_step itself, unless the batch was
48
+ made with forward=True, which ends every step with mj_forward so they are
49
+ current, at the cost of one forward per simulation per call.
46
50
 
47
51
  expand(field) returns (N, ...) per-simulation values of an mjModel field, seeded
48
52
  from the model and applied before every physics call for that simulation.
@@ -61,7 +65,12 @@ mj_setConst on one model. A MuJoCo error on a worker raises RuntimeError naming
61
65
  the first failing simulation; the others still ran, and the failing one keeps the
62
66
  state it had before the call, its writes still pending. Errors are trapped with a
63
67
  thread-local MuJoCo handler scoped to each worker call, so process-global handlers
64
- remain available to the application.)")
68
+ remain available to the application.
69
+
70
+ rays and jac are queries: mj_ray and mj_jac for each simulation against its own
71
+ geometry, which is its state with pending writes on top and its expanded model
72
+ fields. A query runs only the part of the pipeline it needs, consumes no pending
73
+ write, and updates no bound field.)")
65
74
  .def(nb::init<nb::object, int, int, bool, std::optional<std::vector<int>>>(), "model"_a,
66
75
  "num_sims"_a, "num_threads"_a = 0, "forward"_a = false, "cpu_ids"_a = nb::none(),
67
76
  "num_threads=0 uses every logical CPU, clamped to num_sims. forward=True ends "
@@ -113,12 +122,6 @@ remain available to the application.)")
113
122
  "refresh_sensor_range(); columns outside every requested range are preserved.")
114
123
  .def("reset", &Batch::reset, "ids"_a.noconvert() = nb::none(), "keyframe"_a = -1,
115
124
  "mj_resetData, or mj_resetDataKeyframe when keyframe >= 0, then mj_forward.")
116
- .def("jac_site", &Batch::jac_site, "site"_a, "jacp"_a.noconvert() = nb::none(),
117
- "jacr"_a.noconvert() = nb::none(), "ids"_a.noconvert() = nb::none(),
118
- "mj_jacSite per selected simulation into caller-allocated (sel, 3, nv) rows; "
119
- "either output may be None. Runs mj_kinematics and mj_comPos only, not "
120
- "mj_forward, and does not refresh the bound views: the outputs are the "
121
- "caller-allocated rows.")
122
125
  .def("sample_hfield", &Batch::sample_hfield, "geom"_a, "body"_a,
123
126
  "offsets"_a.noconvert(), "out"_a.noconvert(), "ids"_a.noconvert() = nb::none(),
124
127
  "alignment"_a = "world",
@@ -130,5 +133,22 @@ remain available to the application.)")
130
133
  "mj_kinematics only, not mj_forward, and does not refresh the bound views. All "
131
134
  "simulations sample the template's hfield; a per-sim geom_pos or geom_quat "
132
135
  "moves the sampling frame.")
133
- .def("set_const", &Batch::set_const, "ids"_a.noconvert() = nb::none());
136
+ .def("set_const", &Batch::set_const, "ids"_a.noconvert() = nb::none())
137
+ .def("rays", &Batch::rays, "pnt"_a.noconvert(), "vec"_a.noconvert(), "dist"_a.noconvert(),
138
+ "geomid"_a.noconvert() = nb::none(), "normal"_a.noconvert() = nb::none(),
139
+ "geomgroup"_a.noconvert() = nb::none(), "flg_static"_a = true,
140
+ "bodyexclude"_a.noconvert() = nb::none(), "ids"_a.noconvert() = nb::none(),
141
+ "mj_ray for every ray of every simulation. pnt and vec are (num_sims, nray, 3) "
142
+ "origins and directions in the world frame, float32 or the native dtype. dist "
143
+ "(num_sims, nray), and the optional geomid (int32) and normal (num_sims, nray, 3), "
144
+ "are caller-allocated and filled in place: dist is -1 and normal zero where a ray "
145
+ "hits nothing. geomgroup is mj_ray's six-entry uint8 mask, bodyexclude one int32 "
146
+ "body id per ray (-1 for none), and ids restricts which rows are computed.")
147
+ .def("jac", &Batch::jac, "jacp"_a.noconvert(), "jacr"_a.noconvert(), "point"_a.noconvert(),
148
+ "body"_a.noconvert(), "ids"_a.noconvert() = nb::none(),
149
+ "mj_jac for one point per simulation. point is (num_sims, 3) in the world frame, "
150
+ "float32 or the native dtype, and body (num_sims,) int32 is the body it moves with. "
151
+ "jacp and jacr are caller-allocated (num_sims, 3, nv) arrays filled in place with "
152
+ "the translational and rotational Jacobians; either may be None. ids restricts "
153
+ "which rows are computed.");
134
154
  }
@@ -0,0 +1,170 @@
1
+ // SPDX-License-Identifier: Apache-2.0
2
+
3
+ // Persistent thread pool: a blocking parallel-for with sticky slices.
4
+ #pragma once
5
+
6
+ #include <atomic>
7
+ #include <chrono>
8
+ #include <condition_variable>
9
+ #include <cstdint>
10
+ #include <functional>
11
+ #include <memory>
12
+ #include <mutex>
13
+ #include <thread>
14
+ #include <utility>
15
+ #include <vector>
16
+
17
+ #ifdef __linux__
18
+ #include <pthread.h>
19
+ #include <sched.h>
20
+ #else
21
+ #include <cerrno>
22
+ #endif
23
+
24
+ // Pins the calling thread to cpu. Returns 0 on success, an errno-style code
25
+ // otherwise; only Linux supports pinning.
26
+ inline int PinThisThreadToCpu(int cpu) {
27
+ #ifdef __linux__
28
+ cpu_set_t set;
29
+ CPU_ZERO(&set);
30
+ CPU_SET(cpu, &set);
31
+ return pthread_setaffinity_np(pthread_self(), sizeof(set), &set);
32
+ #else
33
+ (void)cpu;
34
+ return ENOTSUP;
35
+ #endif
36
+ }
37
+
38
+ // Runs fn(worker, i) for i in [0, n) and blocks until done. Item i belongs to
39
+ // the slice of worker i * T / n, which keeps a sim on the same core across
40
+ // calls; a worker that finishes its slice claims from the others, so no worker
41
+ // waits on the slowest. The calling thread is worker 0. Only one Run may be
42
+ // active at a time; fn must not throw.
43
+ //
44
+ // Between runs a worker spins briefly before it parks, so back-to-back calls
45
+ // skip the wake-up; the caller does the same while it waits for the others.
46
+ //
47
+ // When cpu_ids is non-empty it must have exactly nthreads entries and worker i
48
+ // pins itself to cpu_ids[i] before entering the work loop (Linux only). The
49
+ // constructor then blocks until every worker has reported, so PinError is
50
+ // final once it returns.
51
+ class ThreadPool {
52
+ public:
53
+ explicit ThreadPool(int nthreads, std::vector<int> cpu_ids = {})
54
+ : nthreads_(nthreads), next_(new Counter[nthreads]), cpu_ids_(std::move(cpu_ids)) {
55
+ for (int t = 1; t < nthreads; ++t) {
56
+ threads_.emplace_back([this, t] { Worker(t); });
57
+ }
58
+ if (!cpu_ids_.empty()) {
59
+ while (started_.load(std::memory_order_acquire) < nthreads_ - 1) {
60
+ std::this_thread::yield();
61
+ }
62
+ }
63
+ }
64
+
65
+ ~ThreadPool() {
66
+ stop_.store(true);
67
+ epoch_.fetch_add(1);
68
+ { std::lock_guard<std::mutex> lock(mu_); }
69
+ wake_.notify_all();
70
+ for (auto& t : threads_) t.join();
71
+ }
72
+
73
+ int size() const { return nthreads_; }
74
+
75
+ // Zero when every requested pin succeeded, otherwise the error code from the
76
+ // first failing pin attempt (ENOTSUP off Linux).
77
+ int PinError() const { return pin_error_.load(std::memory_order_relaxed); }
78
+
79
+ void Run(int n, const std::function<void(int, int)>& fn) {
80
+ fn_ = &fn;
81
+ n_ = n;
82
+ for (int t = 0; t < nthreads_; ++t) next_[t].value.store(Start(t), std::memory_order_relaxed);
83
+ pending_.store(nthreads_ - 1, std::memory_order_relaxed);
84
+ epoch_.fetch_add(1);
85
+ if (sleeping_.load() > 0) {
86
+ {
87
+ std::lock_guard<std::mutex> lock(mu_);
88
+ }
89
+ wake_.notify_all();
90
+ }
91
+ Work(0);
92
+ const auto deadline = Clock::now() + kSpin;
93
+ while (pending_.load(std::memory_order_acquire) != 0) {
94
+ if (Clock::now() < deadline) continue;
95
+ std::unique_lock<std::mutex> lock(mu_);
96
+ done_.wait(lock, [this] { return pending_.load() == 0; });
97
+ }
98
+ }
99
+
100
+ private:
101
+ using Clock = std::chrono::steady_clock;
102
+ // Long enough to bridge back-to-back calls, short enough to cost nothing otherwise.
103
+ static constexpr std::chrono::microseconds kSpin{50};
104
+ // One cache line each: every worker advances its own counter on every item.
105
+ struct alignas(64) Counter {
106
+ std::atomic<int> value;
107
+ };
108
+
109
+ int Start(int t) const { return static_cast<int>(static_cast<int64_t>(t) * n_ / nthreads_); }
110
+
111
+ void Work(int worker) {
112
+ for (int k = 0; k < nthreads_; ++k) {
113
+ const int t = (worker + k) % nthreads_;
114
+ const int end = Start(t + 1);
115
+ std::atomic<int>& next = next_[t].value;
116
+ for (int i = next.fetch_add(1, std::memory_order_relaxed); i < end;
117
+ i = next.fetch_add(1, std::memory_order_relaxed)) {
118
+ (*fn_)(worker, i);
119
+ }
120
+ }
121
+ }
122
+
123
+ void Worker(int worker) {
124
+ if (!cpu_ids_.empty()) {
125
+ int err = PinThisThreadToCpu(cpu_ids_[worker]);
126
+ if (err != 0) {
127
+ int expected = 0;
128
+ pin_error_.compare_exchange_strong(expected, err, std::memory_order_relaxed);
129
+ }
130
+ started_.fetch_add(1, std::memory_order_release);
131
+ }
132
+ uint64_t seen = 0;
133
+ while (true) {
134
+ const auto deadline = Clock::now() + kSpin;
135
+ while (epoch_.load(std::memory_order_acquire) == seen && !stop_.load()) {
136
+ if (Clock::now() < deadline) continue;
137
+ std::unique_lock<std::mutex> lock(mu_);
138
+ sleeping_.fetch_add(1);
139
+ wake_.wait(lock, [this, seen] { return epoch_.load() != seen || stop_.load(); });
140
+ sleeping_.fetch_sub(1);
141
+ }
142
+ if (stop_.load()) return;
143
+ seen = epoch_.load(std::memory_order_acquire);
144
+ Work(worker);
145
+ if (pending_.fetch_sub(1) == 1) {
146
+ {
147
+ std::lock_guard<std::mutex> lock(mu_);
148
+ }
149
+ done_.notify_one();
150
+ }
151
+ }
152
+ }
153
+
154
+ const int nthreads_;
155
+ std::unique_ptr<Counter[]> next_;
156
+ std::vector<std::thread> threads_;
157
+ std::mutex mu_;
158
+ std::condition_variable wake_;
159
+ std::condition_variable done_;
160
+ const std::function<void(int, int)>* fn_ = nullptr;
161
+ int n_ = 0;
162
+ std::atomic<int> pending_{0}; // workers still in the current run
163
+ std::atomic<int> sleeping_{0}; // workers parked on wake_
164
+ std::atomic<uint64_t> epoch_{0};
165
+ std::atomic<bool> stop_{false};
166
+ // CPU affinity state (cold path only, final once the constructor returns).
167
+ std::vector<int> cpu_ids_;
168
+ std::atomic<int> started_{0};
169
+ std::atomic<int> pin_error_{0};
170
+ };
@@ -1,138 +0,0 @@
1
- // SPDX-License-Identifier: Apache-2.0
2
-
3
- // Persistent thread pool: a blocking parallel-for with sticky slices.
4
- #pragma once
5
-
6
- #include <atomic>
7
- #include <condition_variable>
8
- #include <cstdint>
9
- #include <functional>
10
- #include <memory>
11
- #include <mutex>
12
- #include <thread>
13
- #include <utility>
14
- #include <vector>
15
-
16
- #ifdef __linux__
17
- #include <pthread.h>
18
- #include <sched.h>
19
- #else
20
- #include <cerrno>
21
- #endif
22
-
23
- // Pins the calling thread to cpu. Returns 0 on success, an errno-style code
24
- // otherwise; only Linux supports pinning.
25
- inline int PinThisThreadToCpu(int cpu) {
26
- #ifdef __linux__
27
- cpu_set_t set;
28
- CPU_ZERO(&set);
29
- CPU_SET(cpu, &set);
30
- return pthread_setaffinity_np(pthread_self(), sizeof(set), &set);
31
- #else
32
- (void)cpu;
33
- return ENOTSUP;
34
- #endif
35
- }
36
-
37
- // Runs fn(worker, i) for i in [0, n) and blocks until done. Item i belongs to
38
- // the slice of worker i * T / n, which keeps a sim on the same core across
39
- // calls; a worker that finishes its slice claims from the others, so no worker
40
- // waits on the slowest. Only one Run may be active at a time; fn must not throw.
41
- //
42
- // When cpu_ids is non-empty it must have exactly nthreads entries and worker i
43
- // pins itself to cpu_ids[i] before entering the work loop (Linux only). The
44
- // constructor then blocks until every worker has reported, so PinError is
45
- // final once it returns.
46
- class ThreadPool {
47
- public:
48
- explicit ThreadPool(int nthreads, std::vector<int> cpu_ids = {})
49
- : nthreads_(nthreads), next_(new std::atomic<int>[nthreads]),
50
- cpu_ids_(std::move(cpu_ids)) {
51
- for (int t = 0; t < nthreads; ++t) {
52
- threads_.emplace_back([this, t] { Worker(t); });
53
- }
54
- // With pins requested, block until every worker applied its affinity so a
55
- // failure is final (and readable via PinError) before the owner proceeds.
56
- if (!cpu_ids_.empty()) {
57
- while (started_.load(std::memory_order_acquire) < nthreads_) {
58
- std::this_thread::yield();
59
- }
60
- }
61
- }
62
-
63
- ~ThreadPool() {
64
- {
65
- std::lock_guard<std::mutex> lock(mu_);
66
- stop_ = true;
67
- }
68
- wake_.notify_all();
69
- for (auto& t : threads_) t.join();
70
- }
71
-
72
- int size() const { return nthreads_; }
73
-
74
- // Zero when every requested pin succeeded, otherwise the error code from the
75
- // first failing pin attempt (ENOTSUP off Linux).
76
- int PinError() const { return pin_error_.load(std::memory_order_relaxed); }
77
-
78
- void Run(int n, std::function<void(int, int)> fn) {
79
- std::unique_lock<std::mutex> lock(mu_);
80
- fn_ = std::move(fn);
81
- n_ = n;
82
- for (int t = 0; t < nthreads_; ++t) next_[t].store(Start(t), std::memory_order_relaxed);
83
- active_ = nthreads_;
84
- ++epoch_;
85
- wake_.notify_all();
86
- done_.wait(lock, [this] { return active_ == 0; });
87
- fn_ = nullptr;
88
- }
89
-
90
- private:
91
- int Start(int t) const { return static_cast<int>(static_cast<int64_t>(t) * n_ / nthreads_); }
92
-
93
- void Worker(int worker) {
94
- if (!cpu_ids_.empty()) {
95
- int err = PinThisThreadToCpu(cpu_ids_[worker]);
96
- if (err != 0) {
97
- int expected = 0;
98
- pin_error_.compare_exchange_strong(expected, err, std::memory_order_relaxed);
99
- }
100
- started_.fetch_add(1, std::memory_order_release);
101
- }
102
- uint64_t seen = 0;
103
- std::unique_lock<std::mutex> lock(mu_);
104
- while (true) {
105
- wake_.wait(lock, [this, &seen] { return stop_ || epoch_ != seen; });
106
- if (stop_) return;
107
- seen = epoch_;
108
- const std::function<void(int, int)>* fn = &fn_;
109
- lock.unlock();
110
- for (int k = 0; k < nthreads_; ++k) {
111
- const int t = (worker + k) % nthreads_;
112
- const int end = Start(t + 1);
113
- for (int i = next_[t].fetch_add(1, std::memory_order_relaxed); i < end;
114
- i = next_[t].fetch_add(1, std::memory_order_relaxed)) {
115
- (*fn)(worker, i);
116
- }
117
- }
118
- lock.lock();
119
- if (--active_ == 0) done_.notify_one();
120
- }
121
- }
122
-
123
- const int nthreads_;
124
- std::unique_ptr<std::atomic<int>[]> next_;
125
- std::vector<std::thread> threads_;
126
- std::mutex mu_;
127
- std::condition_variable wake_;
128
- std::condition_variable done_;
129
- std::function<void(int, int)> fn_;
130
- int n_ = 0;
131
- int active_ = 0;
132
- uint64_t epoch_ = 0;
133
- bool stop_ = false;
134
- // CPU affinity state (cold path only, final once the constructor returns).
135
- std::vector<int> cpu_ids_;
136
- std::atomic<int> started_{0};
137
- std::atomic<int> pin_error_{0};
138
- };
File without changes