mjbatch-uni 0.2.0__tar.gz → 0.2.2__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.
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/PKG-INFO +33 -1
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/README.md +32 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/pyproject.toml +1 -1
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/_bindings.pyi +12 -1
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/csrc/batch.h +184 -8
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/csrc/bindings.cpp +22 -1
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/CMakeLists.txt +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/LICENSE +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/__init__.py +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/csrc/threadpool.h +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/groups.py +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/model.py +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/py.typed +0 -0
- {mjbatch_uni-0.2.0 → mjbatch_uni-0.2.2}/src/mjbatch/variants.py +0 -0
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: mjbatch-uni
|
|
3
|
-
Version: 0.2.
|
|
3
|
+
Version: 0.2.2
|
|
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>
|
|
@@ -43,6 +43,7 @@ Features include:
|
|
|
43
43
|
* Explicit topology-affine groups, with `ModelAffineBatch` routing global ids to incompatible layouts.
|
|
44
44
|
* 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
45
|
* Per-substep control from Python with `step(..., callback=...)`.
|
|
46
|
+
* Opt-in split substeps with fresh position/velocity sensor views for body wrench control.
|
|
46
47
|
* Optional per-worker CPU pinning on Linux, with `cpu_ids` binding pool worker `i` to `cpu_ids[i]`.
|
|
47
48
|
|
|
48
49
|
For example:
|
|
@@ -60,6 +61,37 @@ for _ in range(1000):
|
|
|
60
61
|
batch.step() # step them in parallel; qpos updates in place
|
|
61
62
|
```
|
|
62
63
|
|
|
64
|
+
Callbacks can also consume tracked sensors at the exact state they are controlling. With
|
|
65
|
+
Euler, `substep_sensor_copyout=(start, stop)` splits each substep at `mj_step1` /
|
|
66
|
+
`mj_step2`, copies the declared sensor columns into a fourth callback view, and applies
|
|
67
|
+
`ctrl` plus bound inputs such as `xfrc_applied` to that substep:
|
|
68
|
+
|
|
69
|
+
```python
|
|
70
|
+
xfrc = batch.bind("xfrc_applied")
|
|
71
|
+
|
|
72
|
+
def control(k, state, ctrl, sensor):
|
|
73
|
+
ctrl[:] = impedance(sensor) # position/velocity sensors match state
|
|
74
|
+
xfrc[:, body_id] = wrench(sensor)
|
|
75
|
+
|
|
76
|
+
batch.step(callback=control, substep_sensor_copyout=(sensor_start, sensor_stop))
|
|
77
|
+
```
|
|
78
|
+
|
|
79
|
+
Acceleration-stage sensors and contact forces are not available before `mj_step2` and may
|
|
80
|
+
be stale in that view. The default callback path and all outputs are unchanged unless the
|
|
81
|
+
optional range is passed.
|
|
82
|
+
|
|
83
|
+
After a normal `step()` with `forward=False`, selected position- and velocity-stage sensor
|
|
84
|
+
columns can be refreshed from the final integration state without a full `mj_forward`:
|
|
85
|
+
|
|
86
|
+
```python
|
|
87
|
+
batch.refresh_sensor_range(ids, (sensor_start, sensor_stop))
|
|
88
|
+
```
|
|
89
|
+
|
|
90
|
+
The refresh runs in native worker threads, copies only the requested columns into the bound
|
|
91
|
+
`sensordata` view, and leaves acceleration-stage sensors (including contact forces) and all
|
|
92
|
+
other bound fields unchanged. Disjoint ranges can be refreshed in one dispatch with
|
|
93
|
+
`refresh_sensor_ranges(ids, (start0, stop0, start1, stop1, ...))`.
|
|
94
|
+
|
|
63
95
|
## Model randomization and variants
|
|
64
96
|
|
|
65
97
|
`expand(field)` returns a live `(num_sims, ...)` view of a non-asset `MjModel` field.
|
|
@@ -14,6 +14,7 @@ Features include:
|
|
|
14
14
|
* Explicit topology-affine groups, with `ModelAffineBatch` routing global ids to incompatible layouts.
|
|
15
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.
|
|
16
16
|
* Per-substep control from Python with `step(..., callback=...)`.
|
|
17
|
+
* Opt-in split substeps with fresh position/velocity sensor views for body wrench control.
|
|
17
18
|
* Optional per-worker CPU pinning on Linux, with `cpu_ids` binding pool worker `i` to `cpu_ids[i]`.
|
|
18
19
|
|
|
19
20
|
For example:
|
|
@@ -31,6 +32,37 @@ for _ in range(1000):
|
|
|
31
32
|
batch.step() # step them in parallel; qpos updates in place
|
|
32
33
|
```
|
|
33
34
|
|
|
35
|
+
Callbacks can also consume tracked sensors at the exact state they are controlling. With
|
|
36
|
+
Euler, `substep_sensor_copyout=(start, stop)` splits each substep at `mj_step1` /
|
|
37
|
+
`mj_step2`, copies the declared sensor columns into a fourth callback view, and applies
|
|
38
|
+
`ctrl` plus bound inputs such as `xfrc_applied` to that substep:
|
|
39
|
+
|
|
40
|
+
```python
|
|
41
|
+
xfrc = batch.bind("xfrc_applied")
|
|
42
|
+
|
|
43
|
+
def control(k, state, ctrl, sensor):
|
|
44
|
+
ctrl[:] = impedance(sensor) # position/velocity sensors match state
|
|
45
|
+
xfrc[:, body_id] = wrench(sensor)
|
|
46
|
+
|
|
47
|
+
batch.step(callback=control, substep_sensor_copyout=(sensor_start, sensor_stop))
|
|
48
|
+
```
|
|
49
|
+
|
|
50
|
+
Acceleration-stage sensors and contact forces are not available before `mj_step2` and may
|
|
51
|
+
be stale in that view. The default callback path and all outputs are unchanged unless the
|
|
52
|
+
optional range is passed.
|
|
53
|
+
|
|
54
|
+
After a normal `step()` with `forward=False`, selected position- and velocity-stage sensor
|
|
55
|
+
columns can be refreshed from the final integration state without a full `mj_forward`:
|
|
56
|
+
|
|
57
|
+
```python
|
|
58
|
+
batch.refresh_sensor_range(ids, (sensor_start, sensor_stop))
|
|
59
|
+
```
|
|
60
|
+
|
|
61
|
+
The refresh runs in native worker threads, copies only the requested columns into the bound
|
|
62
|
+
`sensordata` view, and leaves acceleration-stage sensors (including contact forces) and all
|
|
63
|
+
other bound fields unchanged. Disjoint ranges can be refreshed in one dispatch with
|
|
64
|
+
`refresh_sensor_ranges(ids, (start0, stop0, start1, stop1, ...))`.
|
|
65
|
+
|
|
34
66
|
## Model randomization and variants
|
|
35
67
|
|
|
36
68
|
`expand(field)` returns a live `(num_sims, ...)` view of a non-asset `MjModel` field.
|
|
@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
|
|
|
4
4
|
|
|
5
5
|
[project]
|
|
6
6
|
name = "mjbatch-uni"
|
|
7
|
-
version = "0.2.
|
|
7
|
+
version = "0.2.2"
|
|
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"
|
|
@@ -71,13 +71,24 @@ class Batch:
|
|
|
71
71
|
|
|
72
72
|
def expand(self, name: str, dtype: object | None = None) -> NDArray: ...
|
|
73
73
|
|
|
74
|
-
def step(self, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None, nstep: int = 1, history: NDArray | None = None, *, callback: Callable | None = None) -> None:
|
|
74
|
+
def step(self, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None, nstep: int = 1, history: NDArray | None = None, *, callback: Callable | None = None, substep_sensor_copyout: Sequence[int] | None = None) -> None:
|
|
75
75
|
"""
|
|
76
76
|
nstep mj_step calls per simulation, on one worker. ids: sorted unique ints or a bool mask. history: an optional caller-allocated (sims, nstep, nstate) array filled with each selected simulation's state after every substep, in bind("state") order; the rows of a simulation that raises are undefined.
|
|
77
77
|
callback: fn(k, state, ctrl) invoked on the calling thread before substep k. k=0 gets the state from before the call; a later k gets the state after substep k-1. The views are live (num_sims, ...) batch rows built once per call: writes to ctrl apply to the substep that follows, writes to state at the next substep, like a state write between calls. Substeps are dispatched one at a time, so bound fields other than state stay stale until the call ends. Batch calls from inside the callback raise; an exception stops the simulations at the last completed substep, recoverably.
|
|
78
|
+
substep_sensor_copyout=(start, stop) opts into an Euler-only mj_step1/mj_step2 split. Before each callback, mj_step1 runs and the half-open sensordata column range is copied into a fourth live callback argument, sensor. Position- and velocity-stage sensors in that range are current with state; acceleration-stage values (including contact forces) are not available at this split point and may be stale. Writes to ctrl and bound input fields occur between step1 and step2, so they affect the current substep. If the callback raises, state remains at the last completed substep; intermediate mjData is recovered on the next call.
|
|
78
79
|
"""
|
|
79
80
|
|
|
80
81
|
def forward(self, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None) -> None: ...
|
|
82
|
+
def refresh_sensor_range(
|
|
83
|
+
self,
|
|
84
|
+
ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = ...,
|
|
85
|
+
sensor_range: Sequence[int] = ...,
|
|
86
|
+
) -> None: ...
|
|
87
|
+
def refresh_sensor_ranges(
|
|
88
|
+
self,
|
|
89
|
+
ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = ...,
|
|
90
|
+
sensor_ranges: Sequence[int] = ...,
|
|
91
|
+
) -> None: ...
|
|
81
92
|
|
|
82
93
|
def reset(self, ids: Annotated[NDArray, dict(shape=(None,), order='C')] | None = None, keyframe: int = -1) -> None:
|
|
83
94
|
"""
|
|
@@ -318,6 +318,8 @@ struct QueryCtx {
|
|
|
318
318
|
int npoint = 0;
|
|
319
319
|
HfieldAlign align = HfieldAlign::World; // SampleHfield: sampling grid rotation
|
|
320
320
|
mjtNum* out = nullptr; // (sel, npoint) rows
|
|
321
|
+
Slot* sensor = nullptr; // RefreshSensor: bound sensordata slot
|
|
322
|
+
const std::vector<int>* sensor_ranges = nullptr; // flattened (start, stop) pairs
|
|
321
323
|
};
|
|
322
324
|
|
|
323
325
|
// One substep of a callback-driven step (Op::Substep): the calling thread
|
|
@@ -328,6 +330,9 @@ struct CallbackCtx {
|
|
|
328
330
|
mjtNum* hist = nullptr; // (sel, nstep, nstate) row for this sim
|
|
329
331
|
uint8_t* done = nullptr; // set when this selection row runs its copy-out tail
|
|
330
332
|
bool tail_only = false; // exception cleanup: no mj_step, copy out where it stopped
|
|
333
|
+
Slot* sensor = nullptr; // Substep1: bound sensordata slot receiving selected columns
|
|
334
|
+
int sensor_start = 0;
|
|
335
|
+
int sensor_width = 0;
|
|
331
336
|
};
|
|
332
337
|
|
|
333
338
|
// Bilinear sampling of one hfield geom at XY offsets from a frame body's origin.
|
|
@@ -406,7 +411,18 @@ inline thread_local const Batch* tls_callback = nullptr;
|
|
|
406
411
|
|
|
407
412
|
class Batch {
|
|
408
413
|
public:
|
|
409
|
-
enum class Op {
|
|
414
|
+
enum class Op {
|
|
415
|
+
Step,
|
|
416
|
+
Substep, // callback-step recovery tail
|
|
417
|
+
Substep1, // split callback: mj_step1 and selected sensor copy-out
|
|
418
|
+
Substep2, // split callback: apply writes and mj_step2
|
|
419
|
+
Forward,
|
|
420
|
+
Reset,
|
|
421
|
+
RefreshSensor,
|
|
422
|
+
SetConst,
|
|
423
|
+
JacSite,
|
|
424
|
+
SampleHfield
|
|
425
|
+
};
|
|
410
426
|
using Ids = nb::ndarray<nb::ndim<1>, nb::c_contig>;
|
|
411
427
|
|
|
412
428
|
Batch(nb::object model, int num_sims, int num_threads, bool forward,
|
|
@@ -474,16 +490,32 @@ class Batch {
|
|
|
474
490
|
}
|
|
475
491
|
|
|
476
492
|
void step(std::optional<Ids> ids, int nstep, std::optional<nb::ndarray<>> history,
|
|
477
|
-
std::optional<nb::callable> callback
|
|
493
|
+
std::optional<nb::callable> callback,
|
|
494
|
+
std::optional<std::vector<int>> substep_sensor_copyout) {
|
|
478
495
|
ReentryGuard();
|
|
479
496
|
if (nstep < 1) throw nb::value_error("nstep must be >= 1");
|
|
497
|
+
int sensor_start = 0, sensor_stop = 0;
|
|
498
|
+
if (substep_sensor_copyout) {
|
|
499
|
+
if (substep_sensor_copyout->size() != 2) {
|
|
500
|
+
throw nb::value_error("substep_sensor_copyout must have shape (2,)");
|
|
501
|
+
}
|
|
502
|
+
sensor_start = (*substep_sensor_copyout)[0];
|
|
503
|
+
sensor_stop = (*substep_sensor_copyout)[1];
|
|
504
|
+
if (sensor_start < 0 || sensor_start >= sensor_stop ||
|
|
505
|
+
sensor_stop > template_->nsensordata) {
|
|
506
|
+
throw nb::value_error(
|
|
507
|
+
"substep_sensor_copyout entries must satisfy 0 <= start < stop <= nsensordata");
|
|
508
|
+
}
|
|
509
|
+
if (!callback) throw nb::value_error("substep_sensor_copyout requires callback");
|
|
510
|
+
}
|
|
480
511
|
auto sel = Parse(ids);
|
|
481
512
|
mjtNum* hist = nullptr;
|
|
482
513
|
if (history) {
|
|
483
514
|
hist = HistoryPtr(*history, sel ? static_cast<int>(sel->size()) : num_sims_, nstep);
|
|
484
515
|
}
|
|
485
516
|
if (callback) {
|
|
486
|
-
RunCallback(std::move(sel), nstep, hist, *callback
|
|
517
|
+
RunCallback(std::move(sel), nstep, hist, *callback, sensor_start,
|
|
518
|
+
sensor_stop - sensor_start);
|
|
487
519
|
} else {
|
|
488
520
|
Run(Op::Step, std::move(sel), nstep, hist);
|
|
489
521
|
}
|
|
@@ -493,6 +525,41 @@ class Batch {
|
|
|
493
525
|
Run(Op::Forward, Parse(ids), 0);
|
|
494
526
|
}
|
|
495
527
|
|
|
528
|
+
void RefreshSensorRanges(std::optional<Ids> ids, const std::vector<int>& sensor_ranges) {
|
|
529
|
+
ReentryGuard();
|
|
530
|
+
for (size_t i = 0; i < sensor_ranges.size(); i += 2) {
|
|
531
|
+
int start = sensor_ranges[i], stop = sensor_ranges[i + 1];
|
|
532
|
+
if (start < 0 || start >= stop || stop > template_->nsensordata) {
|
|
533
|
+
throw nb::value_error("sensor range entries must satisfy 0 <= start < stop <= nsensordata");
|
|
534
|
+
}
|
|
535
|
+
}
|
|
536
|
+
auto sel = Parse(ids);
|
|
537
|
+
Slot* sensor;
|
|
538
|
+
{
|
|
539
|
+
std::lock_guard<std::mutex> lock(mu_);
|
|
540
|
+
sensor = &BoundOrAdd(Field(data_fields_, "sensordata"), false);
|
|
541
|
+
}
|
|
542
|
+
QueryCtx ctx;
|
|
543
|
+
ctx.sensor = sensor;
|
|
544
|
+
ctx.sensor_ranges = &sensor_ranges;
|
|
545
|
+
Run(Op::RefreshSensor, std::move(sel), 0, nullptr, &ctx);
|
|
546
|
+
}
|
|
547
|
+
|
|
548
|
+
void refresh_sensor_range(std::optional<Ids> ids, std::optional<std::vector<int>> sensor_range) {
|
|
549
|
+
if (!sensor_range || sensor_range->size() != 2) {
|
|
550
|
+
throw nb::value_error("sensor_range must have shape (2,)");
|
|
551
|
+
}
|
|
552
|
+
RefreshSensorRanges(std::move(ids), *sensor_range);
|
|
553
|
+
}
|
|
554
|
+
|
|
555
|
+
void refresh_sensor_ranges(std::optional<Ids> ids,
|
|
556
|
+
std::optional<std::vector<int>> sensor_ranges) {
|
|
557
|
+
if (!sensor_ranges || sensor_ranges->empty() || sensor_ranges->size() % 2 != 0) {
|
|
558
|
+
throw nb::value_error("sensor_ranges must contain (start, stop) pairs");
|
|
559
|
+
}
|
|
560
|
+
RefreshSensorRanges(std::move(ids), *sensor_ranges);
|
|
561
|
+
}
|
|
562
|
+
|
|
496
563
|
void reset(std::optional<Ids> ids, int keyframe) {
|
|
497
564
|
ReentryGuard();
|
|
498
565
|
if (keyframe < -1 || keyframe >= template_->nkey) {
|
|
@@ -743,6 +810,20 @@ class Batch {
|
|
|
743
810
|
if (s.mirror) std::memcpy(s.mirror.get() + i * s.row, row, s.row);
|
|
744
811
|
}
|
|
745
812
|
|
|
813
|
+
// Copy only the callback's declared sensordata columns into its bound view.
|
|
814
|
+
// Unlike CopyOut, this does not touch a mirror or columns outside the range.
|
|
815
|
+
void CopySensorRange(Slot& s, mjData* d, int i, int start, int width) {
|
|
816
|
+
uint8_t* row = s.buf.get() + i * s.row;
|
|
817
|
+
const mjtNum* src = d->sensordata + start;
|
|
818
|
+
if (s.f32 && sizeof(mjtNum) != sizeof(float)) {
|
|
819
|
+
auto* dst = reinterpret_cast<float*>(row) + start;
|
|
820
|
+
for (int k = 0; k < width; ++k) dst[k] = static_cast<float>(src[k]);
|
|
821
|
+
} else {
|
|
822
|
+
std::memcpy(row + static_cast<size_t>(start) * sizeof(mjtNum), src,
|
|
823
|
+
static_cast<size_t>(width) * sizeof(mjtNum));
|
|
824
|
+
}
|
|
825
|
+
}
|
|
826
|
+
|
|
746
827
|
void Restore(mjModel* m) {
|
|
747
828
|
for (const FieldInfo* f : restorable_) std::memcpy(f->get(m), f->get(template_), f->bytes());
|
|
748
829
|
m->npolygonmax = template_->npolygonmax;
|
|
@@ -830,10 +911,52 @@ class Batch {
|
|
|
830
911
|
for (auto& s : bound_) CopyOut(*s, d, i);
|
|
831
912
|
return;
|
|
832
913
|
}
|
|
914
|
+
case Op::Substep1: {
|
|
915
|
+
// Position- and velocity-stage sensors are current with State(i) here.
|
|
916
|
+
// Input mirrors stay stale because Substep2 applies the callback writes.
|
|
917
|
+
mj_step1(m, d);
|
|
918
|
+
CopySensorRange(*cctx->sensor, d, i, cctx->sensor_start, cctx->sensor_width);
|
|
919
|
+
return;
|
|
920
|
+
}
|
|
921
|
+
case Op::Substep2: {
|
|
922
|
+
// A worker's mjData holds the last phase-one sim it visited, not a
|
|
923
|
+
// per-sim snapshot. Stage one is recomputed after State(i) and the
|
|
924
|
+
// callback writes have been loaded; the sensor view copied above is
|
|
925
|
+
// still the fresh result at x_k. Sync mirrors only after mj_step2.
|
|
926
|
+
mj_step1(m, d);
|
|
927
|
+
for (auto& s : bound_) {
|
|
928
|
+
if (s->mirror) {
|
|
929
|
+
std::memcpy(s->mirror.get() + i * s->row, s->buf.get() + i * s->row, s->row);
|
|
930
|
+
}
|
|
931
|
+
}
|
|
932
|
+
mj_step2(m, d);
|
|
933
|
+
if (cctx->hist) {
|
|
934
|
+
mj_getState(m, d, cctx->hist + static_cast<size_t>(cctx->k) * nstate_,
|
|
935
|
+
mjSTATE_INTEGRATION);
|
|
936
|
+
}
|
|
937
|
+
mj_getState(m, d, State(i), mjSTATE_INTEGRATION);
|
|
938
|
+
std::memcpy(Warning(i), d->warning, sizeof(d->warning));
|
|
939
|
+
if (cctx->k + 1 != cctx->nstep) return;
|
|
940
|
+
cctx->done[0] = 1;
|
|
941
|
+
if (forward_) mj_forward(m, d);
|
|
942
|
+
for (auto& s : bound_) CopyOut(*s, d, i);
|
|
943
|
+
return;
|
|
944
|
+
}
|
|
833
945
|
case Op::Forward:
|
|
834
946
|
case Op::Reset:
|
|
835
947
|
mj_forward(m, d);
|
|
836
948
|
break;
|
|
949
|
+
case Op::RefreshSensor:
|
|
950
|
+
mj_kinematics(m, d);
|
|
951
|
+
mj_comPos(m, d);
|
|
952
|
+
mj_comVel(m, d);
|
|
953
|
+
mj_sensorPos(m, d);
|
|
954
|
+
mj_sensorVel(m, d);
|
|
955
|
+
for (int j = 0; j < static_cast<int>(ctx->sensor_ranges->size()); j += 2) {
|
|
956
|
+
CopySensorRange(*ctx->sensor, d, i, (*ctx->sensor_ranges)[j],
|
|
957
|
+
(*ctx->sensor_ranges)[j + 1] - (*ctx->sensor_ranges)[j]);
|
|
958
|
+
}
|
|
959
|
+
return;
|
|
837
960
|
// The query ops run kinematics only: they do not change the integration
|
|
838
961
|
// state, so the copy-out tail below is skipped. mjData is shared by every
|
|
839
962
|
// sim a worker serves, and copying out would leak another sim's stale
|
|
@@ -924,6 +1047,30 @@ class Batch {
|
|
|
924
1047
|
}
|
|
925
1048
|
}
|
|
926
1049
|
|
|
1050
|
+
// The step1/step2 split is Euler-only. Expanded integrators are checked per
|
|
1051
|
+
// selected simulation so a mixed batch fails closed before touching state.
|
|
1052
|
+
void CheckEuler(const int* p, int n) {
|
|
1053
|
+
const Slot* integrator = nullptr;
|
|
1054
|
+
for (auto& s : expanded_) {
|
|
1055
|
+
if (std::string_view(s->info->name) == "integrator") {
|
|
1056
|
+
integrator = s.get();
|
|
1057
|
+
break;
|
|
1058
|
+
}
|
|
1059
|
+
}
|
|
1060
|
+
for (int j = 0; j < n; ++j) {
|
|
1061
|
+
int i = p ? p[j] : j;
|
|
1062
|
+
int value = template_->opt.integrator;
|
|
1063
|
+
if (integrator) {
|
|
1064
|
+
std::memcpy(&value, integrator->buf.get() + i * integrator->row, sizeof(value));
|
|
1065
|
+
}
|
|
1066
|
+
if (value != mjINT_EULER) {
|
|
1067
|
+
throw nb::value_error(("sim " + std::to_string(i) +
|
|
1068
|
+
": substep_sensor_copyout requires the Euler integrator")
|
|
1069
|
+
.c_str());
|
|
1070
|
+
}
|
|
1071
|
+
}
|
|
1072
|
+
}
|
|
1073
|
+
|
|
927
1074
|
void RunLocked(Op op, const std::optional<std::vector<int>>& sel, int arg,
|
|
928
1075
|
mjtNum* hist = nullptr, const QueryCtx* ctx = nullptr) {
|
|
929
1076
|
const int* p = sel ? sel->data() : nullptr;
|
|
@@ -951,17 +1098,21 @@ class Batch {
|
|
|
951
1098
|
|
|
952
1099
|
// One substep for every selected sim that has not run its copy-out tail yet.
|
|
953
1100
|
void SubstepLocked(const int* p, int n, int k, int nstep, mjtNum* hist, uint8_t* done,
|
|
954
|
-
bool tail_only
|
|
1101
|
+
bool tail_only, Op op = Op::Substep, Slot* sensor = nullptr,
|
|
1102
|
+
int sensor_start = 0, int sensor_width = 0) {
|
|
955
1103
|
CallbackCtx base;
|
|
956
1104
|
base.k = k;
|
|
957
1105
|
base.nstep = nstep;
|
|
958
1106
|
base.tail_only = tail_only;
|
|
1107
|
+
base.sensor = sensor;
|
|
1108
|
+
base.sensor_start = sensor_start;
|
|
1109
|
+
base.sensor_width = sensor_width;
|
|
959
1110
|
auto fn = [&](int t, int j) {
|
|
960
1111
|
if (done[j]) return;
|
|
961
1112
|
CallbackCtx row = base;
|
|
962
1113
|
if (hist) row.hist = hist + static_cast<size_t>(j) * nstep * nstate_;
|
|
963
1114
|
row.done = done + j;
|
|
964
|
-
Guarded(t, p ? p[j] : j,
|
|
1115
|
+
Guarded(t, p ? p[j] : j, op, k, nullptr, nullptr, &row);
|
|
965
1116
|
};
|
|
966
1117
|
if (pool_->size() == 1) {
|
|
967
1118
|
for (int j = 0; j < n; ++j) fn(0, j);
|
|
@@ -974,7 +1125,7 @@ class Batch {
|
|
|
974
1125
|
// callback before every substep, with the GIL held and every worker idle so
|
|
975
1126
|
// the views it passes are stable, then dispatches the substep to the pool.
|
|
976
1127
|
void RunCallback(std::optional<std::vector<int>> sel, int nstep, mjtNum* hist,
|
|
977
|
-
nb::callable callback) {
|
|
1128
|
+
nb::callable callback, int sensor_start, int sensor_width) {
|
|
978
1129
|
{
|
|
979
1130
|
// Lock mu_ without the GIL: the callback needs the GIL while mu_ is held,
|
|
980
1131
|
// so blocking on mu_ with it could deadlock two callback-driven steps.
|
|
@@ -983,21 +1134,44 @@ class Batch {
|
|
|
983
1134
|
}
|
|
984
1135
|
std::unique_lock<std::mutex> lock(mu_, std::adopt_lock);
|
|
985
1136
|
Slot& ctrl = BoundOrAdd(Field(data_fields_, "ctrl"), false);
|
|
1137
|
+
Slot* sensor = nullptr;
|
|
1138
|
+
nb::object sensor_view;
|
|
1139
|
+
if (sensor_width) sensor = &BoundOrAdd(Field(data_fields_, "sensordata"), false);
|
|
986
1140
|
// The views are built once per call, so the callback sees the same live
|
|
987
1141
|
// arrays every substep and the loop allocates nothing.
|
|
988
1142
|
nb::object state = nb::cast(StateView());
|
|
989
1143
|
nb::object ctrl_view = nb::cast(View(ctrl));
|
|
1144
|
+
if (sensor) {
|
|
1145
|
+
nb::object full = nb::cast(View(*sensor));
|
|
1146
|
+
sensor_view = full.attr("__getitem__")(
|
|
1147
|
+
nb::make_tuple(nb::slice(nb::none(), nb::none(), nb::none()),
|
|
1148
|
+
nb::slice(sensor_start, sensor_start + sensor_width)));
|
|
1149
|
+
}
|
|
990
1150
|
const int* p = sel ? sel->data() : nullptr;
|
|
991
1151
|
const int n = sel ? static_cast<int>(sel->size()) : num_sims_;
|
|
992
1152
|
CheckSleep(p, n);
|
|
1153
|
+
if (sensor_width) CheckEuler(p, n);
|
|
993
1154
|
error_.clear();
|
|
994
1155
|
std::vector<uint8_t> done(n, 0);
|
|
995
1156
|
int dispatched = 0, active = n;
|
|
996
1157
|
std::exception_ptr pending;
|
|
997
1158
|
for (int k = 0; k < nstep && active; ++k) {
|
|
1159
|
+
if (sensor_width) {
|
|
1160
|
+
{
|
|
1161
|
+
nb::gil_scoped_release release;
|
|
1162
|
+
SubstepLocked(p, n, k, nstep, hist, done.data(), false, Op::Substep1, sensor,
|
|
1163
|
+
sensor_start, sensor_width);
|
|
1164
|
+
}
|
|
1165
|
+
++dispatched;
|
|
1166
|
+
if (!error_.empty()) break;
|
|
1167
|
+
}
|
|
998
1168
|
tls_callback = this;
|
|
999
1169
|
try {
|
|
1000
|
-
|
|
1170
|
+
if (sensor_width) {
|
|
1171
|
+
callback(k, state, ctrl_view, sensor_view);
|
|
1172
|
+
} else {
|
|
1173
|
+
callback(k, state, ctrl_view);
|
|
1174
|
+
}
|
|
1001
1175
|
} catch (...) {
|
|
1002
1176
|
pending = std::current_exception();
|
|
1003
1177
|
}
|
|
@@ -1005,7 +1179,9 @@ class Batch {
|
|
|
1005
1179
|
if (pending) break;
|
|
1006
1180
|
{
|
|
1007
1181
|
nb::gil_scoped_release release;
|
|
1008
|
-
SubstepLocked(p, n, k, nstep, hist, done.data(), false
|
|
1182
|
+
SubstepLocked(p, n, k, nstep, hist, done.data(), false,
|
|
1183
|
+
sensor_width ? Op::Substep2 : Op::Substep, sensor, sensor_start,
|
|
1184
|
+
sensor_width);
|
|
1009
1185
|
}
|
|
1010
1186
|
++dispatched;
|
|
1011
1187
|
if (!error_.empty()) break;
|
|
@@ -77,6 +77,7 @@ handler installed at import; installing another handler later disables it.)")
|
|
|
77
77
|
.def("expand", &Batch::expand, "name"_a, "dtype"_a = nb::none())
|
|
78
78
|
.def("step", &Batch::step, "ids"_a.noconvert() = nb::none(), "nstep"_a = 1,
|
|
79
79
|
"history"_a.noconvert() = nb::none(), nb::kw_only(), "callback"_a = nb::none(),
|
|
80
|
+
"substep_sensor_copyout"_a = nb::none(),
|
|
80
81
|
"nstep mj_step calls per simulation, on one worker. ids: sorted unique ints or a "
|
|
81
82
|
"bool mask. history: an optional caller-allocated (sims, nstep, nstate) array "
|
|
82
83
|
"filled with each selected simulation's state after every substep, in "
|
|
@@ -88,8 +89,28 @@ handler installed at import; installing another handler later disables it.)")
|
|
|
88
89
|
"the next substep, like a state write between calls. Substeps are dispatched "
|
|
89
90
|
"one at a time, so bound fields other than state stay stale until the call "
|
|
90
91
|
"ends. Batch calls from inside the callback raise; an exception stops the "
|
|
91
|
-
"simulations at the last completed substep, recoverably
|
|
92
|
+
"simulations at the last completed substep, recoverably.\n"
|
|
93
|
+
"substep_sensor_copyout=(start, stop) opts into an Euler-only mj_step1/mj_step2 "
|
|
94
|
+
"split. Before each callback, mj_step1 runs and the half-open sensordata column "
|
|
95
|
+
"range is copied into a fourth live callback argument, sensor. Position- and "
|
|
96
|
+
"velocity-stage sensors in that range are current with state; acceleration-stage "
|
|
97
|
+
"values (including contact forces) are not available at this split point and may "
|
|
98
|
+
"be stale. Writes to ctrl and bound input fields occur between step1 and step2, "
|
|
99
|
+
"so they affect the current substep. If the callback raises, state remains at "
|
|
100
|
+
"the last completed substep; intermediate mjData is recovered on the next call.")
|
|
92
101
|
.def("forward", &Batch::forward, "ids"_a.noconvert() = nb::none())
|
|
102
|
+
.def("refresh_sensor_range", &Batch::refresh_sensor_range, "ids"_a.noconvert() = nb::none(),
|
|
103
|
+
"sensor_range"_a,
|
|
104
|
+
"Refresh position- and velocity-stage sensors in a half-open sensordata column "
|
|
105
|
+
"range from each selected simulation's final integration state. The work runs "
|
|
106
|
+
"in native worker threads and only the declared columns are copied into the "
|
|
107
|
+
"bound sensordata view; acceleration-stage sensors (including contact forces) "
|
|
108
|
+
"and every other bound field are preserved.")
|
|
109
|
+
.def("refresh_sensor_ranges", &Batch::refresh_sensor_ranges, "ids"_a.noconvert() = nb::none(),
|
|
110
|
+
"sensor_ranges"_a,
|
|
111
|
+
"Refresh disjoint position- and velocity-stage sensor column ranges, expressed "
|
|
112
|
+
"as a flattened sequence of half-open (start, stop) pairs. Semantics match "
|
|
113
|
+
"refresh_sensor_range(); columns outside every requested range are preserved.")
|
|
93
114
|
.def("reset", &Batch::reset, "ids"_a.noconvert() = nb::none(), "keyframe"_a = -1,
|
|
94
115
|
"mj_resetData, or mj_resetDataKeyframe when keyframe >= 0, then mj_forward.")
|
|
95
116
|
.def("jac_site", &Batch::jac_site, "site"_a, "jacp"_a.noconvert() = nb::none(),
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|