ZibinDong commited on
Commit
7c09a0c
·
verified ·
1 Parent(s): eae75a9

Publish audited ActionCodec2 release files

Browse files
LICENSE ADDED
@@ -0,0 +1,202 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+
2
+ Apache License
3
+ Version 2.0, January 2004
4
+ http://www.apache.org/licenses/
5
+
6
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
7
+
8
+ 1. Definitions.
9
+
10
+ "License" shall mean the terms and conditions for use, reproduction,
11
+ and distribution as defined by Sections 1 through 9 of this document.
12
+
13
+ "Licensor" shall mean the copyright owner or entity authorized by
14
+ the copyright owner that is granting the License.
15
+
16
+ "Legal Entity" shall mean the union of the acting entity and all
17
+ other entities that control, are controlled by, or are under common
18
+ control with that entity. For the purposes of this definition,
19
+ "control" means (i) the power, direct or indirect, to cause the
20
+ direction or management of such entity, whether by contract or
21
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
22
+ outstanding shares, or (iii) beneficial ownership of such entity.
23
+
24
+ "You" (or "Your") shall mean an individual or Legal Entity
25
+ exercising permissions granted by this License.
26
+
27
+ "Source" form shall mean the preferred form for making modifications,
28
+ including but not limited to software source code, documentation
29
+ source, and configuration files.
30
+
31
+ "Object" form shall mean any form resulting from mechanical
32
+ transformation or translation of a Source form, including but
33
+ not limited to compiled object code, generated documentation,
34
+ and conversions to other media types.
35
+
36
+ "Work" shall mean the work of authorship, whether in Source or
37
+ Object form, made available under the License, as indicated by a
38
+ copyright notice that is included in or attached to the work
39
+ (an example is provided in the Appendix below).
40
+
41
+ "Derivative Works" shall mean any work, whether in Source or Object
42
+ form, that is based on (or derived from) the Work and for which the
43
+ editorial revisions, annotations, elaborations, or other modifications
44
+ represent, as a whole, an original work of authorship. For the purposes
45
+ of this License, Derivative Works shall not include works that remain
46
+ separable from, or merely link (or bind by name) to the interfaces of,
47
+ the Work and Derivative Works thereof.
48
+
49
+ "Contribution" shall mean any work of authorship, including
50
+ the original version of the Work and any modifications or additions
51
+ to that Work or Derivative Works thereof, that is intentionally
52
+ submitted to Licensor for inclusion in the Work by the copyright owner
53
+ or by an individual or Legal Entity authorized to submit on behalf of
54
+ the copyright owner. For the purposes of this definition, "submitted"
55
+ means any form of electronic, verbal, or written communication sent
56
+ to the Licensor or its representatives, including but not limited to
57
+ communication on electronic mailing lists, source code control systems,
58
+ and issue tracking systems that are managed by, or on behalf of, the
59
+ Licensor for the purpose of discussing and improving the Work, but
60
+ excluding communication that is conspicuously marked or otherwise
61
+ designated in writing by the copyright owner as "Not a Contribution."
62
+
63
+ "Contributor" shall mean Licensor and any individual or Legal Entity
64
+ on behalf of whom a Contribution has been received by Licensor and
65
+ subsequently incorporated within the Work.
66
+
67
+ 2. Grant of Copyright License. Subject to the terms and conditions of
68
+ this License, each Contributor hereby grants to You a perpetual,
69
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
70
+ copyright license to reproduce, prepare Derivative Works of,
71
+ publicly display, publicly perform, sublicense, and distribute the
72
+ Work and such Derivative Works in Source or Object form.
73
+
74
+ 3. Grant of Patent License. Subject to the terms and conditions of
75
+ this License, each Contributor hereby grants to You a perpetual,
76
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
77
+ (except as stated in this section) patent license to make, have made,
78
+ use, offer to sell, sell, import, and otherwise transfer the Work,
79
+ where such license applies only to those patent claims licensable
80
+ by such Contributor that are necessarily infringed by their
81
+ Contribution(s) alone or by combination of their Contribution(s)
82
+ with the Work to which such Contribution(s) was submitted. If You
83
+ institute patent litigation against any entity (including a
84
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
85
+ or a Contribution incorporated within the Work constitutes direct
86
+ or contributory patent infringement, then any patent licenses
87
+ granted to You under this License for that Work shall terminate
88
+ as of the date such litigation is filed.
89
+
90
+ 4. Redistribution. You may reproduce and distribute copies of the
91
+ Work or Derivative Works thereof in any medium, with or without
92
+ modifications, and in Source or Object form, provided that You
93
+ meet the following conditions:
94
+
95
+ (a) You must give any other recipients of the Work or
96
+ Derivative Works a copy of this License; and
97
+
98
+ (b) You must cause any modified files to carry prominent notices
99
+ stating that You changed the files; and
100
+
101
+ (c) You must retain, in the Source form of any Derivative Works
102
+ that You distribute, all copyright, patent, trademark, and
103
+ attribution notices from the Source form of the Work,
104
+ excluding those notices that do not pertain to any part of
105
+ the Derivative Works; and
106
+
107
+ (d) If the Work includes a "NOTICE" text file as part of its
108
+ distribution, then any Derivative Works that You distribute must
109
+ include a readable copy of the attribution notices contained
110
+ within such NOTICE file, excluding those notices that do not
111
+ pertain to any part of the Derivative Works, in at least one
112
+ of the following places: within a NOTICE text file distributed
113
+ as part of the Derivative Works; within the Source form or
114
+ documentation, if provided along with the Derivative Works; or,
115
+ within a display generated by the Derivative Works, if and
116
+ wherever such third-party notices normally appear. The contents
117
+ of the NOTICE file are for informational purposes only and
118
+ do not modify the License. You may add Your own attribution
119
+ notices within Derivative Works that You distribute, alongside
120
+ or as an addendum to the NOTICE text from the Work, provided
121
+ that such additional attribution notices cannot be construed
122
+ as modifying the License.
123
+
124
+ You may add Your own copyright statement to Your modifications and
125
+ may provide additional or different license terms and conditions
126
+ for use, reproduction, or distribution of Your modifications, or
127
+ for any such Derivative Works as a whole, provided Your use,
128
+ reproduction, and distribution of the Work otherwise complies with
129
+ the conditions stated in this License.
130
+
131
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
132
+ any Contribution intentionally submitted for inclusion in the Work
133
+ by You to the Licensor shall be under the terms and conditions of
134
+ this License, without any additional terms or conditions.
135
+ Notwithstanding the above, nothing herein shall supersede or modify
136
+ the terms of any separate license agreement you may have executed
137
+ with Licensor regarding such Contributions.
138
+
139
+ 6. Trademarks. This License does not grant permission to use the trade
140
+ names, trademarks, service marks, or product names of the Licensor,
141
+ except as required for reasonable and customary use in describing the
142
+ origin of the Work and reproducing the content of the NOTICE file.
143
+
144
+ 7. Disclaimer of Warranty. Unless required by applicable law or
145
+ agreed to in writing, Licensor provides the Work (and each
146
+ Contributor provides its Contributions) on an "AS IS" BASIS,
147
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
148
+ implied, including, without limitation, any warranties or conditions
149
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
150
+ PARTICULAR PURPOSE. You are solely responsible for determining the
151
+ appropriateness of using or redistributing the Work and assume any
152
+ risks associated with Your exercise of permissions under this License.
153
+
154
+ 8. Limitation of Liability. In no event and under no legal theory,
155
+ whether in tort (including negligence), contract, or otherwise,
156
+ unless required by applicable law (such as deliberate and grossly
157
+ negligent acts) or agreed to in writing, shall any Contributor be
158
+ liable to You for damages, including any direct, indirect, special,
159
+ incidental, or consequential damages of any character arising as a
160
+ result of this License or out of the use or inability to use the
161
+ Work (including but not limited to damages for loss of goodwill,
162
+ work stoppage, computer failure or malfunction, or any and all
163
+ other commercial damages or losses), even if such Contributor
164
+ has been advised of the possibility of such damages.
165
+
166
+ 9. Accepting Warranty or Additional Liability. While redistributing
167
+ the Work or Derivative Works thereof, You may choose to offer,
168
+ and charge a fee for, acceptance of support, warranty, indemnity,
169
+ or other liability obligations and/or rights consistent with this
170
+ License. However, in accepting such obligations, You may act only
171
+ on Your own behalf and on Your sole responsibility, not on behalf
172
+ of any other Contributor, and only if You agree to indemnify,
173
+ defend, and hold each Contributor harmless for any liability
174
+ incurred by, or claims asserted against, such Contributor by reason
175
+ of your accepting any such warranty or additional liability.
176
+
177
+ END OF TERMS AND CONDITIONS
178
+
179
+ APPENDIX: How to apply the Apache License to your work.
180
+
181
+ To apply the Apache License to your work, attach the following
182
+ boilerplate notice, with the fields enclosed by brackets "[]"
183
+ replaced with your own identifying information. (Don't include
184
+ the brackets!) The text should be enclosed in the appropriate
185
+ comment syntax for the file format. We also recommend that a
186
+ file or class name and description of purpose be included on the
187
+ same "printed page" as the copyright notice for easier
188
+ identification within third-party archives.
189
+
190
+ Copyright [yyyy] [name of copyright owner]
191
+
192
+ Licensed under the Apache License, Version 2.0 (the "License");
193
+ you may not use this file except in compliance with the License.
194
+ You may obtain a copy of the License at
195
+
196
+ http://www.apache.org/licenses/LICENSE-2.0
197
+
198
+ Unless required by applicable law or agreed to in writing, software
199
+ distributed under the License is distributed on an "AS IS" BASIS,
200
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
201
+ See the License for the specific language governing permissions and
202
+ limitations under the License.
actioncodec2-0.1.0.tar.gz ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:3fe9e405acdac0b7c2b0bc8917cf29de32a4fd2e0742bf8024f66bc402ead181
3
+ size 298688
actioncodec2_kernels/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Portable ActionCodec2 CPU kernels."""
actioncodec2_kernels/grammar.cpp ADDED
@@ -0,0 +1,162 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+ #include <pybind11/stl.h>
4
+
5
+ #include <algorithm>
6
+ #include <cstdint>
7
+ #include <limits>
8
+ #include <map>
9
+ #include <memory>
10
+ #include <stdexcept>
11
+ #include <vector>
12
+
13
+ namespace py = pybind11;
14
+ namespace {
15
+ using State = std::vector<int64_t>;
16
+
17
+ class CoverageGrammar {
18
+ protected:
19
+ int64_t horizon_, vocabulary_;
20
+ virtual void validate_state(const State&) const = 0;
21
+ virtual bool allowed(const State&, int64_t) const = 0;
22
+ virtual State apply(const State&, int64_t) const = 0;
23
+ public:
24
+ virtual ~CoverageGrammar() = default;
25
+ State advance(const State& state, int64_t token) const {
26
+ validate_state(state);
27
+ if (token < 0 || token >= vocabulary_ || !allowed(state, token)) return {};
28
+ return apply(state, token);
29
+ }
30
+ py::array_t<bool> mask(const State& state) const {
31
+ validate_state(state);
32
+ py::array_t<bool> result(vocabulary_);
33
+ auto* output = result.mutable_data();
34
+ py::gil_scoped_release release;
35
+ for (int64_t token = 0; token < vocabulary_; ++token) output[token] = allowed(state, token);
36
+ return result;
37
+ }
38
+ };
39
+
40
+ class SetGrammar : public CoverageGrammar {
41
+ uint64_t full_;
42
+ std::vector<int64_t> roots_;
43
+ std::vector<std::vector<std::pair<int64_t, uint64_t>>> patterns_;
44
+ std::vector<bool> scope_;
45
+
46
+ void validate_state(const State& state) const override {
47
+ if (static_cast<int64_t>(state.size()) != horizon_)
48
+ throw std::invalid_argument("invalid grammar state length");
49
+ for (int64_t value : state) {
50
+ if (value < 0 || (static_cast<uint64_t>(value) & ~full_))
51
+ throw std::invalid_argument("invalid grammar state support");
52
+ }
53
+ }
54
+ int64_t front(const State& state) const {
55
+ for (int64_t t = 0; t < horizon_; ++t)
56
+ if (static_cast<uint64_t>(state[t]) != full_) return t;
57
+ return horizon_;
58
+ }
59
+ bool allowed(const State& state, int64_t token) const override {
60
+ if (!scope_[token]) return false;
61
+ const int64_t start = front(state);
62
+ if (start == horizon_) return false;
63
+ uint64_t missing = full_ & ~static_cast<uint64_t>(state[start]);
64
+ int64_t root = 0;
65
+ while (!(missing & 1)) { missing >>= 1; ++root; }
66
+ if (roots_[token] != root) return false;
67
+ for (auto [offset, support] : patterns_[token]) {
68
+ if (offset >= horizon_ - start || (static_cast<uint64_t>(state[start + offset]) & support))
69
+ return false;
70
+ }
71
+ return true;
72
+ }
73
+ State apply(const State& state, int64_t token) const override {
74
+ State result = state;
75
+ const int64_t start = front(state);
76
+ for (auto [offset, support] : patterns_[token])
77
+ result[start + offset] |= static_cast<int64_t>(support);
78
+ return result;
79
+ }
80
+ public:
81
+ SetGrammar(int64_t horizon, uint64_t full, std::vector<int64_t> roots,
82
+ std::vector<std::vector<std::pair<int64_t, uint64_t>>> patterns)
83
+ : full_(full), roots_(std::move(roots)), patterns_(std::move(patterns)) {
84
+ if (horizon < 1 || !full || full > static_cast<uint64_t>(INT64_MAX) ||
85
+ roots_.empty() || roots_.size() != patterns_.size())
86
+ throw std::invalid_argument("invalid Set-BPE grammar metadata");
87
+ horizon_ = horizon;
88
+ vocabulary_ = roots_.size();
89
+ for (int64_t token = 0; token < vocabulary_; ++token) {
90
+ if (roots_[token] < 0 || roots_[token] >= 63 || patterns_[token].empty())
91
+ throw std::invalid_argument("invalid grammar root or empty pattern");
92
+ std::map<int64_t, uint64_t> coverage;
93
+ bool scope = true;
94
+ for (auto [offset, support] : patterns_[token]) {
95
+ if (offset < 0 || !support || support > static_cast<uint64_t>(INT64_MAX) ||
96
+ (coverage[offset] & support))
97
+ throw std::invalid_argument("invalid grammar pattern offset or support");
98
+ coverage[offset] |= support;
99
+ scope = scope && !(support & ~full_);
100
+ }
101
+ const uint64_t anchor = coverage[0];
102
+ if (!(anchor & (uint64_t{1} << roots_[token])) ||
103
+ (anchor & ((uint64_t{1} << roots_[token]) - 1)))
104
+ throw std::invalid_argument("grammar root must anchor the pattern");
105
+ scope_.push_back(scope);
106
+ }
107
+ }
108
+ };
109
+
110
+ class PlainGrammar : public CoverageGrammar {
111
+ int64_t width_, cells_;
112
+ std::vector<int64_t> lengths_;
113
+ std::vector<bool> phase_;
114
+
115
+ void validate_state(const State& state) const override {
116
+ if (state.size() != 1 || state[0] < 0 || state[0] > cells_)
117
+ throw std::invalid_argument("invalid plain-BPE grammar state");
118
+ }
119
+ bool allowed(const State& state, int64_t token) const override {
120
+ return lengths_[token] <= cells_ - state[0] &&
121
+ phase_[(state[0] % width_) * vocabulary_ + token];
122
+ }
123
+ State apply(const State& state, int64_t token) const override {
124
+ return {state[0] + lengths_[token]};
125
+ }
126
+ public:
127
+ PlainGrammar(int64_t horizon, std::vector<int64_t> lengths,
128
+ const py::array_t<bool, py::array::c_style | py::array::forcecast>& phase)
129
+ : lengths_(std::move(lengths)) {
130
+ if (horizon < 1 || lengths_.empty() || phase.ndim() != 2 || phase.shape(0) < 1 ||
131
+ phase.shape(1) != static_cast<int64_t>(lengths_.size()) ||
132
+ horizon > std::numeric_limits<int64_t>::max() / phase.shape(0))
133
+ throw std::invalid_argument("invalid plain-BPE grammar metadata");
134
+ for (int64_t length : lengths_) {
135
+ if (length < 1) throw std::invalid_argument("token expansion must be positive");
136
+ }
137
+ horizon_ = horizon;
138
+ width_ = phase.shape(0);
139
+ cells_ = horizon * width_;
140
+ vocabulary_ = lengths_.size();
141
+ phase_.assign(phase.data(), phase.data() + phase.size());
142
+ }
143
+ };
144
+ }
145
+
146
+ void register_grammar(py::module_& module) {
147
+ py::class_<CoverageGrammar, std::shared_ptr<CoverageGrammar>>(module, "CoverageGrammar")
148
+ .def("advance", [](const CoverageGrammar& grammar, const State& state, int64_t token) -> py::object {
149
+ State next = grammar.advance(state, token);
150
+ if (next.empty()) return py::none();
151
+ py::tuple result(next.size());
152
+ for (size_t i = 0; i < next.size(); ++i) result[i] = next[i];
153
+ return result;
154
+ })
155
+ .def("mask", &CoverageGrammar::mask);
156
+ py::class_<SetGrammar, CoverageGrammar, std::shared_ptr<SetGrammar>>(module, "SetGrammar")
157
+ .def(py::init<int64_t, uint64_t, std::vector<int64_t>,
158
+ std::vector<std::vector<std::pair<int64_t, uint64_t>>>>());
159
+ py::class_<PlainGrammar, CoverageGrammar, std::shared_ptr<PlainGrammar>>(module, "PlainGrammar")
160
+ .def(py::init<int64_t, std::vector<int64_t>,
161
+ const py::array_t<bool, py::array::c_style | py::array::forcecast>&>());
162
+ }
actioncodec2_kernels/module.cpp ADDED
@@ -0,0 +1,88 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+
4
+ #include <cstdint>
5
+ #include <stdexcept>
6
+
7
+ #include "parallel.hpp"
8
+
9
+ namespace py = pybind11;
10
+
11
+ py::array_t<int32_t> so3_second_order_encode_batch(
12
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
13
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>&,
14
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
15
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
16
+ int32_t, bool, double, double, bool, int);
17
+
18
+ py::array_t<int32_t> additive_second_order_encode_batch(
19
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&,
20
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>&,
21
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
22
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
23
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&, bool, int);
24
+
25
+ py::array_t<float> physical_decode_batch(
26
+ const py::array_t<float>&, const py::array_t<int32_t>&, const py::array_t<uint8_t>&,
27
+ const py::array_t<float>&, const py::array_t<float>&,
28
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>&,
29
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&,
30
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&, int32_t, int);
31
+
32
+ py::array_t<int32_t> physical_encode_batch(
33
+ const py::array_t<float>&, const py::array_t<int32_t>&, const py::array_t<uint8_t>&,
34
+ const py::array_t<float>&, const py::array_t<float>&,
35
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&,
36
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&,
37
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&, int32_t, int,
38
+ const py::array_t<float>&, bool);
39
+
40
+ py::array_t<int32_t> so3_encode_batch(
41
+ const py::array_t<double, py::array::c_style | py::array::forcecast>&,
42
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>&,
43
+ const py::array_t<float, py::array::c_style | py::array::forcecast>&, int32_t, bool, bool,
44
+ double, bool, int);
45
+
46
+ void register_setbpe(py::module_& module);
47
+ void register_resample(py::module_& module);
48
+ void register_reconstruction(py::module_& module);
49
+ void register_grammar(py::module_& module);
50
+
51
+ PYBIND11_MODULE(_actioncodec2_native, module) {
52
+ module.doc() = "ActionCodec2 encode/decode kernels: physical stage, resampling, Set-BPE";
53
+ register_setbpe(module);
54
+ register_resample(module);
55
+ register_reconstruction(module);
56
+ register_grammar(module);
57
+ module.def(
58
+ "set_thread_budget",
59
+ [](int threads) {
60
+ if (threads < 0) throw std::invalid_argument("thread budget must be non-negative");
61
+ ac2::thread_budget().store(threads);
62
+ },
63
+ py::arg("threads"),
64
+ "Cap threads per call for this process; 0 uses every CPU it may run on.");
65
+ module.def("thread_budget", [] { return ac2::thread_budget().load(); });
66
+ module.def("plan_threads", &ac2::plan_threads, py::arg("count"), py::arg("requested"),
67
+ py::arg("seconds_each"), "Threads a call would use for ``count`` items.");
68
+ module.def("so3_second_order_encode_batch", &so3_second_order_encode_batch,
69
+ py::arg("table"), py::arg("bins"), py::arg("quaternions"),
70
+ py::arg("previous_first_order"), py::arg("input_order"), py::arg("left"),
71
+ py::arg("limit"), py::arg("radius"), py::arg("smooth"), py::arg("threads") = 0);
72
+ module.def("additive_second_order_encode_batch", &additive_second_order_encode_batch,
73
+ py::arg("table"), py::arg("bins"), py::arg("positions"),
74
+ py::arg("previous_first_order"), py::arg("epsilon"), py::arg("smooth"),
75
+ py::arg("threads") = 0);
76
+ module.def("physical_decode_batch", &physical_decode_batch, py::arg("table"),
77
+ py::arg("bins"), py::arg("binary"), py::arg("lower"), py::arg("upper"),
78
+ py::arg("cells"), py::arg("initial"), py::arg("initial_velocity"),
79
+ py::arg("order"), py::arg("threads") = 0);
80
+ module.def("physical_encode_batch", &physical_encode_batch, py::arg("table"), py::arg("bins"),
81
+ py::arg("binary"), py::arg("lower"), py::arg("upper"), py::arg("positions"),
82
+ py::arg("initial"), py::arg("initial_velocity"), py::arg("order"),
83
+ py::arg("threads") = 0, py::arg("epsilon") = py::array_t<float>(),
84
+ py::arg("smooth") = false);
85
+ module.def("so3_encode_batch", &so3_encode_batch, py::arg("table"), py::arg("bins"),
86
+ py::arg("actions"), py::arg("input_order"), py::arg("rotation_vector"),
87
+ py::arg("left"), py::arg("limit"), py::arg("smooth"), py::arg("threads") = 0);
88
+ }
actioncodec2_kernels/parallel.hpp ADDED
@@ -0,0 +1,88 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #pragma once
2
+
3
+ #include <algorithm>
4
+ #include <atomic>
5
+ #include <chrono>
6
+ #include <cmath>
7
+ #include <cstdint>
8
+ #include <exception>
9
+ #include <mutex>
10
+ #include <thread>
11
+ #include <vector>
12
+
13
+ #ifdef __linux__
14
+ #include <sched.h>
15
+ #endif
16
+
17
+ namespace ac2 {
18
+
19
+ inline std::atomic<int>& thread_budget() {
20
+ static std::atomic<int> value{0};
21
+ return value;
22
+ }
23
+
24
+ inline int available_cpus() {
25
+ #ifdef __linux__
26
+ cpu_set_t allowed;
27
+ CPU_ZERO(&allowed);
28
+ if (sched_getaffinity(0, sizeof(allowed), &allowed) == 0) {
29
+ return std::max(1, CPU_COUNT(&allowed));
30
+ }
31
+ #endif
32
+ return std::max(1, static_cast<int>(std::thread::hardware_concurrency()));
33
+ }
34
+
35
+ constexpr double kThreadStartSeconds = 30e-6;
36
+
37
+ inline int64_t plan_threads(int64_t count, int requested, double seconds_each) {
38
+ const int budget = thread_budget().load();
39
+ int64_t threads = budget > 0 ? budget : available_cpus();
40
+ if (requested > 0) threads = std::min<int64_t>(threads, requested);
41
+ const double by_work = std::sqrt(count * seconds_each / kThreadStartSeconds);
42
+ if (by_work < static_cast<double>(threads)) threads = static_cast<int64_t>(by_work);
43
+ return std::max<int64_t>(1, std::min(threads, count));
44
+ }
45
+
46
+ template <class Body>
47
+ void parallel_for(int64_t count, int threads, Body&& body) {
48
+ if (count <= 0) return;
49
+ const auto start = std::chrono::steady_clock::now();
50
+ body(0);
51
+ const double seconds_each =
52
+ std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count();
53
+ const int64_t rest = count - 1;
54
+ if (rest == 0) return;
55
+ const int64_t workers = plan_threads(rest, threads, seconds_each);
56
+ auto run = [&](int64_t worker) {
57
+ const int64_t begin = 1 + rest * worker / workers;
58
+ const int64_t end = 1 + rest * (worker + 1) / workers;
59
+ for (int64_t index = begin; index < end; ++index) body(index);
60
+ };
61
+ if (workers == 1) {
62
+ run(0);
63
+ return;
64
+ }
65
+ std::exception_ptr failure;
66
+ std::mutex failure_lock;
67
+ auto guarded = [&](int64_t worker) {
68
+ try {
69
+ run(worker);
70
+ } catch (...) {
71
+ std::lock_guard<std::mutex> hold(failure_lock);
72
+ if (!failure) failure = std::current_exception();
73
+ }
74
+ };
75
+ std::vector<std::thread> pool;
76
+ pool.reserve(static_cast<size_t>(workers - 1));
77
+ try {
78
+ for (int64_t worker = 1; worker < workers; ++worker) pool.emplace_back(guarded, worker);
79
+ } catch (...) {
80
+ for (auto& thread : pool) thread.join();
81
+ throw;
82
+ }
83
+ guarded(0);
84
+ for (auto& thread : pool) thread.join();
85
+ if (failure) std::rethrow_exception(failure);
86
+ }
87
+
88
+ }
actioncodec2_kernels/physical.cpp ADDED
@@ -0,0 +1,457 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/pybind11.h>
2
+ #include <pybind11/numpy.h>
3
+ #include <pybind11/stl.h>
4
+
5
+ #include <algorithm>
6
+ #include <cfloat>
7
+ #include <cmath>
8
+ #include <cstdint>
9
+ #include <limits>
10
+ #include <vector>
11
+
12
+ #include "parallel.hpp"
13
+
14
+ namespace py = pybind11;
15
+
16
+ namespace {
17
+
18
+ struct SecondOrderAxis {
19
+ const float* values;
20
+ int32_t count;
21
+ bool ordered;
22
+ int32_t nonnegative;
23
+ int32_t nonpositive;
24
+ };
25
+
26
+ template <bool Smooth>
27
+ inline int32_t pick_second_order_axis(const SecondOrderAxis& axis, double request,
28
+ double direction, double epsilon) {
29
+ const float* values = axis.values;
30
+ if (axis.ordered) {
31
+ const int32_t insertion = std::lower_bound(values, values + axis.count, request) - values;
32
+ const int32_t left = std::max(0, insertion - 1);
33
+ const int32_t right = std::min(insertion, axis.count - 1);
34
+ int32_t nearest = std::fabs(request - values[left]) <= std::fabs(request - values[right])
35
+ ? left : right;
36
+
37
+ while (nearest > 0 && std::fabs(request - values[nearest - 1]) == std::fabs(request - values[nearest]))
38
+ --nearest;
39
+ if constexpr (!Smooth) return nearest;
40
+ const int32_t preferred = direction > 0 ? std::max(nearest, axis.nonnegative)
41
+ : direction < 0 ? std::min(nearest, axis.nonpositive) : nearest;
42
+ const double primitive = values[preferred];
43
+ const double sign = primitive > 0 ? 1 : primitive < 0 ? -1 : 0;
44
+ return sign * direction >= 0 && std::fabs(request - primitive) <= epsilon ? preferred : nearest;
45
+ }
46
+
47
+ int32_t nearest = 0, feasible = -1, preferred = -1;
48
+ double nearest_distance = INFINITY, feasible_distance = INFINITY, preferred_distance = INFINITY;
49
+ for (int32_t k = 0; k < axis.count; ++k) {
50
+ const double primitive = values[k];
51
+ const double distance = std::fabs(request - primitive);
52
+ if (distance < nearest_distance) { nearest = k; nearest_distance = distance; }
53
+ if constexpr (!Smooth) continue;
54
+ if (distance > epsilon) continue;
55
+ if (distance < feasible_distance) { feasible = k; feasible_distance = distance; }
56
+ const double sign = primitive > 0 ? 1 : primitive < 0 ? -1 : 0;
57
+ if (sign * direction >= 0 && distance < preferred_distance) {
58
+ preferred = k;
59
+ preferred_distance = distance;
60
+ }
61
+ }
62
+ return preferred >= 0 ? preferred : feasible >= 0 ? feasible : nearest;
63
+ }
64
+
65
+ }
66
+
67
+ py::array_t<int32_t> additive_second_order_encode_batch(
68
+ const py::array_t<float, py::array::c_style | py::array::forcecast>& table,
69
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>& bins,
70
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& positions,
71
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& previous_first_order,
72
+ const py::array_t<float, py::array::c_style | py::array::forcecast>& epsilon,
73
+ bool smooth, int threads) {
74
+ if (positions.ndim() != 3 || table.ndim() != 2 || previous_first_order.ndim() != 2)
75
+ throw std::invalid_argument("invalid second-order additive batch shapes");
76
+ const int64_t batch = positions.shape(0), steps = positions.shape(1), dim = positions.shape(2);
77
+ if (table.shape(0) != dim || bins.size() != dim || epsilon.size() != dim ||
78
+ previous_first_order.shape(0) != batch || previous_first_order.shape(1) != dim)
79
+ throw std::invalid_argument("inconsistent second-order additive batch dimensions");
80
+ for (int64_t j = 0; j < dim; ++j) {
81
+ if (bins.data()[j] < 1 || bins.data()[j] > table.shape(1))
82
+ throw std::invalid_argument("primitive count exceeds table width");
83
+ }
84
+ py::array_t<int32_t> output({batch, steps, dim});
85
+ const auto* targets = positions.data();
86
+ const auto* initial = previous_first_order.data();
87
+ const auto* axes = table.data();
88
+ const auto* counts = bins.data();
89
+ const auto* limits = epsilon.data();
90
+ auto* cells = output.mutable_data();
91
+ const int64_t width = table.shape(1);
92
+ std::vector<SecondOrderAxis> search;
93
+ for (int64_t j = 0; j < dim; ++j) {
94
+ const float* begin = axes + j * width;
95
+ const float* end = begin + counts[j];
96
+ const bool ordered = counts[j] > 16 &&
97
+ std::adjacent_find(begin, end, [](float a, float b) { return a >= b; }) == end;
98
+ const int32_t nonnegative = ordered ? std::lower_bound(begin, end, 0.0f) - begin : 0;
99
+ const int32_t nonpositive = ordered ? std::upper_bound(begin, end, 0.0f) - begin - 1 : 0;
100
+ search.push_back({begin, counts[j], ordered, std::min(nonnegative, counts[j] - 1),
101
+ std::max(nonpositive, 0)});
102
+ }
103
+ py::gil_scoped_release release;
104
+ ac2::parallel_for(batch, threads, [&](int64_t i) {
105
+ std::vector<double> position(dim, 0), velocity(initial+i*dim, initial+(i+1)*dim), direction(dim, 0);
106
+ for (int64_t t = 0; t < steps; ++t) {
107
+ for (int64_t j = 0; j < dim; ++j) {
108
+ const double request = targets[(i*steps+t)*dim+j] - (position[j]+velocity[j]);
109
+ const int32_t choice = smooth
110
+ ? pick_second_order_axis<true>(search[j], request, direction[j], limits[j])
111
+ : pick_second_order_axis<false>(search[j], request, direction[j], limits[j]);
112
+ const double primitive = axes[j*width+choice];
113
+ velocity[j] += primitive;
114
+ position[j] += velocity[j];
115
+ if (primitive != 0) direction[j] = primitive > 0 ? 1 : -1;
116
+ cells[(i*steps+t)*dim+j] = choice;
117
+ }
118
+ }
119
+ });
120
+ return output;
121
+ }
122
+
123
+ namespace {
124
+
125
+ struct Layout {
126
+ int32_t dimension;
127
+ int32_t width;
128
+ const float* table;
129
+ const int32_t* bins;
130
+ const uint8_t* binary;
131
+ const float* lower;
132
+ const float* upper;
133
+ const float* epsilon;
134
+ bool smooth;
135
+ };
136
+
137
+ constexpr float kSourceTolerance = 8.0F * FLT_EPSILON;
138
+
139
+ inline int32_t nearest(const Layout& layout, int32_t axis, float request) {
140
+ const float* values = layout.table + static_cast<size_t>(axis) * layout.width;
141
+ const int32_t count = layout.bins[axis];
142
+ int32_t best = 0;
143
+ float best_distance = std::fabs(values[0] - request);
144
+ for (int32_t index = 1; index < count; ++index) {
145
+ const float distance = std::fabs(values[index] - request);
146
+ if (distance < best_distance) {
147
+ best_distance = distance;
148
+ best = index;
149
+ }
150
+ }
151
+ return best;
152
+ }
153
+
154
+ inline int32_t select_smooth(const Layout& layout, int32_t axis, float request, float direction) {
155
+ const float* values = layout.table + static_cast<size_t>(axis) * layout.width;
156
+ const int32_t count = layout.bins[axis];
157
+ const float radius = layout.epsilon[axis] + kSourceTolerance;
158
+ int32_t nearest_index = 0;
159
+ float nearest_distance = std::fabs(values[0] - request);
160
+ bool any_feasible = false;
161
+ int32_t feasible_index = 0;
162
+ float feasible_distance = 0.0F;
163
+ bool any_preferred = false;
164
+ int32_t preferred_index = 0;
165
+ float preferred_distance = 0.0F;
166
+ for (int32_t index = 0; index < count; ++index) {
167
+ const float distance = std::fabs(values[index] - request);
168
+ if (index > 0 && distance < nearest_distance) {
169
+ nearest_distance = distance;
170
+ nearest_index = index;
171
+ }
172
+ if (!(distance <= radius)) continue;
173
+ if (!any_feasible || distance < feasible_distance) {
174
+ any_feasible = true;
175
+ feasible_distance = distance;
176
+ feasible_index = index;
177
+ }
178
+
179
+ const float sign = values[index] > 0.0F ? 1.0F : (values[index] < 0.0F ? -1.0F : 0.0F);
180
+ if (!(sign * direction >= 0.0F)) continue;
181
+ if (!any_preferred || distance < preferred_distance) {
182
+ any_preferred = true;
183
+ preferred_distance = distance;
184
+ preferred_index = index;
185
+ }
186
+ }
187
+ if (any_preferred) return preferred_index;
188
+ if (any_feasible) return feasible_index;
189
+ return nearest_index;
190
+ }
191
+
192
+ void encode_first_order(const Layout& layout, const float* positions, int64_t steps,
193
+ const float* initial, int32_t* cells) {
194
+ const int32_t dimension = layout.dimension;
195
+ std::vector<float> state(initial, initial + dimension);
196
+
197
+ std::vector<float> direction(dimension, 0.0F);
198
+ for (int64_t step = 0; step < steps; ++step) {
199
+ const float* target = positions + step * dimension;
200
+ int32_t* row = cells + step * dimension;
201
+ for (int32_t axis = 0; axis < dimension; ++axis) {
202
+ const float* values = layout.table + static_cast<size_t>(axis) * layout.width;
203
+ if (layout.binary[axis]) {
204
+ const int32_t cell = nearest(layout, axis, target[axis]);
205
+ state[axis] = values[cell];
206
+ row[axis] = cell;
207
+ continue;
208
+ }
209
+ const float request = target[axis] - state[axis];
210
+ const int32_t cell = layout.smooth ? select_smooth(layout, axis, request, direction[axis])
211
+ : nearest(layout, axis, request);
212
+ const float picked = values[cell];
213
+ state[axis] = std::min(std::max(state[axis] + picked, layout.lower[axis]),
214
+ layout.upper[axis]);
215
+ if (picked != 0.0F) direction[axis] = picked > 0.0F ? 1.0F : -1.0F;
216
+ row[axis] = cell;
217
+ }
218
+ }
219
+ }
220
+
221
+ void decode_first_order(const Layout& layout, const int32_t* cells, int64_t steps,
222
+ const float* initial, float* positions) {
223
+ const int32_t dimension = layout.dimension;
224
+ std::vector<float> state(initial, initial + dimension);
225
+ for (int64_t step = 0; step < steps; ++step) {
226
+ const int32_t* row = cells + step * dimension;
227
+ float* out = positions + step * dimension;
228
+ for (int32_t axis = 0; axis < dimension; ++axis) {
229
+ const float* values = layout.table + static_cast<size_t>(axis) * layout.width;
230
+ const float primitive = values[row[axis]];
231
+ if (layout.binary[axis]) {
232
+ state[axis] = primitive;
233
+ } else {
234
+ state[axis] = std::min(std::max(state[axis] + primitive, layout.lower[axis]),
235
+ layout.upper[axis]);
236
+ }
237
+ out[axis] = state[axis];
238
+ }
239
+ }
240
+ }
241
+
242
+ void encode_second_order(const Layout& layout, const float* positions, int64_t steps,
243
+ const float* initial, const float* initial_velocity, int32_t* cells) {
244
+ const int32_t dimension = layout.dimension;
245
+ std::vector<float> source(initial, initial + dimension);
246
+ std::vector<float> source_velocity(initial_velocity, initial_velocity + dimension);
247
+ std::vector<float> recon(initial, initial + dimension);
248
+ std::vector<float> recon_velocity(initial_velocity, initial_velocity + dimension);
249
+ std::vector<float> error(dimension, 0.0F);
250
+ std::vector<float> previous(dimension, 0.0F);
251
+ for (int64_t step = 0; step < steps; ++step) {
252
+ const float* target = positions + step * dimension;
253
+ int32_t* row = cells + step * dimension;
254
+ for (int32_t axis = 0; axis < dimension; ++axis) {
255
+ const float* values = layout.table + static_cast<size_t>(axis) * layout.width;
256
+ if (layout.binary[axis]) {
257
+ row[axis] = nearest(layout, axis, target[axis]);
258
+ continue;
259
+ }
260
+ const float velocity = target[axis] - source[axis];
261
+ const float request =
262
+ 2.0F * error[axis] - previous[axis] + (velocity - source_velocity[axis]);
263
+ const int32_t cell = nearest(layout, axis, request);
264
+ recon_velocity[axis] += values[cell];
265
+ recon[axis] += recon_velocity[axis];
266
+ source[axis] = target[axis];
267
+ source_velocity[axis] = velocity;
268
+ previous[axis] = error[axis];
269
+ error[axis] = source[axis] - recon[axis];
270
+ row[axis] = cell;
271
+ }
272
+ }
273
+ }
274
+
275
+ void decode_second_order(const Layout& layout, const int32_t* cells, int64_t steps,
276
+ const float* initial, const float* initial_velocity, float* positions) {
277
+ const int32_t dimension = layout.dimension;
278
+ std::vector<float> state(initial, initial + dimension);
279
+ std::vector<float> velocity(initial_velocity, initial_velocity + dimension);
280
+ for (int64_t step = 0; step < steps; ++step) {
281
+ const int32_t* row = cells + step * dimension;
282
+ float* out = positions + step * dimension;
283
+ for (int32_t axis = 0; axis < dimension; ++axis) {
284
+ const float primitive = layout.table[static_cast<size_t>(axis) * layout.width + row[axis]];
285
+ if (layout.binary[axis]) {
286
+ out[axis] = primitive;
287
+ continue;
288
+ }
289
+ velocity[axis] += primitive;
290
+ state[axis] += velocity[axis];
291
+ out[axis] = state[axis];
292
+ }
293
+ }
294
+ }
295
+
296
+ Layout make_layout(const py::array_t<float>& table, const py::array_t<int32_t>& bins,
297
+ const py::array_t<uint8_t>& binary, const py::array_t<float>& lower,
298
+ const py::array_t<float>& upper, const py::array_t<float>& epsilon,
299
+ bool smooth) {
300
+ if (table.ndim() != 2 || table.shape(0) < 1 || table.shape(1) < 1 ||
301
+ table.shape(0) > std::numeric_limits<int32_t>::max() ||
302
+ table.shape(1) > std::numeric_limits<int32_t>::max()) {
303
+ throw std::invalid_argument("table must have non-empty shape [D, K]");
304
+ }
305
+ for (const py::array* array : {static_cast<const py::array*>(&table),
306
+ static_cast<const py::array*>(&bins),
307
+ static_cast<const py::array*>(&binary),
308
+ static_cast<const py::array*>(&lower),
309
+ static_cast<const py::array*>(&upper),
310
+ static_cast<const py::array*>(&epsilon)}) {
311
+ if (!(array->flags() & py::array::c_style)) {
312
+ throw std::invalid_argument("physical layout arrays must be C-contiguous");
313
+ }
314
+ }
315
+ const auto dimension = table.shape(0);
316
+ if (bins.ndim() != 1 || binary.ndim() != 1 || lower.ndim() != 1 || upper.ndim() != 1 ||
317
+ bins.size() != dimension || binary.size() != dimension ||
318
+ lower.size() != dimension || upper.size() != dimension) {
319
+ throw std::invalid_argument("layout vectors must have shape [D]");
320
+ }
321
+ if ((epsilon.size() && (epsilon.ndim() != 1 || epsilon.size() != dimension)) ||
322
+ (smooth && !epsilon.size())) {
323
+ throw std::invalid_argument("smooth selection needs epsilon with shape [D]");
324
+ }
325
+ for (py::ssize_t axis = 0; axis < dimension; ++axis) {
326
+ const int32_t count = bins.data()[axis];
327
+ if (count < 1 || count > table.shape(1) || binary.data()[axis] > 1 ||
328
+ !(lower.data()[axis] <= upper.data()[axis])) {
329
+ throw std::invalid_argument("invalid physical layout bounds or primitive count");
330
+ }
331
+ if (epsilon.size() &&
332
+ !(std::isfinite(epsilon.data()[axis]) && epsilon.data()[axis] >= 0)) {
333
+ throw std::invalid_argument("epsilon must be finite and non-negative");
334
+ }
335
+ for (int32_t cell = 0; cell < count; ++cell) {
336
+ if (!std::isfinite(table.data()[axis * table.shape(1) + cell])) {
337
+ throw std::invalid_argument("active primitives must be finite");
338
+ }
339
+ }
340
+ }
341
+ Layout layout;
342
+ layout.dimension = static_cast<int32_t>(table.shape(0));
343
+ layout.width = static_cast<int32_t>(table.shape(1));
344
+ layout.table = table.data();
345
+ layout.bins = bins.data();
346
+ layout.binary = binary.data();
347
+ layout.lower = lower.data();
348
+ layout.upper = upper.data();
349
+ layout.epsilon = epsilon.size() == 0 ? nullptr : epsilon.data();
350
+ layout.smooth = smooth && layout.epsilon != nullptr;
351
+ return layout;
352
+ }
353
+
354
+ using Array2D = py::array_t<float, py::array::c_style | py::array::forcecast>;
355
+ using Array3D = py::array_t<float, py::array::c_style | py::array::forcecast>;
356
+ using Cells3D = py::array_t<int32_t, py::array::c_style | py::array::forcecast>;
357
+
358
+ }
359
+
360
+ py::array_t<float> physical_decode_batch(const py::array_t<float>& table,
361
+ const py::array_t<int32_t>& bins,
362
+ const py::array_t<uint8_t>& binary,
363
+ const py::array_t<float>& lower,
364
+ const py::array_t<float>& upper, const Cells3D& cells,
365
+ const Array2D& initial,
366
+ const Array2D& initial_velocity, int32_t order,
367
+ int threads) {
368
+ const Layout layout =
369
+ make_layout(table, bins, binary, lower, upper, py::array_t<float>(), false);
370
+ if (cells.ndim() != 3 || cells.shape(1) < 1 || cells.shape(2) != layout.dimension || initial.ndim() != 2 ||
371
+ initial_velocity.ndim() != 2 || initial.shape(0) != cells.shape(0) ||
372
+ initial_velocity.shape(0) != cells.shape(0) || initial.shape(1) != cells.shape(2) ||
373
+ initial_velocity.shape(1) != cells.shape(2) || (order != 1 && order != 2)) {
374
+ throw std::invalid_argument("invalid physical decode batch shapes or order");
375
+ }
376
+ const int64_t batch = cells.shape(0);
377
+ const int64_t steps = cells.shape(1);
378
+ const int64_t dimension = layout.dimension;
379
+ for (py::ssize_t index = 0; index < initial.size(); ++index) {
380
+ if (!std::isfinite(initial.data()[index]) || !std::isfinite(initial_velocity.data()[index])) {
381
+ throw std::invalid_argument("initial state and velocity must be finite");
382
+ }
383
+ }
384
+ for (py::ssize_t index = 0; index < cells.size(); ++index) {
385
+ const int32_t cell = cells.data()[index];
386
+ if (cell < 0 || cell >= layout.bins[index % dimension]) {
387
+ throw std::invalid_argument("cell index is outside its primitive table");
388
+ }
389
+ }
390
+ py::array_t<float> positions({batch, steps, dimension});
391
+ {
392
+ py::gil_scoped_release release;
393
+ ac2::parallel_for(batch, threads, [&](int64_t index) {
394
+ const int32_t* source = cells.data() + index * steps * dimension;
395
+ float* target = positions.mutable_data() + index * steps * dimension;
396
+ if (order == 1) {
397
+ decode_first_order(layout, source, steps, initial.data() + index * dimension, target);
398
+ } else {
399
+ decode_second_order(layout, source, steps, initial.data() + index * dimension,
400
+ initial_velocity.data() + index * dimension, target);
401
+ }
402
+ });
403
+ }
404
+ return positions;
405
+ }
406
+
407
+ py::array_t<int32_t> physical_encode_batch(const py::array_t<float>& table,
408
+ const py::array_t<int32_t>& bins,
409
+ const py::array_t<uint8_t>& binary,
410
+ const py::array_t<float>& lower,
411
+ const py::array_t<float>& upper,
412
+ const Array3D& positions, const Array2D& initial,
413
+ const Array2D& initial_velocity, int32_t order,
414
+ int threads, const py::array_t<float>& epsilon,
415
+ bool smooth) {
416
+ const Layout layout = make_layout(table, bins, binary, lower, upper, epsilon, smooth);
417
+ if (positions.ndim() != 3 || positions.shape(1) < 1 ||
418
+ positions.shape(2) != layout.dimension || initial.ndim() != 2 ||
419
+ initial_velocity.ndim() != 2 || initial.shape(0) != positions.shape(0) ||
420
+ initial_velocity.shape(0) != positions.shape(0) ||
421
+ initial.shape(1) != layout.dimension || initial_velocity.shape(1) != layout.dimension ||
422
+ (order != 1 && order != 2)) {
423
+ throw std::invalid_argument("invalid physical encode batch shapes or order");
424
+ }
425
+ for (py::ssize_t index = 0; index < positions.size(); ++index) {
426
+ if (!std::isfinite(positions.data()[index])) {
427
+ throw std::invalid_argument("positions must be finite");
428
+ }
429
+ }
430
+ for (py::ssize_t index = 0; index < initial.size(); ++index) {
431
+ if (!std::isfinite(initial.data()[index]) || !std::isfinite(initial_velocity.data()[index])) {
432
+ throw std::invalid_argument("initial state and velocity must be finite");
433
+ }
434
+ }
435
+ const int64_t batch = positions.shape(0);
436
+ const int64_t steps = positions.shape(1);
437
+ const int64_t dimension = layout.dimension;
438
+ py::array_t<int32_t> cells({batch, steps, dimension});
439
+ const float* source = positions.data();
440
+ const float* starts = initial.data();
441
+ const float* velocities = initial_velocity.data();
442
+ int32_t* output = cells.mutable_data();
443
+ {
444
+ py::gil_scoped_release release;
445
+ ac2::parallel_for(batch, threads, [&](int64_t index) {
446
+ const float* chunk = source + index * steps * dimension;
447
+ int32_t* target = output + index * steps * dimension;
448
+ if (order == 1) {
449
+ encode_first_order(layout, chunk, steps, starts + index * dimension, target);
450
+ } else {
451
+ encode_second_order(layout, chunk, steps, starts + index * dimension,
452
+ velocities + index * dimension, target);
453
+ }
454
+ });
455
+ }
456
+ return cells;
457
+ }
actioncodec2_kernels/quaternion.hpp ADDED
@@ -0,0 +1,46 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #pragma once
2
+ #include <algorithm>
3
+ #include <array>
4
+ #include <cmath>
5
+ #include <stdexcept>
6
+
7
+ namespace ac2q {
8
+ using Q = std::array<double, 4>;
9
+ inline Q normalize(Q q) {
10
+ double n = std::sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
11
+ if (n < 1e-12) throw std::invalid_argument("a pose contains a zero quaternion");
12
+ for (double& v : q) v /= n;
13
+ return q;
14
+ }
15
+ inline Q multiply(const Q& a, const Q& b) {
16
+ return normalize({a[3]*b[0]+b[3]*a[0]+(a[1]*b[2]-a[2]*b[1]),
17
+ a[3]*b[1]+b[3]*a[1]+(a[2]*b[0]-a[0]*b[2]),
18
+ a[3]*b[2]+b[3]*a[2]+(a[0]*b[1]-a[1]*b[0]),
19
+ a[3]*b[3]-(a[0]*b[0]+a[1]*b[1]+a[2]*b[2])});
20
+ }
21
+ inline Q conjugate(Q q) { for (int i=0;i<3;++i) q[i] = -q[i]; return q; }
22
+ inline Q from_rotvec(const double* v) {
23
+ const double a = std::sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
24
+ const double s = a >= 1e-12 ? std::sin(a/2)/a : .5;
25
+ return normalize({v[0]*s,v[1]*s,v[2]*s,std::cos(a/2)});
26
+ }
27
+ inline void to_rotvec(Q q, double* out) {
28
+ q=normalize(q); if(q[3]<0) for(double& x:q)x=-x;
29
+ const double n=std::sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]);
30
+ const double a=2*std::atan2(n,std::clamp(q[3],0.,1.));
31
+ const double s=n>=1e-12?a/n:2.;
32
+ for(int j=0;j<3;++j)out[j]=q[j]*s;
33
+ }
34
+ inline Q from_euler(const double* v) {
35
+ const double sx=std::sin(v[0]/2),sy=std::sin(v[1]/2),sz=std::sin(v[2]/2);
36
+ const double cx=std::cos(v[0]/2),cy=std::cos(v[1]/2),cz=std::cos(v[2]/2);
37
+ return normalize({sx*cy*cz-cx*sy*sz,cx*sy*cz+sx*cy*sz,
38
+ cx*cy*sz-sx*sy*cz,cx*cy*cz+sx*sy*sz});
39
+ }
40
+ inline void to_euler(Q q, double* out) {
41
+ q=normalize(q);const auto[x,y,z,w]=q;
42
+ out[0]=std::atan2(2*(w*x+y*z),1-2*(x*x+y*y));
43
+ out[1]=std::asin(std::clamp(2*(w*y-z*x),-1.,1.));
44
+ out[2]=std::atan2(2*(w*z+x*y),1-2*(y*y+z*z));
45
+ }
46
+ }
actioncodec2_kernels/reconstruct.cpp ADDED
@@ -0,0 +1,146 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+
4
+ #include <algorithm>
5
+ #include <cmath>
6
+ #include <cstdint>
7
+ #include <limits>
8
+ #include <stdexcept>
9
+
10
+ #include "parallel.hpp"
11
+ #include "quaternion.hpp"
12
+
13
+ namespace py = pybind11;
14
+ namespace {
15
+ using I64 = py::array_t<int64_t, py::array::c_style | py::array::forcecast>;
16
+ using I32 = py::array_t<int32_t, py::array::c_style | py::array::forcecast>;
17
+ using F64 = py::array_t<double, py::array::c_style | py::array::forcecast>;
18
+ using F32 = py::array_t<float, py::array::c_style | py::array::forcecast>;
19
+
20
+ void validate_reconstruction(const F64& table, const I32& bins, const I64& cells,
21
+ const F32& previous, const F32& scale) {
22
+ if (table.ndim() != 2 || table.shape(0) < 1 || table.shape(1) < 1 ||
23
+ bins.ndim() != 1 || bins.size() != table.shape(0) ||
24
+ cells.ndim() != 3 || cells.shape(0) < 1 || cells.shape(1) < 1 ||
25
+ cells.shape(2) != table.shape(0) || previous.ndim() != 2 ||
26
+ previous.shape(0) != cells.shape(0) || previous.shape(1) != cells.shape(2) ||
27
+ scale.ndim() != 1 || scale.size() != cells.shape(2)) {
28
+ throw std::invalid_argument("invalid reconstruction shapes");
29
+ }
30
+ for (py::ssize_t d = 0; d < bins.size(); ++d) {
31
+ if (bins.data()[d] < 1 || bins.data()[d] > table.shape(1))
32
+ throw std::invalid_argument("primitive count exceeds table width");
33
+ if (!std::isfinite(scale.data()[d]) || scale.data()[d] <= 0)
34
+ throw std::invalid_argument("scale must be positive and finite");
35
+ for (int32_t k = 0; k < bins.data()[d]; ++k) {
36
+ if (!std::isfinite(table.data()[d * table.shape(1) + k]))
37
+ throw std::invalid_argument("active primitives must be finite");
38
+ }
39
+ }
40
+ for (py::ssize_t i = 0; i < previous.size(); ++i) {
41
+ if (!std::isfinite(previous.data()[i]))
42
+ throw std::invalid_argument("previous first-order values must be finite");
43
+ }
44
+ for (py::ssize_t i = 0; i < cells.size(); ++i) {
45
+ if (cells.data()[i] < 0 || cells.data()[i] >= bins.data()[i % bins.size()])
46
+ throw std::invalid_argument("cell index is outside its primitive table");
47
+ }
48
+ }
49
+
50
+ F32 so3_reconstruct(const F64& table, const I32& bins, const I64& cells,
51
+ const F32& previous, const F32& scale, int input_order,
52
+ int primitive_order, bool left, bool rotation_vector, double radius) {
53
+ validate_reconstruction(table, bins, cells, previous, scale);
54
+ if (table.shape(0) != 3 || (input_order != 0 && input_order != 1) ||
55
+ (primitive_order != 1 && primitive_order != 2) ||
56
+ std::isnan(radius) || radius <= 0)
57
+ throw std::invalid_argument("invalid SO(3) reconstruction order or radius");
58
+ const int64_t batch = cells.shape(0), steps = cells.shape(1);
59
+ F32 output({batch, steps, int64_t{3}});
60
+ auto* target = output.mutable_data();
61
+ py::gil_scoped_release release;
62
+ ac2::parallel_for(batch, 0, [&](int64_t b) {
63
+ double boundary[3];
64
+ for (int d = 0; d < 3; ++d)
65
+ boundary[d] = static_cast<double>(previous.data()[3 * b + d]) * scale.data()[d];
66
+ auto increment = rotation_vector ? ac2q::from_rotvec(boundary) : ac2q::from_euler(boundary);
67
+ ac2q::Q state{0, 0, 0, 1};
68
+ float accumulated[3] = {0, 0, 0};
69
+ auto apply = [&](ac2q::Q a, ac2q::Q v) {
70
+ return left ? ac2q::multiply(v, a) : ac2q::multiply(a, v);
71
+ };
72
+ for (int64_t t = 0; t < steps; ++t) {
73
+ double v[3], represented[3];
74
+ for (int d = 0; d < 3; ++d)
75
+ v[d] = table.data()[d * table.shape(1) + cells.data()[(b * steps + t) * 3 + d]];
76
+ if (primitive_order == 2) {
77
+ const double norm = std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
78
+ const double factor = std::min(1., radius / std::max(norm, std::numeric_limits<double>::min()));
79
+ for (double& x : v) x *= factor;
80
+ increment = apply(increment, ac2q::from_rotvec(v));
81
+ state = apply(state, increment);
82
+ auto q = input_order == 1 ? increment : state;
83
+ if (rotation_vector) ac2q::to_rotvec(q, represented);
84
+ else ac2q::to_euler(q, represented);
85
+ } else if (input_order == 1) {
86
+ if (rotation_vector) std::copy(v, v + 3, represented);
87
+ else {
88
+ for (double& x : v) x = static_cast<float>(x);
89
+ ac2q::to_euler(ac2q::from_rotvec(v), represented);
90
+ }
91
+ } else {
92
+ state = apply(state, ac2q::from_rotvec(v));
93
+ if (rotation_vector) ac2q::to_rotvec(state, represented);
94
+ else ac2q::to_euler(state, represented);
95
+ }
96
+ for (int d = 0; d < 3; ++d) {
97
+ float value = static_cast<float>(represented[d] / scale.data()[d]);
98
+ if (primitive_order == 1 && input_order == 1) {
99
+ const float next = accumulated[d] + value;
100
+ value = next - accumulated[d];
101
+ accumulated[d] = next;
102
+ }
103
+ target[(b * steps + t) * 3 + d] = value;
104
+ }
105
+ }
106
+ });
107
+ return output;
108
+ }
109
+
110
+ F32 additive_reconstruct(const F64& table, const I32& bins, const I64& cells,
111
+ const F32& previous, const F32& scale, int input_order) {
112
+ validate_reconstruction(table, bins, cells, previous, scale);
113
+ if (input_order < 0 || input_order > 2)
114
+ throw std::invalid_argument("invalid additive reconstruction order");
115
+ const int64_t batch = cells.shape(0), steps = cells.shape(1), width = cells.shape(2);
116
+ F32 output({batch, steps, width});
117
+ auto* target = output.mutable_data();
118
+ py::gil_scoped_release release;
119
+ ac2::parallel_for(batch, 0, [&](int64_t b) {
120
+ for (int64_t d = 0; d < width; ++d) {
121
+ double change_sum = 0, position = 0;
122
+ const float boundary = previous.data()[b * width + d] * scale.data()[d];
123
+ for (int64_t t = 0; t < steps; ++t) {
124
+ const int64_t index = (b * steps + t) * width + d;
125
+ const double primitive = table.data()[d * table.shape(1) + cells.data()[index]];
126
+ change_sum += primitive;
127
+ const double velocity = change_sum + boundary;
128
+ position += velocity;
129
+ const double value = input_order == 2 ? primitive : input_order == 1 ? velocity : position;
130
+ target[index] = static_cast<float>(value / scale.data()[d]);
131
+ }
132
+ }
133
+ });
134
+ return output;
135
+ }
136
+ }
137
+
138
+ void register_reconstruction(py::module_& module) {
139
+ module.def("so3_reconstruct", &so3_reconstruct, py::arg("table"), py::arg("bins"),
140
+ py::arg("cells"), py::arg("previous_first_order"), py::arg("scale"),
141
+ py::arg("input_order"), py::arg("primitive_order"), py::arg("left"),
142
+ py::arg("rotation_vector"), py::arg("radius"));
143
+ module.def("additive_reconstruct", &additive_reconstruct, py::arg("table"), py::arg("bins"),
144
+ py::arg("cells"), py::arg("previous_first_order"), py::arg("scale"),
145
+ py::arg("input_order"));
146
+ }
actioncodec2_kernels/resample.cpp ADDED
@@ -0,0 +1,794 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+
4
+ #include <algorithm>
5
+ #include <array>
6
+ #include <cmath>
7
+ #include <cstdint>
8
+ #include <stdexcept>
9
+ #include <string>
10
+ #include <vector>
11
+
12
+ #include "parallel.hpp"
13
+
14
+ namespace py = pybind11;
15
+
16
+ namespace {
17
+
18
+ using Vec = std::vector<double>;
19
+
20
+ struct Quat {
21
+ double x, y, z, w;
22
+ };
23
+
24
+ Quat so3_normalize(const Quat& q) {
25
+ const double norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
26
+ if (norm < 1e-12) throw std::invalid_argument("a pose contains a zero quaternion");
27
+ return {q.x / norm, q.y / norm, q.z / norm, q.w / norm};
28
+ }
29
+
30
+ Quat so3_multiply(const Quat& a, const Quat& b) {
31
+ const double cx = a.y * b.z - a.z * b.y;
32
+ const double cy = a.z * b.x - a.x * b.z;
33
+ const double cz = a.x * b.y - a.y * b.x;
34
+ return so3_normalize({a.w * b.x + b.w * a.x + cx, a.w * b.y + b.w * a.y + cy,
35
+ a.w * b.z + b.w * a.z + cz,
36
+ a.w * b.w - (a.x * b.x + a.y * b.y + a.z * b.z)});
37
+ }
38
+
39
+ Quat so3_from_rotvec(double x, double y, double z) {
40
+ const double angle = std::sqrt(x * x + y * y + z * z);
41
+ const double scale = angle >= 1e-12 ? std::sin(0.5 * angle) / angle : 0.5;
42
+ return so3_normalize({x * scale, y * scale, z * scale, std::cos(0.5 * angle)});
43
+ }
44
+
45
+ void so3_to_rotvec(const Quat& raw, double* out) {
46
+ Quat q = so3_normalize(raw);
47
+ if (q.w < 0.0) q = {-q.x, -q.y, -q.z, -q.w};
48
+ const double vector_norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
49
+ const double angle = 2.0 * std::atan2(vector_norm, std::min(std::max(q.w, 0.0), 1.0));
50
+ const double scale = vector_norm >= 1e-12 ? angle / vector_norm : 2.0;
51
+ out[0] = q.x * scale;
52
+ out[1] = q.y * scale;
53
+ out[2] = q.z * scale;
54
+ }
55
+
56
+ Quat sp_from_quat(const Quat& q) {
57
+ const double norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
58
+ if (!(norm > 0.0)) throw std::invalid_argument("found zero norm quaternions in `quat`");
59
+ return {q.x / norm, q.y / norm, q.z / norm, q.w / norm};
60
+ }
61
+
62
+ Quat sp_inv(const Quat& q) { return {-q.x, -q.y, -q.z, q.w}; }
63
+
64
+ Quat sp_compose(const Quat& p, const Quat& q) {
65
+ const double cx = p.y * q.z - p.z * q.y;
66
+ const double cy = p.z * q.x - p.x * q.z;
67
+ const double cz = p.x * q.y - p.y * q.x;
68
+ const Quat r{p.w * q.x + q.w * p.x + cx, p.w * q.y + q.w * p.y + cy, p.w * q.z + q.w * p.z + cz,
69
+ p.w * q.w - (p.x * q.x + p.y * q.y + p.z * q.z)};
70
+ return sp_from_quat(r);
71
+ }
72
+
73
+ void sp_as_rotvec(Quat q, double* out) {
74
+ if (q.w < 0.0) q = {-q.x, -q.y, -q.z, -q.w};
75
+ const double angle = 2.0 * std::atan2(std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z), q.w);
76
+ double scale;
77
+ if (angle <= 1e-3) {
78
+ const double angle2 = angle * angle;
79
+ scale = 2.0 + angle2 / 12.0 + 7.0 * angle2 * angle2 / 2880.0;
80
+ } else {
81
+ scale = angle / std::sin(angle / 2.0);
82
+ }
83
+ out[0] = scale * q.x;
84
+ out[1] = scale * q.y;
85
+ out[2] = scale * q.z;
86
+ }
87
+
88
+ Quat sp_from_rotvec(const double* v) {
89
+ const double angle = std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
90
+ double scale;
91
+ if (angle <= 1e-3) {
92
+ const double angle2 = angle * angle;
93
+ scale = 0.5 - angle2 / 48.0 + angle2 * angle2 / 3840.0;
94
+ } else {
95
+ scale = std::sin(angle / 2.0) / angle;
96
+ }
97
+ return {scale * v[0], scale * v[1], scale * v[2], std::cos(angle / 2.0)};
98
+ }
99
+
100
+ int64_t ppoly_interval(const Vec& x, double t) {
101
+ const int64_t n = static_cast<int64_t>(x.size());
102
+ if (t >= x[n - 1]) return n - 2;
103
+ if (t < x[0]) return 0;
104
+ return static_cast<int64_t>(std::upper_bound(x.begin(), x.end(), t) - x.begin()) - 1;
105
+ }
106
+
107
+ double ppoly_value(const double* c, double s) {
108
+ double result = 0.0, z = 1.0;
109
+ result = result + c[3] * z;
110
+ z *= s;
111
+ result = result + c[2] * z;
112
+ z *= s;
113
+ result = result + c[1] * z;
114
+ z *= s;
115
+ result = result + c[0] * z;
116
+ return result;
117
+ }
118
+
119
+ Vec hermite_coefficients(const Vec& x, const Vec& y, const Vec& slopes, int64_t cols) {
120
+ const int64_t n = static_cast<int64_t>(x.size());
121
+ Vec c(static_cast<size_t>((n - 1) * 4 * cols));
122
+ for (int64_t k = 0; k < n - 1; ++k) {
123
+ const double dx = x[k + 1] - x[k];
124
+ for (int64_t a = 0; a < cols; ++a) {
125
+ const double y0 = y[k * cols + a], y1 = y[(k + 1) * cols + a];
126
+ const double d0 = slopes[k * cols + a], d1 = slopes[(k + 1) * cols + a];
127
+ const double slope = (y1 - y0) / dx;
128
+ const double t = (d0 + d1 - 2.0 * slope) / dx;
129
+ c[(k * 4 + 0) * cols + a] = t / dx;
130
+ c[(k * 4 + 1) * cols + a] = (slope - d0) / dx - t;
131
+ c[(k * 4 + 2) * cols + a] = d0;
132
+ c[(k * 4 + 3) * cols + a] = y0;
133
+ }
134
+ }
135
+ return c;
136
+ }
137
+
138
+ void ppoly_evaluate(const Vec& x, const Vec& c, int64_t cols, const Vec& times, int64_t out_cols,
139
+ int64_t col_offset, double* out) {
140
+ for (size_t i = 0; i < times.size(); ++i) {
141
+ const int64_t k = ppoly_interval(x, times[i]);
142
+ const double s = times[i] - x[k];
143
+ for (int64_t a = 0; a < cols; ++a) {
144
+ const double coeff[4] = {c[(k * 4 + 0) * cols + a], c[(k * 4 + 1) * cols + a],
145
+ c[(k * 4 + 2) * cols + a], c[(k * 4 + 3) * cols + a]};
146
+ out[i * out_cols + col_offset + a] = ppoly_value(coeff, s);
147
+ }
148
+ }
149
+ }
150
+
151
+ void gtsv(Vec dl, Vec d, Vec du, Vec& b, int64_t cols) {
152
+ const int64_t n = static_cast<int64_t>(d.size());
153
+ auto pivot = [&](int64_t i, bool last) {
154
+ if (std::fabs(d[i]) >= std::fabs(dl[i])) {
155
+ if (d[i] == 0.0) throw std::invalid_argument("singular matrix");
156
+ const double fact = dl[i] / d[i];
157
+ d[i + 1] = d[i + 1] - fact * du[i];
158
+ for (int64_t j = 0; j < cols; ++j) b[(i + 1) * cols + j] = b[(i + 1) * cols + j] - fact * b[i * cols + j];
159
+ if (!last) dl[i] = 0.0;
160
+ } else {
161
+ const double fact = d[i] / dl[i];
162
+ d[i] = dl[i];
163
+ const double temp = d[i + 1];
164
+ d[i + 1] = du[i] - fact * temp;
165
+ if (!last) {
166
+ dl[i] = du[i + 1];
167
+ du[i + 1] = -fact * dl[i];
168
+ }
169
+ du[i] = temp;
170
+ for (int64_t j = 0; j < cols; ++j) {
171
+ const double swap = b[i * cols + j];
172
+ b[i * cols + j] = b[(i + 1) * cols + j];
173
+ b[(i + 1) * cols + j] = swap - fact * b[(i + 1) * cols + j];
174
+ }
175
+ }
176
+ };
177
+ for (int64_t i = 0; i + 2 < n; ++i) pivot(i, false);
178
+ if (n > 1) pivot(n - 2, true);
179
+ if (d[n - 1] == 0.0) throw std::invalid_argument("singular matrix");
180
+ for (int64_t j = 0; j < cols; ++j) {
181
+ b[(n - 1) * cols + j] = b[(n - 1) * cols + j] / d[n - 1];
182
+ if (n > 1)
183
+ b[(n - 2) * cols + j] = (b[(n - 2) * cols + j] - du[n - 2] * b[(n - 1) * cols + j]) / d[n - 2];
184
+ for (int64_t i = n - 3; i >= 0; --i) {
185
+ b[i * cols + j] =
186
+ (b[i * cols + j] - du[i] * b[(i + 1) * cols + j] - dl[i] * b[(i + 2) * cols + j]) / d[i];
187
+ }
188
+ }
189
+ }
190
+
191
+ Vec natural_cubic_slopes(const Vec& x, const Vec& y, int64_t cols) {
192
+ const int64_t n = static_cast<int64_t>(x.size());
193
+ Vec dx(n - 1), slope((n - 1) * cols);
194
+ for (int64_t k = 0; k < n - 1; ++k) {
195
+ dx[k] = x[k + 1] - x[k];
196
+ for (int64_t a = 0; a < cols; ++a) slope[k * cols + a] = (y[(k + 1) * cols + a] - y[k * cols + a]) / dx[k];
197
+ }
198
+
199
+ Vec upper(n, 0.0), diagonal(n, 0.0), lower(n, 0.0), b(n * cols);
200
+ for (int64_t k = 1; k < n - 1; ++k) diagonal[k] = 2.0 * (dx[k - 1] + dx[k]);
201
+ for (int64_t k = 2; k < n; ++k) upper[k] = dx[k - 2];
202
+ for (int64_t k = 0; k + 2 < n; ++k) lower[k] = dx[k + 1];
203
+ for (int64_t k = 1; k < n - 1; ++k) {
204
+ for (int64_t a = 0; a < cols; ++a)
205
+ b[k * cols + a] = 3.0 * (dx[k] * slope[(k - 1) * cols + a] + dx[k - 1] * slope[k * cols + a]);
206
+ }
207
+
208
+ diagonal[0] = 2.0 * dx[0];
209
+ upper[1] = dx[0];
210
+ diagonal[n - 1] = 2.0 * dx[n - 2];
211
+ lower[n - 2] = dx[n - 2];
212
+ for (int64_t a = 0; a < cols; ++a) {
213
+ b[a] = -0.5 * 0.0 * (dx[0] * dx[0]) + 3.0 * (y[cols + a] - y[a]);
214
+ b[(n - 1) * cols + a] =
215
+ 0.5 * 0.0 * (dx[n - 2] * dx[n - 2]) + 3.0 * (y[(n - 1) * cols + a] - y[(n - 2) * cols + a]);
216
+ }
217
+ Vec dl(lower.begin(), lower.end() - 1), du(upper.begin() + 1, upper.end());
218
+ gtsv(dl, diagonal, du, b, cols);
219
+ return b;
220
+ }
221
+
222
+ double sign(double v) { return v > 0.0 ? 1.0 : (v < 0.0 ? -1.0 : (v == 0.0 ? 0.0 : v)); }
223
+
224
+ double pchip_edge(double h0, double h1, double m0, double m1) {
225
+ double d = ((2.0 * h0 + h1) * m0 - h0 * m1) / (h0 + h1);
226
+ const bool mask = sign(d) != sign(m0);
227
+ const bool mask2 = (sign(m0) != sign(m1)) && (std::fabs(d) > 3.0 * std::fabs(m0));
228
+ if (mask) d = 0.0;
229
+ else if (mask2) d = 3.0 * m0;
230
+ return d;
231
+ }
232
+
233
+ Vec pchip_slopes(const Vec& x, const Vec& y, int64_t cols) {
234
+ const int64_t n = static_cast<int64_t>(x.size());
235
+ Vec h(n - 1), m((n - 1) * cols), dk(n * cols, 0.0);
236
+ for (int64_t k = 0; k < n - 1; ++k) {
237
+ h[k] = x[k + 1] - x[k];
238
+ for (int64_t a = 0; a < cols; ++a) m[k * cols + a] = (y[(k + 1) * cols + a] - y[k * cols + a]) / h[k];
239
+ }
240
+ if (n == 2) {
241
+ for (int64_t a = 0; a < cols; ++a) dk[a] = dk[cols + a] = m[a];
242
+ return dk;
243
+ }
244
+ for (int64_t k = 1; k < n - 1; ++k) {
245
+ const double w1 = 2.0 * h[k] + h[k - 1];
246
+ const double w2 = h[k] + 2.0 * h[k - 1];
247
+ for (int64_t a = 0; a < cols; ++a) {
248
+ const double left = m[(k - 1) * cols + a], right = m[k * cols + a];
249
+ const bool condition = sign(right) != sign(left) || right == 0.0 || left == 0.0;
250
+ if (condition) {
251
+ dk[k * cols + a] = 0.0;
252
+ } else {
253
+ const double whmean = (w1 / left + w2 / right) / (w1 + w2);
254
+ dk[k * cols + a] = 1.0 / whmean;
255
+ }
256
+ }
257
+ }
258
+ for (int64_t a = 0; a < cols; ++a) {
259
+ dk[a] = pchip_edge(h[0], h[1], m[a], m[cols + a]);
260
+ dk[(n - 1) * cols + a] = pchip_edge(h[n - 2], h[n - 3], m[(n - 2) * cols + a], m[(n - 3) * cols + a]);
261
+ }
262
+ return dk;
263
+ }
264
+
265
+ using Mat3 = std::array<double, 9>;
266
+
267
+ Mat3 skew(const double* v) { return {0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0}; }
268
+
269
+ Mat3 matmul(const Mat3& a, const Mat3& b) {
270
+ Mat3 r{};
271
+ for (int i = 0; i < 3; ++i)
272
+ for (int j = 0; j < 3; ++j)
273
+ r[i * 3 + j] = a[i * 3 + 0] * b[0 * 3 + j] + a[i * 3 + 1] * b[1 * 3 + j] + a[i * 3 + 2] * b[2 * 3 + j];
274
+ return r;
275
+ }
276
+
277
+ void matvec(const Mat3& a, const double* v, double* out) {
278
+ for (int i = 0; i < 3; ++i) out[i] = a[i * 3 + 0] * v[0] + a[i * 3 + 1] * v[1] + a[i * 3 + 2] * v[2];
279
+ }
280
+
281
+ double norm3(const double* v) { return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); }
282
+
283
+ void cross(const double* a, const double* b, double* out) {
284
+ out[0] = a[1] * b[2] - a[2] * b[1];
285
+ out[1] = a[2] * b[0] - a[0] * b[2];
286
+ out[2] = a[0] * b[1] - a[1] * b[0];
287
+ }
288
+
289
+ Mat3 rate_to_rotvec_dot(const double* rotvec) {
290
+ const double nm = norm3(rotvec);
291
+ const double k = nm > 1e-4 ? (1.0 - 0.5 * nm / std::tan(0.5 * nm)) / (nm * nm)
292
+ : 1.0 / 12.0 + 1.0 / 720.0 * (nm * nm);
293
+ const Mat3 s = skew(rotvec), ss = matmul(s, s);
294
+ Mat3 r{};
295
+ for (int i = 0; i < 9; ++i) r[i] = ((i % 4 == 0 ? 1.0 : 0.0) + 0.5 * s[i]) + k * ss[i];
296
+ return r;
297
+ }
298
+
299
+ Mat3 rotvec_dot_to_rate(const double* rotvec) {
300
+ const double nm = norm3(rotvec);
301
+ double k1, k2;
302
+ if (nm > 1e-4) {
303
+ k1 = (1.0 - std::cos(nm)) / (nm * nm);
304
+ k2 = (nm - std::sin(nm)) / std::pow(nm, 3.0);
305
+ } else {
306
+ k1 = 0.5 - (nm * nm) / 24.0;
307
+ k2 = 1.0 / 6.0 - (nm * nm) / 120.0;
308
+ }
309
+ const Mat3 s = skew(rotvec), ss = matmul(s, s);
310
+ Mat3 r{};
311
+ for (int i = 0; i < 9; ++i) r[i] = ((i % 4 == 0 ? 1.0 : 0.0) - k1 * s[i]) + k2 * ss[i];
312
+ return r;
313
+ }
314
+
315
+ void nonlinear_term(const double* rv, const double* rvd, double* out) {
316
+ const double nm = norm3(rv);
317
+ const double dp = rv[0] * rvd[0] + rv[1] * rvd[1] + rv[2] * rvd[2];
318
+ double cp[3], ccp[3], dccp[3];
319
+ cross(rv, rvd, cp);
320
+ cross(rv, cp, ccp);
321
+ cross(rvd, cp, dccp);
322
+ double k1, k2, k3;
323
+ if (nm > 1e-4) {
324
+ const double sn = std::sin(nm), cs = std::cos(nm);
325
+ k1 = (-nm * sn - 2.0 * (cs - 1.0)) / std::pow(nm, 4.0);
326
+ k2 = (-2.0 * nm + 3.0 * sn - nm * cs) / std::pow(nm, 5.0);
327
+ k3 = (nm - sn) / std::pow(nm, 3.0);
328
+ } else {
329
+ k1 = 1.0 / 12.0 - (nm * nm) / 180.0;
330
+ k2 = -1.0 / 60.0 + (nm * nm) / 12604.0;
331
+ k3 = 1.0 / 6.0 - (nm * nm) / 120.0;
332
+ }
333
+ for (int i = 0; i < 3; ++i) out[i] = dp * (k1 * cp[i] + k2 * ccp[i]) + k3 * dccp[i];
334
+ }
335
+
336
+ void gbsv(int64_t n, int64_t kl, int64_t ku, Vec& ab, Vec& b) {
337
+ const int64_t ldab = 2 * kl + ku + 1, kv = ku + kl;
338
+ auto at = [&](int64_t row, int64_t col) -> double& { return ab[row + ldab * col]; };
339
+ std::vector<int64_t> pivots(n);
340
+ int64_t ju = 0;
341
+ for (int64_t j = 0; j < n; ++j) {
342
+ const int64_t km = std::min(kl, n - 1 - j);
343
+ int64_t jp = 0;
344
+ double best = std::fabs(at(kv, j));
345
+ for (int64_t i = 1; i <= km; ++i) {
346
+ if (std::fabs(at(kv + i, j)) > best) { best = std::fabs(at(kv + i, j)); jp = i; }
347
+ }
348
+ pivots[j] = jp + j;
349
+ if (at(kv + jp, j) == 0.0) throw std::invalid_argument("singular matrix");
350
+ ju = std::max(ju, std::min(j + ku + jp, n - 1));
351
+ if (jp != 0) {
352
+ for (int64_t c = 0; c <= ju - j; ++c) std::swap(at(kv + jp - c, j + c), at(kv - c, j + c));
353
+ }
354
+ if (km > 0) {
355
+ const double reciprocal = 1.0 / at(kv, j);
356
+ for (int64_t i = 1; i <= km; ++i) at(kv + i, j) = reciprocal * at(kv + i, j);
357
+ for (int64_t c = 1; c <= ju - j; ++c) {
358
+ const double y = at(kv - c, j + c);
359
+ if (y != 0.0) {
360
+ const double temp = -1.0 * y;
361
+ for (int64_t i = 1; i <= km; ++i) at(kv - c + i, j + c) = at(kv - c + i, j + c) + at(kv + i, j) * temp;
362
+ }
363
+ }
364
+ }
365
+ }
366
+
367
+ for (int64_t j = 0; j + 1 < n; ++j) {
368
+ const int64_t lm = std::min(kl, n - 1 - j);
369
+ if (pivots[j] != j) std::swap(b[pivots[j]], b[j]);
370
+ if (b[j] != 0.0) {
371
+ const double temp = -1.0 * b[j];
372
+ for (int64_t i = 1; i <= lm; ++i) b[j + i] = b[j + i] + at(kv + i, j) * temp;
373
+ }
374
+ }
375
+
376
+ const int64_t k = kl + ku;
377
+ for (int64_t j = n - 1; j >= 0; --j) {
378
+ if (b[j] != 0.0) {
379
+ b[j] = b[j] / at(k, j);
380
+ const double temp = b[j];
381
+ for (int64_t i = j - 1; i >= std::max<int64_t>(0, j - k); --i) b[i] = b[i] - temp * at(k + i - j, j);
382
+ }
383
+ }
384
+ }
385
+
386
+ struct RotationSpline {
387
+ Vec times;
388
+ std::vector<Quat> rotations;
389
+ Vec coefficients;
390
+
391
+ RotationSpline(const Vec& knots, const std::vector<Quat>& raw) : times(knots) {
392
+ const int64_t n = static_cast<int64_t>(knots.size());
393
+ rotations.reserve(n);
394
+ for (const Quat& q : raw) rotations.push_back(sp_from_quat(q));
395
+ const int64_t m = n - 1;
396
+ Vec dt(m), rotvecs(m * 3), rates(m * 3), rotvecs_dot;
397
+ for (int64_t k = 0; k < m; ++k) {
398
+ dt[k] = times[k + 1] - times[k];
399
+ sp_as_rotvec(sp_compose(sp_inv(rotations[k]), rotations[k + 1]), &rotvecs[k * 3]);
400
+ for (int a = 0; a < 3; ++a) rates[k * 3 + a] = rotvecs[k * 3 + a] / dt[k];
401
+ }
402
+ if (n == 2) {
403
+ rotvecs_dot = rates;
404
+ } else {
405
+ solve_rates(dt, rates, rotvecs, rotvecs_dot);
406
+ }
407
+ coefficients.assign(m * 4 * 3, 0.0);
408
+ for (int64_t k = 0; k < m; ++k) {
409
+ for (int a = 0; a < 3; ++a) {
410
+ const double rv = rotvecs[k * 3 + a], ar = rates[k * 3 + a], rd = rotvecs_dot[k * 3 + a];
411
+ coefficients[(k * 4 + 0) * 3 + a] = (-2.0 * rv + dt[k] * ar + dt[k] * rd) / std::pow(dt[k], 3.0);
412
+ coefficients[(k * 4 + 1) * 3 + a] = (3.0 * rv - 2.0 * dt[k] * ar - dt[k] * rd) / (dt[k] * dt[k]);
413
+ coefficients[(k * 4 + 2) * 3 + a] = ar;
414
+ coefficients[(k * 4 + 3) * 3 + a] = 0.0;
415
+ }
416
+ }
417
+ }
418
+
419
+
420
+
421
+ static void solve_rates(const Vec& dt, Vec& rates, const Vec& rotvecs, Vec& rotvecs_dot) {
422
+ const int64_t m = static_cast<int64_t>(dt.size());
423
+ const double first[3] = {rates[0], rates[1], rates[2]};
424
+ std::vector<Mat3> a(m), a_inv(m);
425
+ for (int64_t k = 0; k < m; ++k) {
426
+ a[k] = rate_to_rotvec_dot(&rotvecs[k * 3]);
427
+ a_inv[k] = rotvec_dot_to_rate(&rotvecs[k * 3]);
428
+ }
429
+
430
+ const int64_t size = 3 * (m - 1), kl = 5, ku = 5, ldab = 2 * kl + ku + 1;
431
+ Vec band(ldab * size, 0.0);
432
+ auto set = [&](int64_t i, int64_t j, double value) { band[(kl + ku + i - j) + ldab * j] = value; };
433
+ for (int64_t k = 0; k < m - 2; ++k) {
434
+ for (int r = 0; r < 3; ++r) {
435
+ for (int c = 0; c < 3; ++c) {
436
+ set(3 * (k + 1) + r, 3 * k + c, 2.0 * a_inv[k + 1][r * 3 + c] / dt[k + 1]);
437
+ set(3 * k + r, 3 * (k + 1) + c, 2.0 * a[k + 1][r * 3 + c] / dt[k + 1]);
438
+ }
439
+ }
440
+ }
441
+ for (int64_t k = 0; k < m - 1; ++k) {
442
+ const double diagonal = 4.0 * (1.0 / dt[k] + 1.0 / dt[k + 1]);
443
+ for (int r = 0; r < 3; ++r) set(3 * k + r, 3 * k + r, diagonal);
444
+ }
445
+ Vec b0(size);
446
+ for (int64_t k = 0; k < m - 1; ++k) {
447
+ for (int r = 0; r < 3; ++r)
448
+ b0[3 * k + r] = 6.0 * (rotvecs[k * 3 + r] * std::pow(dt[k], -2.0) +
449
+ rotvecs[(k + 1) * 3 + r] * std::pow(dt[k + 1], -2.0));
450
+ }
451
+ double product[3];
452
+ matvec(a_inv[0], first, product);
453
+ for (int r = 0; r < 3; ++r) b0[r] -= 2.0 / dt[0] * product[r];
454
+ matvec(a[m - 1], &rates[(m - 1) * 3], product);
455
+ for (int r = 0; r < 3; ++r) b0[size - 3 + r] -= 2.0 / dt[m - 1] * product[r];
456
+
457
+ rotvecs_dot.assign(m * 3, 0.0);
458
+ for (int iteration = 0; iteration < 10; ++iteration) {
459
+ for (int64_t k = 0; k < m; ++k) matvec(a[k], &rates[k * 3], &rotvecs_dot[k * 3]);
460
+ Vec b(size);
461
+ for (int64_t k = 0; k < m - 1; ++k) {
462
+ double delta_beta[3];
463
+ nonlinear_term(&rotvecs[k * 3], &rotvecs_dot[k * 3], delta_beta);
464
+ for (int r = 0; r < 3; ++r) b[3 * k + r] = b0[3 * k + r] - delta_beta[r];
465
+ }
466
+ Vec work = band;
467
+ gbsv(size, kl, ku, work, b);
468
+ bool converged = true;
469
+ for (int64_t i = 0; i < size; ++i) {
470
+ const double delta = std::fabs(b[i] - rates[i]);
471
+ rates[i] = b[i];
472
+ if (!(delta < 1e-9 * (1.0 + std::fabs(b[i])))) converged = false;
473
+ }
474
+ if (converged) break;
475
+ }
476
+ for (int64_t k = 0; k < m; ++k) matvec(a[k], &rates[k * 3], &rotvecs_dot[k * 3]);
477
+
478
+ Vec shifted(m * 3);
479
+ for (int r = 0; r < 3; ++r) shifted[r] = first[r];
480
+ for (int64_t i = 0; i < 3 * (m - 1); ++i) shifted[3 + i] = rates[i];
481
+ rates = shifted;
482
+ }
483
+
484
+ Quat operator()(double t) const {
485
+ const int64_t m = static_cast<int64_t>(times.size()) - 1;
486
+ const int64_t k = ppoly_interval(times, t);
487
+ const double s = t - times[k];
488
+ double rotvec[3];
489
+ for (int a = 0; a < 3; ++a) {
490
+ const double coeff[4] = {coefficients[(k * 4 + 0) * 3 + a], coefficients[(k * 4 + 1) * 3 + a],
491
+ coefficients[(k * 4 + 2) * 3 + a], coefficients[(k * 4 + 3) * 3 + a]};
492
+ rotvec[a] = ppoly_value(coeff, s);
493
+ }
494
+ int64_t index = static_cast<int64_t>(std::upper_bound(times.begin(), times.end(), t) - times.begin()) - 1;
495
+ index = std::min(std::max<int64_t>(index, 0), m - 1);
496
+ return sp_compose(rotations[index], sp_from_rotvec(rotvec));
497
+ }
498
+ };
499
+
500
+ Quat slerp(const Quat& left, const Quat& right, double weight) {
501
+ const Quat first = so3_normalize(left);
502
+ Quat second = so3_normalize(right);
503
+ double dot = first.x * second.x + first.y * second.y + first.z * second.z + first.w * second.w;
504
+ if (dot < 0.0) second = {-second.x, -second.y, -second.z, -second.w};
505
+ dot = std::min(std::max(std::fabs(dot), 0.0), 1.0);
506
+ Quat out;
507
+ if (dot > 0.9995) {
508
+ out = {first.x + weight * (second.x - first.x), first.y + weight * (second.y - first.y),
509
+ first.z + weight * (second.z - first.z), first.w + weight * (second.w - first.w)};
510
+ } else {
511
+ const double angle = std::acos(dot), denominator = std::sin(angle);
512
+ const double a = std::sin((1.0 - weight) * angle) / denominator, b = std::sin(weight * angle) / denominator;
513
+ out = {a * first.x + b * second.x, a * first.y + b * second.y, a * first.z + b * second.z,
514
+ a * first.w + b * second.w};
515
+ }
516
+ return so3_normalize(out);
517
+ }
518
+
519
+ enum Kind { kLinear = 0, kHold = 1, kPose = 2 };
520
+ enum Method { kLinearMethod = 0, kPchip = 1, kCubic = 2 };
521
+
522
+ int64_t resampled_horizon(int64_t length, double source_hz, double target_hz) {
523
+ return std::max<int64_t>(1, static_cast<int64_t>(std::floor(length * target_hz / source_hz + 0.5)));
524
+ }
525
+
526
+ int64_t output_width(int kind, int64_t width) { return kind == kPose ? 7 : width; }
527
+
528
+ void resample_row(const float* input, int64_t steps, int64_t width, double source_hz, double target_hz,
529
+ int kind, int64_t count, bool identity, int method, float* output) {
530
+ Vec values(steps * width);
531
+ for (int64_t i = 0; i < steps * width; ++i) values[i] = input[i];
532
+ const int64_t out_width = output_width(kind, width);
533
+ if (identity) {
534
+ for (int64_t i = 0; i < steps * width; ++i) output[i] = static_cast<float>(values[i]);
535
+ return;
536
+ }
537
+ Vec target_time(count), source_time(steps), interpolation_time(count);
538
+ for (int64_t i = 0; i < count; ++i) target_time[i] = static_cast<double>(i) / target_hz;
539
+ for (int64_t i = 0; i < steps; ++i) source_time[i] = static_cast<double>(i) / source_hz;
540
+ std::vector<int64_t> left(count), right(count);
541
+ Vec weight(count);
542
+ for (int64_t i = 0; i < count; ++i) {
543
+ interpolation_time[i] = std::min(target_time[i], source_time[steps - 1]);
544
+ const double position = target_time[i] * source_hz;
545
+ left[i] = std::min(static_cast<int64_t>(std::floor(position)), steps - 1);
546
+ right[i] = std::min(left[i] + 1, steps - 1);
547
+ weight[i] = position - static_cast<double>(left[i]);
548
+ }
549
+ Vec result(count * out_width);
550
+ if (kind == kHold) {
551
+ for (int64_t i = 0; i < count; ++i)
552
+ for (int64_t a = 0; a < width; ++a) result[i * out_width + a] = values[left[i] * width + a];
553
+ } else {
554
+ const bool pose = kind == kPose;
555
+ const int64_t cols = pose ? 3 : width;
556
+ Vec continuous(steps * cols);
557
+ for (int64_t t = 0; t < steps; ++t)
558
+ for (int64_t a = 0; a < cols; ++a) continuous[t * cols + a] = values[t * width + a];
559
+ if (steps == 1) {
560
+ for (int64_t i = 0; i < count; ++i)
561
+ for (int64_t a = 0; a < cols; ++a) result[i * out_width + a] = continuous[a];
562
+ } else if (method == kPchip) {
563
+ const Vec c = hermite_coefficients(source_time, continuous, pchip_slopes(source_time, continuous, cols), cols);
564
+ ppoly_evaluate(source_time, c, cols, interpolation_time, out_width, 0, result.data());
565
+ } else if (method == kCubic) {
566
+ const Vec c = hermite_coefficients(source_time, continuous, natural_cubic_slopes(source_time, continuous, cols), cols);
567
+ ppoly_evaluate(source_time, c, cols, interpolation_time, out_width, 0, result.data());
568
+
569
+ std::vector<int64_t> fallback;
570
+ for (int64_t a = 0; a < cols; ++a) {
571
+ double lower = continuous[a], upper = continuous[a];
572
+ for (int64_t t = 1; t < steps; ++t) {
573
+ lower = std::min(lower, continuous[t * cols + a]);
574
+ upper = std::max(upper, continuous[t * cols + a]);
575
+ }
576
+ const double tolerance = 1e-9 + 0.05 * (upper - lower);
577
+ for (int64_t i = 0; i < count; ++i) {
578
+ const double v = result[i * out_width + a];
579
+ if (v < lower - tolerance || v > upper + tolerance) { fallback.push_back(a); break; }
580
+ }
581
+ }
582
+ if (!fallback.empty()) {
583
+ const int64_t f = static_cast<int64_t>(fallback.size());
584
+ Vec subset(steps * f);
585
+ for (int64_t t = 0; t < steps; ++t)
586
+ for (int64_t j = 0; j < f; ++j) subset[t * f + j] = continuous[t * cols + fallback[j]];
587
+ const Vec c = hermite_coefficients(source_time, subset, pchip_slopes(source_time, subset, f), f);
588
+ Vec sampled(count * f);
589
+ ppoly_evaluate(source_time, c, f, interpolation_time, f, 0, sampled.data());
590
+ for (int64_t i = 0; i < count; ++i)
591
+ for (int64_t j = 0; j < f; ++j) result[i * out_width + fallback[j]] = sampled[i * f + j];
592
+ }
593
+ } else {
594
+ for (int64_t i = 0; i < count; ++i)
595
+ for (int64_t a = 0; a < cols; ++a) {
596
+ const double lo = values[left[i] * width + a], hi = values[right[i] * width + a];
597
+ result[i * out_width + a] = lo + weight[i] * (hi - lo);
598
+ }
599
+ }
600
+ if (pose) {
601
+ std::vector<Quat> quaternions(steps);
602
+ for (int64_t t = 0; t < steps; ++t)
603
+ quaternions[t] = {values[t * width + 3], values[t * width + 4], values[t * width + 5], values[t * width + 6]};
604
+ auto store = [&](int64_t i, const Quat& q) {
605
+ result[i * out_width + 3] = q.x;
606
+ result[i * out_width + 4] = q.y;
607
+ result[i * out_width + 5] = q.z;
608
+ result[i * out_width + 6] = q.w;
609
+ };
610
+ if (steps == 1) {
611
+ for (int64_t i = 0; i < count; ++i) store(i, quaternions[0]);
612
+ } else if (method == kLinearMethod) {
613
+ for (int64_t i = 0; i < count; ++i) store(i, slerp(quaternions[left[i]], quaternions[right[i]], weight[i]));
614
+ } else {
615
+ const RotationSpline spline(source_time, quaternions);
616
+ for (int64_t i = 0; i < count; ++i) store(i, spline(interpolation_time[i]));
617
+ }
618
+ }
619
+ }
620
+ for (int64_t i = 0; i < count * out_width; ++i) output[i] = static_cast<float>(result[i]);
621
+ }
622
+
623
+ int parse_kind(const std::string& kind) {
624
+ if (kind == "linear") return kLinear;
625
+ if (kind == "hold") return kHold;
626
+ if (kind == "pose_xyzw") return kPose;
627
+ throw std::invalid_argument("unsupported resampling kind");
628
+ }
629
+
630
+ int parse_method(const std::string& method) {
631
+ if (method == "linear") return kLinearMethod;
632
+ if (method == "pchip") return kPchip;
633
+ if (method == "cubic") return kCubic;
634
+ throw std::invalid_argument("method must be one of ['cubic', 'linear', 'pchip']");
635
+ }
636
+
637
+ using F32 = py::array_t<float, py::array::c_style | py::array::forcecast>;
638
+
639
+ void check_finite(const F32& values) {
640
+ const float* data = values.data();
641
+ for (py::ssize_t i = 0; i < values.size(); ++i)
642
+ if (!std::isfinite(data[i])) throw std::invalid_argument("values must be a non-empty finite [T, D] array");
643
+ }
644
+
645
+ F32 resample_uniform_batch(const F32& values, double source_hz, double target_hz, const std::string& kind_name,
646
+ int64_t target_length, const std::string& method_name, int threads) {
647
+ if (values.ndim() != 3 || values.shape(0) < 1 || values.shape(1) < 1)
648
+ throw std::invalid_argument("values must have non-empty shape [B, T, D]");
649
+ if (!(std::isfinite(source_hz) && source_hz > 0.0)) throw std::invalid_argument("source_hz must be positive and finite");
650
+ if (!(std::isfinite(target_hz) && target_hz > 0.0)) throw std::invalid_argument("target_hz must be positive and finite");
651
+ check_finite(values);
652
+ const int kind = parse_kind(kind_name), method = parse_method(method_name);
653
+ const int64_t batch = values.shape(0), steps = values.shape(1), width = values.shape(2);
654
+ if (kind == kPose && width != 7) throw std::invalid_argument("pose_xyzw values must have shape [T, 7]");
655
+ const int64_t standard = resampled_horizon(steps, source_hz, target_hz);
656
+ const bool identity = source_hz == target_hz && (target_length < 0 || target_length == standard);
657
+ const int64_t count = identity ? steps : (target_length < 0 ? standard : target_length);
658
+ if (count < 1) throw std::invalid_argument("target_length must be a positive integer");
659
+ const int64_t out_width = output_width(kind, width);
660
+ F32 output({batch, count, out_width});
661
+ const float* in = values.data();
662
+ float* out = output.mutable_data();
663
+ {
664
+ py::gil_scoped_release release;
665
+ ac2::parallel_for(batch, threads, [&](int64_t row) {
666
+ resample_row(in + row * steps * width, steps, width, source_hz, target_hz, kind, count, identity, method,
667
+ out + row * count * out_width);
668
+ });
669
+ }
670
+ return output;
671
+ }
672
+
673
+ template <class Row>
674
+ F32 map_rows(const F32& values, int64_t in_width, int64_t out_width, int threads, Row&& row) {
675
+ if (values.ndim() != 3 || values.shape(2) != in_width || values.shape(1) < 1)
676
+ throw std::invalid_argument("expected shape [B, T, " + std::to_string(in_width) + "]");
677
+ check_finite(values);
678
+ const int64_t batch = values.shape(0), steps = values.shape(1);
679
+ F32 output({batch, steps, out_width});
680
+ const float* in = values.data();
681
+ float* out = output.mutable_data();
682
+ {
683
+ py::gil_scoped_release release;
684
+ ac2::parallel_for(batch, threads, [&](int64_t b) {
685
+ row(in + b * steps * in_width, steps, out + b * steps * out_width);
686
+ });
687
+ }
688
+ return output;
689
+ }
690
+
691
+ F32 integrate_eef_deltas_batch(const F32& deltas, int threads) {
692
+ return map_rows(deltas, 6, 7, threads, [](const float* in, int64_t steps, float* out) {
693
+ double position[3] = {0.0, 0.0, 0.0};
694
+ Quat current{0.0, 0.0, 0.0, 1.0};
695
+ for (int64_t t = 0; t < steps; ++t) {
696
+ for (int a = 0; a < 3; ++a) {
697
+ position[a] = position[a] + static_cast<double>(in[t * 6 + a]);
698
+ out[t * 7 + a] = static_cast<float>(position[a]);
699
+ }
700
+ const Quat increment = so3_from_rotvec(in[t * 6 + 3], in[t * 6 + 4], in[t * 6 + 5]);
701
+ current = so3_multiply(current, increment);
702
+ out[t * 7 + 3] = static_cast<float>(current.x);
703
+ out[t * 7 + 4] = static_cast<float>(current.y);
704
+ out[t * 7 + 5] = static_cast<float>(current.z);
705
+ out[t * 7 + 6] = static_cast<float>(current.w);
706
+ }
707
+ });
708
+ }
709
+
710
+ F32 eef_pose_from_rotvec_batch(const F32& poses, int threads) {
711
+ return map_rows(poses, 6, 7, threads, [](const float* in, int64_t steps, float* out) {
712
+ for (int64_t t = 0; t < steps; ++t) {
713
+ for (int a = 0; a < 3; ++a) out[t * 7 + a] = in[t * 6 + a];
714
+ const Quat q = so3_from_rotvec(in[t * 6 + 3], in[t * 6 + 4], in[t * 6 + 5]);
715
+ out[t * 7 + 3] = static_cast<float>(q.x);
716
+ out[t * 7 + 4] = static_cast<float>(q.y);
717
+ out[t * 7 + 5] = static_cast<float>(q.z);
718
+ out[t * 7 + 6] = static_cast<float>(q.w);
719
+ }
720
+ });
721
+ }
722
+
723
+ F32 eef_pose_absolute_batch(const F32& poses, int threads) {
724
+ return map_rows(poses, 7, 6, threads, [](const float* in, int64_t steps, float* out) {
725
+ for (int64_t t = 0; t < steps; ++t) {
726
+ for (int a = 0; a < 3; ++a) out[t * 6 + a] = in[t * 7 + a];
727
+ double rotvec[3];
728
+ so3_to_rotvec({in[t * 7 + 3], in[t * 7 + 4], in[t * 7 + 5], in[t * 7 + 6]}, rotvec);
729
+ for (int a = 0; a < 3; ++a) out[t * 6 + 3 + a] = static_cast<float>(rotvec[a]);
730
+ }
731
+ });
732
+ }
733
+
734
+ F32 eef_pose_deltas_batch(const F32& poses, int threads) {
735
+ return map_rows(poses, 7, 6, threads, [](const float* in, int64_t steps, float* out) {
736
+ Quat previous{0.0, 0.0, 0.0, 1.0};
737
+ for (int64_t t = 0; t < steps; ++t) {
738
+ for (int a = 0; a < 3; ++a) {
739
+ const double before = t == 0 ? 0.0 : static_cast<double>(in[(t - 1) * 7 + a]);
740
+ out[t * 6 + a] = static_cast<float>(static_cast<double>(in[t * 7 + a]) - before);
741
+ }
742
+ const Quat q = so3_normalize({in[t * 7 + 3], in[t * 7 + 4], in[t * 7 + 5], in[t * 7 + 6]});
743
+ double rotvec[3];
744
+ so3_to_rotvec(so3_multiply({-previous.x, -previous.y, -previous.z, previous.w}, q), rotvec);
745
+ for (int a = 0; a < 3; ++a) out[t * 6 + 3 + a] = static_cast<float>(rotvec[a]);
746
+ previous = q;
747
+ }
748
+ });
749
+ }
750
+
751
+ F32 rotvec_left_to_right_batch(const F32& increments, int threads) {
752
+ return map_rows(increments, 3, 3, threads, [](const float* in, int64_t steps, float* out) {
753
+ Quat current{0.0, 0.0, 0.0, 1.0};
754
+ for (int64_t t = 0; t < steps; ++t) {
755
+ const double v[3] = {in[t * 3], in[t * 3 + 1], in[t * 3 + 2]};
756
+ const Quat following = sp_compose(sp_from_rotvec(v), current);
757
+ double rotvec[3];
758
+ sp_as_rotvec(sp_compose(sp_inv(current), following), rotvec);
759
+ for (int a = 0; a < 3; ++a) out[t * 3 + a] = static_cast<float>(rotvec[a]);
760
+ current = following;
761
+ }
762
+ });
763
+ }
764
+
765
+ F32 rotvec_right_to_left_batch(const F32& increments, int threads) {
766
+ return map_rows(increments, 3, 3, threads, [](const float* in, int64_t steps, float* out) {
767
+ Quat current{0.0, 0.0, 0.0, 1.0};
768
+ for (int64_t t = 0; t < steps; ++t) {
769
+ const double v[3] = {in[t * 3], in[t * 3 + 1], in[t * 3 + 2]};
770
+ const Quat following = sp_compose(current, sp_from_rotvec(v));
771
+ double rotvec[3];
772
+ sp_as_rotvec(sp_compose(following, sp_inv(current)), rotvec);
773
+ for (int a = 0; a < 3; ++a) out[t * 3 + a] = static_cast<float>(rotvec[a]);
774
+ current = following;
775
+ }
776
+ });
777
+ }
778
+
779
+ }
780
+
781
+ void register_resample(py::module_& module) {
782
+ module.def("resample_uniform_batch", &resample_uniform_batch, py::arg("values"), py::arg("source_hz"),
783
+ py::arg("target_hz"), py::arg("kind"), py::arg("target_length") = -1,
784
+ py::arg("method") = "linear", py::arg("threads") = 0,
785
+ "runtime resample_uniform over each [T, D] row of a [B, T, D] batch.");
786
+ module.def("integrate_eef_deltas_batch", &integrate_eef_deltas_batch, py::arg("deltas"), py::arg("threads") = 0);
787
+ module.def("eef_pose_from_rotvec_batch", &eef_pose_from_rotvec_batch, py::arg("poses"), py::arg("threads") = 0);
788
+ module.def("eef_pose_absolute_batch", &eef_pose_absolute_batch, py::arg("poses"), py::arg("threads") = 0);
789
+ module.def("eef_pose_deltas_batch", &eef_pose_deltas_batch, py::arg("poses"), py::arg("threads") = 0);
790
+ module.def("rotvec_left_to_right_batch", &rotvec_left_to_right_batch, py::arg("increments"),
791
+ py::arg("threads") = 0);
792
+ module.def("rotvec_right_to_left_batch", &rotvec_right_to_left_batch, py::arg("increments"),
793
+ py::arg("threads") = 0);
794
+ }
actioncodec2_kernels/setbpe.cpp ADDED
@@ -0,0 +1,423 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+ #include <pybind11/stl.h>
4
+
5
+ #include <algorithm>
6
+ #include <cstdint>
7
+ #include <functional>
8
+ #include <queue>
9
+ #include <stdexcept>
10
+ #include <string>
11
+ #include <tuple>
12
+ #include <unordered_map>
13
+ #include <utility>
14
+ #include <vector>
15
+
16
+ #include "parallel.hpp"
17
+
18
+ namespace py = pybind11;
19
+
20
+ namespace {
21
+
22
+ int lowest_bit(uint64_t mask) { return mask ? __builtin_ctzll(mask) : -1; }
23
+
24
+ uint64_t pair_key(int64_t left, int64_t right) {
25
+ return (static_cast<uint64_t>(left) << 32) | static_cast<uint32_t>(right);
26
+ }
27
+
28
+ struct Rule {
29
+ int64_t child;
30
+ int64_t rank;
31
+ };
32
+
33
+ class SetBPE {
34
+ public:
35
+ SetBPE(std::vector<int64_t> bins, int64_t lookahead,
36
+ const std::vector<std::pair<int64_t, int64_t>>& spatial_merges,
37
+ const std::vector<std::tuple<int64_t, int64_t, int64_t>>& temporal_merges)
38
+ : bins_(std::move(bins)), lookahead_(lookahead) {
39
+ const int64_t dimension = static_cast<int64_t>(bins_.size());
40
+ if (dimension < 1 || dimension > 63) throw std::invalid_argument("bins must have 1..63 slots");
41
+ if (lookahead_ < 1) throw std::invalid_argument("lookahead must be positive");
42
+ offsets_.assign(dimension + 1, 0);
43
+ for (int64_t d = 0; d < dimension; ++d) {
44
+ if (bins_[d] < 1) throw std::invalid_argument("every dimension needs at least one bin");
45
+ offsets_[d + 1] = offsets_[d] + bins_[d];
46
+ }
47
+ atom_count_ = offsets_[dimension];
48
+ spatial_size_ = atom_count_ + static_cast<int64_t>(spatial_merges.size());
49
+ support_.assign(spatial_size_, 0);
50
+ expansion_.assign(spatial_size_ * dimension, -1);
51
+ for (int64_t d = 0; d < dimension; ++d) {
52
+ for (int64_t v = 0; v < bins_[d]; ++v) {
53
+ const int64_t token = offsets_[d] + v;
54
+ support_[token] = uint64_t{1} << d;
55
+ expansion_[token * dimension + d] = v;
56
+ }
57
+ }
58
+ for (size_t rank = 0; rank < spatial_merges.size(); ++rank) {
59
+ const auto [left, right] = spatial_merges[rank];
60
+ const int64_t token = atom_count_ + static_cast<int64_t>(rank);
61
+ if (left < 0 || right < 0 || left >= token || right >= token)
62
+ throw std::invalid_argument("a merge rule must reference already-created tokens");
63
+ if (support_[left] & support_[right])
64
+ throw std::invalid_argument("a merge rule joins two overlapping supports");
65
+ support_[token] = support_[left] | support_[right];
66
+ for (int64_t d = 0; d < dimension; ++d) {
67
+ expansion_[token * dimension + d] =
68
+ std::max(expansion_[left * dimension + d], expansion_[right * dimension + d]);
69
+ }
70
+ spatial_rules_[pair_key(std::min(left, right), std::max(left, right))] =
71
+ Rule{token, static_cast<int64_t>(rank)};
72
+ }
73
+ spatial_anchor_.resize(spatial_size_);
74
+ for (int64_t token = 0; token < spatial_size_; ++token)
75
+ spatial_anchor_[token] = lowest_bit(support_[token]);
76
+
77
+
78
+ vocab_size_ = spatial_size_ + static_cast<int64_t>(temporal_merges.size());
79
+ patterns_.resize(vocab_size_);
80
+ root_.resize(vocab_size_);
81
+ for (int64_t token = 0; token < spatial_size_; ++token) {
82
+ patterns_[token] = {{0, token}};
83
+ root_[token] = token;
84
+ }
85
+ for (size_t rank = 0; rank < temporal_merges.size(); ++rank) {
86
+ const auto [left, right, offset] = temporal_merges[rank];
87
+ const int64_t child = spatial_size_ + static_cast<int64_t>(rank);
88
+ if (left < 0 || right < 0 || left >= child || right >= child)
89
+ throw std::invalid_argument("a temporal merge must reference already-created tokens");
90
+ if (offset < 1 || offset >= lookahead_)
91
+ throw std::invalid_argument("a temporal merge offset must be in [1, lookahead)");
92
+ std::vector<std::pair<int64_t, int64_t>> pattern = patterns_[left];
93
+ for (const auto& [time, token] : patterns_[right]) pattern.emplace_back(time + offset, token);
94
+ std::sort(pattern.begin(), pattern.end());
95
+ const int64_t span = 1 + pattern.back().first;
96
+ if (span > lookahead_) throw std::invalid_argument("temporal merge exceeds the lookahead horizon");
97
+ std::vector<uint64_t> masks(span, 0);
98
+ int roots = 0;
99
+ for (const auto& [time, token] : pattern) {
100
+ if (masks[time] & support_[token])
101
+ throw std::invalid_argument("temporal merge joins overlapping spatial supports");
102
+ masks[time] |= support_[token];
103
+ roots += time == 0;
104
+ }
105
+ if (roots != 1)
106
+ throw std::invalid_argument("a rooted temporal token must have exactly one time-zero spatial token");
107
+ patterns_[child] = std::move(pattern);
108
+ root_[child] = root_[left];
109
+ const auto key = std::make_tuple(left, right, offset);
110
+ if (temporal_rules_.count(key)) throw std::invalid_argument("duplicate temporal merge rule");
111
+ temporal_rules_[key] = Rule{child, static_cast<int64_t>(rank)};
112
+ }
113
+ }
114
+
115
+ int64_t vocab_size() const { return vocab_size_; }
116
+ int64_t dimension() const { return static_cast<int64_t>(bins_.size()); }
117
+
118
+
119
+ std::vector<int64_t> encode(const int64_t* cells, int64_t steps) const {
120
+ const int64_t dimension = static_cast<int64_t>(bins_.size());
121
+ if (steps < 1) throw std::invalid_argument("every episode must have at least one step");
122
+ uint64_t first_support = 0;
123
+ for (int64_t t = 0; t < steps; ++t) {
124
+ uint64_t support = 0;
125
+ for (int64_t d = 0; d < dimension; ++d) {
126
+ const int64_t value = cells[t * dimension + d];
127
+ if (value < -1 || value >= bins_[d])
128
+ throw std::invalid_argument("episodes must contain -1 or an in-range cell index");
129
+ if (value >= 0) support |= uint64_t{1} << d;
130
+ }
131
+ if (!support) throw std::invalid_argument("every timestep needs at least one active coordinate");
132
+ if (t == 0) first_support = support;
133
+ else if (support != first_support)
134
+ throw std::invalid_argument("active dimensions must remain constant within an episode");
135
+ }
136
+
137
+
138
+ std::vector<int64_t> tokens, anchors;
139
+ std::vector<char> active;
140
+ std::vector<std::vector<std::pair<int64_t, int64_t>>> at(steps);
141
+ for (int64_t t = 0; t < steps; ++t) {
142
+ for (int64_t token : spatial_encode(cells + t * dimension)) {
143
+ at[t].emplace_back(token, static_cast<int64_t>(tokens.size()));
144
+ tokens.push_back(token);
145
+ anchors.push_back(t);
146
+ active.push_back(1);
147
+ }
148
+ }
149
+
150
+
151
+ using Offer = std::tuple<int64_t, int64_t, int64_t, int64_t, int64_t, int64_t>;
152
+ std::priority_queue<Offer, std::vector<Offer>, std::greater<Offer>> frontier;
153
+ auto offer = [&](int64_t left_instance, int64_t right_instance) {
154
+ const auto found = temporal_rules_.find(std::make_tuple(
155
+ tokens[left_instance], tokens[right_instance],
156
+ anchors[right_instance] - anchors[left_instance]));
157
+ if (found == temporal_rules_.end()) return;
158
+ const Rule& rule = found->second;
159
+ frontier.emplace(rule.rank, anchors[left_instance],
160
+ spatial_anchor_[root_[rule.child]], left_instance, right_instance,
161
+ rule.child);
162
+ };
163
+ for (int64_t anchor = 0; anchor < steps; ++anchor) {
164
+ for (int64_t offset = 1; offset < std::min<int64_t>(lookahead_, steps - anchor); ++offset) {
165
+ for (const auto& [left_token, left_instance] : at[anchor]) {
166
+ for (const auto& [right_token, right_instance] : at[anchor + offset]) {
167
+ offer(left_instance, right_instance);
168
+ }
169
+ }
170
+ }
171
+ }
172
+ auto erase = [&](int64_t step, int64_t token) {
173
+ auto& items = at[step];
174
+ for (auto it = items.begin(); it != items.end(); ++it) {
175
+ if (it->first == token) { items.erase(it); return; }
176
+ }
177
+ };
178
+ while (!frontier.empty()) {
179
+ const auto [rank, anchor, root_anchor, left_instance, right_instance, child] = frontier.top();
180
+ frontier.pop();
181
+ (void)rank; (void)root_anchor;
182
+ if (!active[left_instance] || !active[right_instance]) continue;
183
+ if (anchors[left_instance] != anchor) continue;
184
+ const auto resolved = temporal_rules_.find(std::make_tuple(
185
+ tokens[left_instance], tokens[right_instance], anchors[right_instance] - anchor));
186
+ if (resolved == temporal_rules_.end() || resolved->second.child != child) continue;
187
+ active[left_instance] = 0;
188
+ active[right_instance] = 0;
189
+ erase(anchor, tokens[left_instance]);
190
+ erase(anchors[right_instance], tokens[right_instance]);
191
+ const int64_t new_instance = static_cast<int64_t>(tokens.size());
192
+ tokens.push_back(child);
193
+ anchors.push_back(anchor);
194
+ active.push_back(1);
195
+ at[anchor].emplace_back(child, new_instance);
196
+ for (int64_t offset = 1; offset < lookahead_; ++offset) {
197
+ const int64_t future = anchor + offset;
198
+ if (future < steps) {
199
+ for (const auto& [token, other] : at[future]) offer(new_instance, other);
200
+ }
201
+ const int64_t past = anchor - offset;
202
+ if (past >= 0) {
203
+ for (const auto& [token, other] : at[past]) offer(other, new_instance);
204
+ }
205
+ }
206
+ }
207
+
208
+ std::vector<std::tuple<int64_t, int64_t, int64_t>> ordered;
209
+ for (int64_t anchor = 0; anchor < steps; ++anchor) {
210
+ for (const auto& [token, instance] : at[anchor]) {
211
+ if (active[instance]) ordered.emplace_back(anchor, spatial_anchor_[root_[token]], token);
212
+ }
213
+ }
214
+ std::sort(ordered.begin(), ordered.end());
215
+ std::vector<int64_t> stream;
216
+ stream.reserve(ordered.size());
217
+ for (const auto& item : ordered) stream.push_back(std::get<2>(item));
218
+ return stream;
219
+ }
220
+
221
+
222
+ std::vector<int64_t> decode(const int64_t* stream, int64_t length, uint64_t full) const {
223
+ const int64_t dimension = static_cast<int64_t>(bins_.size());
224
+ std::vector<uint64_t> coverage{0};
225
+ std::vector<std::vector<int64_t>> spatial_tokens(1);
226
+ size_t front = 0;
227
+ for (int64_t index = 0; index < length; ++index) {
228
+ const int64_t token = stream[index];
229
+ if (token < 0 || token >= vocab_size_)
230
+ throw std::invalid_argument("temporal token " + std::to_string(token) + " is outside the vocabulary");
231
+ while (front < coverage.size() && coverage[front] == full) ++front;
232
+ if (front == coverage.size()) {
233
+ coverage.push_back(0);
234
+ spatial_tokens.emplace_back();
235
+ }
236
+ const uint64_t uncovered = full & ~coverage[front];
237
+ if (spatial_anchor_[root_[token]] != lowest_bit(uncovered))
238
+ throw std::invalid_argument("temporal stream violates root order at the earliest incomplete step");
239
+ for (const auto& [offset, spatial_token] : patterns_[token]) {
240
+ const size_t step = front + static_cast<size_t>(offset);
241
+ while (step >= coverage.size()) {
242
+ coverage.push_back(0);
243
+ spatial_tokens.emplace_back();
244
+ }
245
+ if (coverage[step] & support_[spatial_token])
246
+ throw std::invalid_argument("temporal stream contains overlapping spatial support");
247
+ coverage[step] |= support_[spatial_token];
248
+ spatial_tokens[step].push_back(spatial_token);
249
+ }
250
+ }
251
+ for (uint64_t mask : coverage) {
252
+ if (mask != full) throw std::invalid_argument("temporal stream ended with an incomplete control step");
253
+ }
254
+ std::vector<int64_t> cells(coverage.size() * dimension, -1);
255
+ for (size_t step = 0; step < coverage.size(); ++step) {
256
+ for (int64_t token : spatial_tokens[step]) {
257
+ for (int64_t d = 0; d < dimension; ++d) {
258
+ const int64_t value = expansion_[token * dimension + d];
259
+ if (value >= 0) cells[step * dimension + d] = value;
260
+ }
261
+ }
262
+ }
263
+ return cells;
264
+ }
265
+
266
+ private:
267
+
268
+ std::vector<int64_t> spatial_encode(const int64_t* row) const {
269
+ const int64_t dimension = static_cast<int64_t>(bins_.size());
270
+ std::vector<int64_t> present;
271
+ for (int64_t d = 0; d < dimension; ++d) {
272
+ if (row[d] >= 0) present.push_back(offsets_[d] + row[d]);
273
+ }
274
+ std::sort(present.begin(), present.end());
275
+ using Candidate = std::tuple<int64_t, int64_t, int64_t, int64_t>;
276
+ std::priority_queue<Candidate, std::vector<Candidate>, std::greater<Candidate>> heap;
277
+ for (size_t i = 0; i < present.size(); ++i) {
278
+ for (size_t j = i + 1; j < present.size(); ++j) {
279
+ const auto found = spatial_rules_.find(pair_key(present[i], present[j]));
280
+ if (found != spatial_rules_.end())
281
+ heap.emplace(found->second.rank, found->second.child, present[i], present[j]);
282
+ }
283
+ }
284
+ std::vector<int64_t> tokens = present;
285
+ auto contains = [&](int64_t token) {
286
+ return std::find(tokens.begin(), tokens.end(), token) != tokens.end();
287
+ };
288
+ while (!heap.empty()) {
289
+ const auto [rank, child, left, right] = heap.top();
290
+ heap.pop();
291
+ (void)rank;
292
+ if (!contains(left) || !contains(right)) continue;
293
+ tokens.erase(std::find(tokens.begin(), tokens.end(), left));
294
+ tokens.erase(std::find(tokens.begin(), tokens.end(), right));
295
+ for (int64_t other : tokens) {
296
+ const int64_t low = std::min(child, other), high = std::max(child, other);
297
+ const auto found = spatial_rules_.find(pair_key(low, high));
298
+ if (found != spatial_rules_.end())
299
+ heap.emplace(found->second.rank, found->second.child, low, high);
300
+ }
301
+ tokens.push_back(child);
302
+ }
303
+ std::sort(tokens.begin(), tokens.end(), [&](int64_t a, int64_t b) {
304
+ return std::make_pair(spatial_anchor_[a], a) < std::make_pair(spatial_anchor_[b], b);
305
+ });
306
+ return tokens;
307
+ }
308
+
309
+ struct TupleHash {
310
+ size_t operator()(const std::tuple<int64_t, int64_t, int64_t>& key) const {
311
+ const auto [a, b, c] = key;
312
+ return std::hash<uint64_t>()((static_cast<uint64_t>(a) * 1000003u) ^
313
+ (static_cast<uint64_t>(b) << 20) ^ static_cast<uint64_t>(c));
314
+ }
315
+ };
316
+
317
+ std::vector<int64_t> bins_;
318
+ int64_t lookahead_;
319
+ std::vector<int64_t> offsets_;
320
+ int64_t atom_count_ = 0, spatial_size_ = 0, vocab_size_ = 0;
321
+ std::vector<uint64_t> support_;
322
+ std::vector<int64_t> expansion_;
323
+ std::vector<int> spatial_anchor_;
324
+ std::unordered_map<uint64_t, Rule> spatial_rules_;
325
+ std::vector<std::vector<std::pair<int64_t, int64_t>>> patterns_;
326
+ std::vector<int64_t> root_;
327
+ std::unordered_map<std::tuple<int64_t, int64_t, int64_t>, Rule, TupleHash> temporal_rules_;
328
+ };
329
+
330
+ py::list encode_batch(const SetBPE& codec,
331
+ const py::array_t<int64_t, py::array::c_style | py::array::forcecast>& cells,
332
+ int threads) {
333
+ if (cells.ndim() != 3 || cells.shape(1) < 1 || cells.shape(2) != codec.dimension())
334
+ throw std::invalid_argument("cells must have shape [B, T, D] with the codec's dimension");
335
+ const int64_t batch = cells.shape(0), steps = cells.shape(1), width = cells.shape(2);
336
+ const int64_t* data = cells.data();
337
+ std::vector<std::vector<int64_t>> streams(batch);
338
+ {
339
+ py::gil_scoped_release release;
340
+ ac2::parallel_for(batch, threads, [&](int64_t index) {
341
+ streams[index] = codec.encode(data + index * steps * width, steps);
342
+ });
343
+ }
344
+ py::list output(batch);
345
+ for (int64_t index = 0; index < batch; ++index) output[index] = py::cast(streams[index]);
346
+ return output;
347
+ }
348
+
349
+ py::array_t<int64_t> decode_batch(const SetBPE& codec, const std::vector<std::vector<int64_t>>& streams,
350
+ int64_t width, uint64_t full, int threads) {
351
+ const int64_t batch = static_cast<int64_t>(streams.size());
352
+ if (batch < 1) throw std::invalid_argument("streams must not be empty");
353
+ if (width != codec.dimension())
354
+ throw std::invalid_argument("width must equal the codec's dimension");
355
+ const uint64_t all = (uint64_t{1} << codec.dimension()) - 1;
356
+ if (!full || (full & ~all))
357
+ throw std::invalid_argument("full_support must select existing dimensions");
358
+ std::vector<std::vector<int64_t>> cells(batch);
359
+ {
360
+ py::gil_scoped_release release;
361
+ ac2::parallel_for(batch, threads, [&](int64_t index) {
362
+ if (streams[index].empty()) throw std::invalid_argument("token stream must not be empty");
363
+ cells[index] = codec.decode(streams[index].data(), static_cast<int64_t>(streams[index].size()), full);
364
+ });
365
+ }
366
+ const int64_t steps = static_cast<int64_t>(cells[0].size()) / width;
367
+ for (const auto& row : cells) {
368
+ if (static_cast<int64_t>(row.size()) != steps * width)
369
+ throw std::invalid_argument("all token streams in a batch must decode to the same length");
370
+ }
371
+ py::array_t<int64_t> output({batch, steps, width});
372
+ int64_t* out = output.mutable_data();
373
+ for (int64_t index = 0; index < batch; ++index)
374
+ std::copy(cells[index].begin(), cells[index].end(), out + index * steps * width);
375
+ return output;
376
+ }
377
+
378
+ }
379
+
380
+ void register_setbpe(py::module_& module) {
381
+ py::class_<SetBPE>(module, "SetBPE")
382
+ .def(py::init<std::vector<int64_t>, int64_t, const std::vector<std::pair<int64_t, int64_t>>&,
383
+ const std::vector<std::tuple<int64_t, int64_t, int64_t>>&>(),
384
+ py::arg("bins"), py::arg("lookahead"), py::arg("spatial_merges"),
385
+ py::arg("temporal_merges"))
386
+ .def_property_readonly("vocab_size", &SetBPE::vocab_size)
387
+ .def("encode_batch", &encode_batch, py::arg("cells"), py::arg("threads") = 0,
388
+ "Dense [B, T, D] cells (-1 = absent) -> one token stream per row.")
389
+ .def("decode_batch", &decode_batch, py::arg("streams"), py::arg("width"),
390
+ py::arg("full_support"), py::arg("threads") = 0,
391
+ "Equal-horizon streams -> dense [B, T, D] cells, -1 outside full_support.")
392
+ .def("decode_checked_batch", [](const SetBPE& codec, const py::iterable& streams,
393
+ int64_t width, uint64_t full, int threads) {
394
+ const py::object numpy_integer = py::module_::import("numpy").attr("integer");
395
+ py::object integral;
396
+ std::vector<std::vector<int64_t>> rows;
397
+ if (PyList_Check(streams.ptr())) rows.reserve(PyList_GET_SIZE(streams.ptr()));
398
+ for (py::handle stream : streams) {
399
+ std::vector<int64_t> row;
400
+ if (PyList_Check(stream.ptr())) row.reserve(PyList_GET_SIZE(stream.ptr()));
401
+ for (py::handle raw : py::reinterpret_borrow<py::iterable>(stream)) {
402
+ bool integer = PyLong_Check(raw.ptr()) || py::isinstance(raw, numpy_integer);
403
+ if (!integer) {
404
+ if (!integral) integral = py::module_::import("numbers").attr("Integral");
405
+ integer = py::isinstance(raw, integral);
406
+ }
407
+ if (PyBool_Check(raw.ptr()) || !integer)
408
+ throw std::invalid_argument("tokens must be integers");
409
+ py::object number = PyLong_Check(raw.ptr())
410
+ ? py::reinterpret_borrow<py::object>(raw)
411
+ : py::reinterpret_steal<py::object>(PyNumber_Long(raw.ptr()));
412
+ if (!number) throw py::error_already_set();
413
+ int overflow = 0;
414
+ const int64_t token = PyLong_AsLongLongAndOverflow(number.ptr(), &overflow);
415
+ if (PyErr_Occurred()) throw py::error_already_set();
416
+ if (overflow) throw std::invalid_argument("token is outside the vocabulary");
417
+ row.push_back(token);
418
+ }
419
+ rows.push_back(std::move(row));
420
+ }
421
+ return decode_batch(codec, rows, width, full, threads);
422
+ }, py::arg("streams"), py::arg("width"), py::arg("full_support"), py::arg("threads") = 0);
423
+ }
actioncodec2_kernels/so3.cpp ADDED
@@ -0,0 +1,311 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #include <pybind11/numpy.h>
2
+ #include <pybind11/pybind11.h>
3
+
4
+ #include <algorithm>
5
+ #include <array>
6
+ #include <cmath>
7
+ #include <cstdint>
8
+ #include <limits>
9
+ #include <vector>
10
+
11
+ #include "parallel.hpp"
12
+
13
+ namespace py = pybind11;
14
+
15
+ namespace {
16
+
17
+ struct Quaternion {
18
+ double x;
19
+ double y;
20
+ double z;
21
+ double w;
22
+ };
23
+
24
+ Quaternion normalize(Quaternion q) {
25
+ const double norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
26
+ q.x /= norm;
27
+ q.y /= norm;
28
+ q.z /= norm;
29
+ q.w /= norm;
30
+ return q;
31
+ }
32
+
33
+ Quaternion multiply(const Quaternion& a, const Quaternion& b) {
34
+ return normalize({a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
35
+ a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
36
+ a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
37
+ a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z});
38
+ }
39
+
40
+ Quaternion conjugate(const Quaternion& q) { return {-q.x, -q.y, -q.z, q.w}; }
41
+
42
+ Quaternion from_euler(const float* euler) {
43
+ const double roll = 0.5 * static_cast<double>(euler[0]);
44
+ const double pitch = 0.5 * static_cast<double>(euler[1]);
45
+ const double yaw = 0.5 * static_cast<double>(euler[2]);
46
+ const double sr = std::sin(roll), cr = std::cos(roll);
47
+ const double sp = std::sin(pitch), cp = std::cos(pitch);
48
+ const double sy = std::sin(yaw), cy = std::cos(yaw);
49
+ return normalize({sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy,
50
+ cr * cp * sy - sr * sp * cy, cr * cp * cy + sr * sp * sy});
51
+ }
52
+
53
+ void to_rotvec(Quaternion q, double* output) {
54
+ if (q.w < 0.0) {
55
+ q.x = -q.x;
56
+ q.y = -q.y;
57
+ q.z = -q.z;
58
+ q.w = -q.w;
59
+ }
60
+ const double norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
61
+ const double scale = norm < 1e-12 ? 2.0 : 2.0 * std::atan2(norm, std::max(0.0, q.w)) / norm;
62
+ output[0] = q.x * scale;
63
+ output[1] = q.y * scale;
64
+ output[2] = q.z * scale;
65
+ }
66
+
67
+ Quaternion from_rotvec(const double* value) {
68
+ const double angle = std::sqrt(value[0] * value[0] + value[1] * value[1] +
69
+ value[2] * value[2]);
70
+ const double scale = angle < 1e-12 ? 0.5 : std::sin(0.5 * angle) / angle;
71
+ return normalize({value[0] * scale, value[1] * scale, value[2] * scale,
72
+ std::cos(0.5 * angle)});
73
+ }
74
+
75
+ Quaternion from_input(const float* value, bool rotation_vector) {
76
+ if (!rotation_vector) return from_euler(value);
77
+ const double converted[3] = {static_cast<double>(value[0]), static_cast<double>(value[1]),
78
+ static_cast<double>(value[2])};
79
+ return from_rotvec(converted);
80
+ }
81
+
82
+ Quaternion residual(const Quaternion& target, const Quaternion& reconstruction, bool left) {
83
+ return left ? multiply(target, conjugate(reconstruction))
84
+ : multiply(conjugate(reconstruction), target);
85
+ }
86
+
87
+ Quaternion apply(const Quaternion& state, const Quaternion& increment, bool left) {
88
+ return left ? multiply(increment, state) : multiply(state, increment);
89
+ }
90
+
91
+ std::array<double, 3> as_rotvec(const Quaternion& value) {
92
+ std::array<double, 3> output{};
93
+ to_rotvec(value, output.data());
94
+ return output;
95
+ }
96
+
97
+ struct SO3Layout {
98
+ const double* table;
99
+ const int32_t* bins;
100
+ int64_t width;
101
+ double limit;
102
+ bool smooth;
103
+ bool rotation_vector;
104
+ bool left;
105
+ int32_t input_order;
106
+ double projection_radius = std::numeric_limits<double>::infinity();
107
+ };
108
+
109
+ void project_primitive(double* value, double radius) {
110
+ if (!std::isfinite(radius)) return;
111
+ const double norm = std::sqrt(value[0]*value[0] + value[1]*value[1] + value[2]*value[2]);
112
+ if (norm > radius) {
113
+ const double scale = radius / norm;
114
+ for (int j = 0; j < 3; ++j) value[j] *= scale;
115
+ }
116
+ }
117
+
118
+ inline double primitive(const SO3Layout& layout, int axis, int32_t index) {
119
+ return layout.table[static_cast<int64_t>(axis) * layout.width + index];
120
+ }
121
+
122
+ std::array<int32_t, 3> nearest_choice(const SO3Layout& layout,
123
+ const std::array<double, 3>& request) {
124
+ std::array<int32_t, 3> choice{};
125
+ for (int axis = 0; axis < 3; ++axis) {
126
+ double best = std::numeric_limits<double>::infinity();
127
+ for (int32_t index = 0; index < layout.bins[axis]; ++index) {
128
+ const double distance = std::fabs(request[axis] - primitive(layout, axis, index));
129
+ if (distance < best) {
130
+ best = distance;
131
+ choice[axis] = index;
132
+ }
133
+ }
134
+ }
135
+ return choice;
136
+ }
137
+
138
+ std::vector<int32_t> smooth_axis(const SO3Layout& layout, int axis, double request,
139
+ int32_t nearest) {
140
+ std::vector<int32_t> output;
141
+ bool has_nearest = false;
142
+ const double radius = 2.0 * layout.limit + 1e-12;
143
+ for (int32_t index = 0; index < layout.bins[axis]; ++index) {
144
+ if (std::fabs(primitive(layout, axis, index) - request) <= radius) {
145
+ output.push_back(index);
146
+ has_nearest = has_nearest || index == nearest;
147
+ }
148
+ }
149
+ if (!has_nearest) output.push_back(nearest);
150
+ return output;
151
+ }
152
+
153
+ bool earlier(double error, const std::array<int32_t, 3>& choice, double best_error,
154
+ const std::array<int32_t, 3>& best_choice, bool found) {
155
+ return !found || error < best_error || (error == best_error && choice < best_choice);
156
+ }
157
+
158
+ std::array<int32_t, 3> smooth_choice(const SO3Layout& layout,
159
+ const std::array<double, 3>& request,
160
+ const Quaternion& reconstruction,
161
+ const Quaternion& target,
162
+ const std::array<double, 3>& direction,
163
+ const std::array<int32_t, 3>& nearest) {
164
+ const auto x = smooth_axis(layout, 0, request[0], nearest[0]);
165
+ const auto y = smooth_axis(layout, 1, request[1], nearest[1]);
166
+ const auto z = smooth_axis(layout, 2, request[2], nearest[2]);
167
+ std::array<int32_t, 3> best_any{}, best_preferred{};
168
+ double any_error = 0.0, preferred_error = 0.0;
169
+ bool found_any = false, found_preferred = false;
170
+ for (const int32_t ix : x) {
171
+ for (const int32_t iy : y) {
172
+ for (const int32_t iz : z) {
173
+ const std::array<int32_t, 3> choice{ix, iy, iz};
174
+ double vector[3] = {primitive(layout, 0, ix), primitive(layout, 1, iy),
175
+ primitive(layout, 2, iz)};
176
+ project_primitive(vector, layout.projection_radius);
177
+ const Quaternion rebuilt = apply(reconstruction, from_rotvec(vector), layout.left);
178
+ const auto remaining = as_rotvec(residual(target, rebuilt, layout.left));
179
+ const double error = std::sqrt(remaining[0] * remaining[0] + remaining[1] * remaining[1] +
180
+ remaining[2] * remaining[2]);
181
+ if (error > layout.limit + 1e-12) continue;
182
+ if (earlier(error, choice, any_error, best_any, found_any)) {
183
+ best_any = choice;
184
+ any_error = error;
185
+ found_any = true;
186
+ }
187
+ const double dot = vector[0] * direction[0] + vector[1] * direction[1] +
188
+ vector[2] * direction[2];
189
+ if (dot >= -1e-15 && earlier(error, choice, preferred_error, best_preferred,
190
+ found_preferred)) {
191
+ best_preferred = choice;
192
+ preferred_error = error;
193
+ found_preferred = true;
194
+ }
195
+ }
196
+ }
197
+ }
198
+ return found_preferred ? best_preferred : (found_any ? best_any : nearest);
199
+ }
200
+
201
+ void encode_trajectory(const SO3Layout& layout, const float* actions, int64_t steps,
202
+ int32_t* cells) {
203
+ Quaternion truth{0.0, 0.0, 0.0, 1.0};
204
+ Quaternion reconstruction = truth;
205
+ std::array<double, 3> direction{};
206
+ for (int64_t step = 0; step < steps; ++step) {
207
+ const Quaternion value = from_input(actions + 3 * step, layout.rotation_vector);
208
+ truth = layout.input_order == 0 ? value : apply(truth, value, layout.left);
209
+ const auto request = as_rotvec(residual(truth, reconstruction, layout.left));
210
+ auto choice = nearest_choice(layout, request);
211
+ if (layout.smooth) {
212
+ choice = smooth_choice(layout, request, reconstruction, truth, direction, choice);
213
+ }
214
+ const double vector[3] = {primitive(layout, 0, choice[0]),
215
+ primitive(layout, 1, choice[1]),
216
+ primitive(layout, 2, choice[2])};
217
+ reconstruction = apply(reconstruction, from_rotvec(vector), layout.left);
218
+ if (vector[0] != 0.0 || vector[1] != 0.0 || vector[2] != 0.0) {
219
+ direction = {vector[0], vector[1], vector[2]};
220
+ }
221
+ for (int axis = 0; axis < 3; ++axis) cells[3 * step + axis] = choice[axis];
222
+ }
223
+ }
224
+
225
+ }
226
+
227
+ py::array_t<int32_t> so3_second_order_encode_batch(
228
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& table,
229
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>& bins,
230
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& quaternions,
231
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& previous_first_order,
232
+ int32_t input_order, bool left, double limit, double radius, bool smooth, int threads) {
233
+ if (table.ndim() != 2 || table.shape(0) != 3 || bins.size() != 3 ||
234
+ quaternions.ndim() != 3 || quaternions.shape(2) != 4 ||
235
+ previous_first_order.ndim() != 2 || previous_first_order.shape(1) != 4 ||
236
+ previous_first_order.shape(0) != quaternions.shape(0) ||
237
+ (input_order != 0 && input_order != 1) || !(limit > 0) || !(radius > 0)) {
238
+ throw std::invalid_argument("invalid second-order SO(3) batch arguments");
239
+ }
240
+ for (int j = 0; j < 3; ++j) {
241
+ if (bins.data()[j] < 1 || bins.data()[j] > table.shape(1))
242
+ throw std::invalid_argument("SO(3) primitive count exceeds table width");
243
+ }
244
+ const SO3Layout layout{table.data(), bins.data(), table.shape(1), limit, smooth,
245
+ true, left, input_order, radius};
246
+ const int64_t batch = quaternions.shape(0), steps = quaternions.shape(1);
247
+ py::array_t<int32_t> output({batch, steps, int64_t{3}});
248
+ const double* values = quaternions.data();
249
+ const double* previous = previous_first_order.data();
250
+ int32_t* result = output.mutable_data();
251
+ const Quaternion identity{0, 0, 0, 1};
252
+ py::gil_scoped_release release;
253
+ ac2::parallel_for(batch, threads, [&](int64_t i) {
254
+ Quaternion truth = identity, reconstruction = identity;
255
+ Quaternion increment{previous[4*i], previous[4*i+1], previous[4*i+2], previous[4*i+3]};
256
+ std::array<double, 3> direction{};
257
+ for (int64_t t = 0; t < steps; ++t) {
258
+ const double* raw = values + (i*steps+t)*4;
259
+ const Quaternion value{raw[0], raw[1], raw[2], raw[3]};
260
+ truth = input_order == 0 ? value : apply(truth, value, left);
261
+ const auto desired = residual(truth, reconstruction, left);
262
+ const auto correction = residual(desired, increment, left);
263
+ const auto request = as_rotvec(correction);
264
+ auto choice = nearest_choice(layout, request);
265
+ if (smooth) {
266
+
267
+ SO3Layout selection = layout;
268
+ selection.left = false;
269
+ choice = smooth_choice(selection, request, identity, correction, direction, choice);
270
+ }
271
+ double vector[3] = {primitive(layout, 0, choice[0]), primitive(layout, 1, choice[1]), primitive(layout, 2, choice[2])};
272
+ project_primitive(vector, radius);
273
+ increment = apply(increment, from_rotvec(vector), left);
274
+ reconstruction = apply(reconstruction, increment, left);
275
+ if (vector[0] != 0 || vector[1] != 0 || vector[2] != 0)
276
+ direction = {vector[0], vector[1], vector[2]};
277
+ for (int j = 0; j < 3; ++j) result[(i*steps+t)*3+j] = choice[j];
278
+ }
279
+ });
280
+ return output;
281
+ }
282
+
283
+ py::array_t<int32_t> so3_encode_batch(
284
+ const py::array_t<double, py::array::c_style | py::array::forcecast>& table,
285
+ const py::array_t<int32_t, py::array::c_style | py::array::forcecast>& bins,
286
+ const py::array_t<float, py::array::c_style | py::array::forcecast>& actions,
287
+ int32_t input_order, bool rotation_vector, bool left, double limit, bool smooth,
288
+ int threads) {
289
+ if (table.ndim() != 2 || table.shape(0) != 3 || bins.size() != 3 || actions.ndim() != 3 ||
290
+ actions.shape(2) != 3 || (input_order != 0 && input_order != 1) || limit <= 0.0) {
291
+ throw std::invalid_argument("invalid SO(3) batch encoder arguments");
292
+ }
293
+ for (int axis = 0; axis < 3; ++axis) {
294
+ if (bins.data()[axis] < 1 || bins.data()[axis] > table.shape(1)) {
295
+ throw std::invalid_argument("SO(3) primitive count exceeds table width");
296
+ }
297
+ }
298
+ const SO3Layout layout{table.data(), bins.data(), table.shape(1), limit, smooth,
299
+ rotation_vector, left, input_order};
300
+ const int64_t batch = actions.shape(0);
301
+ const int64_t steps = actions.shape(1);
302
+ py::array_t<int32_t> output({batch, steps, int64_t{3}});
303
+ {
304
+ py::gil_scoped_release release;
305
+ ac2::parallel_for(batch, threads, [&](int64_t index) {
306
+ encode_trajectory(layout, actions.data() + index * steps * 3, steps,
307
+ output.mutable_data() + index * steps * 3);
308
+ });
309
+ }
310
+ return output;
311
+ }
config.json ADDED
@@ -0,0 +1,4 @@
 
 
 
 
 
1
+ {
2
+ "model_type": "actioncodec2",
3
+ "processor_class": "ActionCodec2"
4
+ }
processing_actioncodec2.py ADDED
@@ -0,0 +1,219 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Load a named ActionCodec2 variant or a standalone saved processor."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import hashlib
6
+ import importlib
7
+ import importlib.util
8
+ import json
9
+ from pathlib import Path
10
+ import re
11
+ import sys
12
+
13
+
14
+ PROFILE_FILES = ("config.yaml", "primitives.npz", "bpe.json", "config.json", "processor_config.json")
15
+ SHARED_PATTERNS = (
16
+ "config.json", "processor_config.json", "processing_actioncodec2.py", "variants.json",
17
+ "runtime/**", "actioncodec2_kernels/**", "setup.py", "pyproject.toml", "LICENSE",
18
+ "README.md", "requirements.txt",
19
+ )
20
+
21
+
22
+ def asset_inventory(directory: Path) -> dict[str, str]:
23
+ """Hash the complete configuration and fitted assets for one variant."""
24
+ names = ["router_config.yaml", "config.json", "processor_config.json"]
25
+ profiles = [name for name in ("eef", "joint") if (directory / "profiles" / name).is_dir()]
26
+ if not profiles:
27
+ raise ValueError("variant has no fitted profiles")
28
+ names.extend(f"profiles/{profile}/{name}" for profile in profiles for name in PROFILE_FILES)
29
+ return {name: hashlib.sha256((directory / name).read_bytes()).hexdigest()
30
+ for name in sorted(names)}
31
+
32
+
33
+ def inventory_fingerprint(files: dict[str, str]) -> str:
34
+ """Return the stable identity of a fitted asset inventory."""
35
+ return hashlib.sha256(json.dumps(files, sort_keys=True, separators=(",", ":")).encode()).hexdigest()
36
+
37
+
38
+ def _check_inventory(directory: Path, record: dict) -> None:
39
+ files = asset_inventory(directory)
40
+ if files != record.get("files") or inventory_fingerprint(files) != record.get("fingerprint"):
41
+ raise ValueError("ActionCodec2 asset fingerprint mismatch; restore the complete selected variant")
42
+
43
+
44
+ class _VariantBinding:
45
+ @property
46
+ def variant(self):
47
+ return self._variant_origin.get("name")
48
+
49
+ @property
50
+ def variant_fingerprint(self):
51
+ return self._variant_origin.get("fingerprint")
52
+
53
+ def for_action_space(self, name):
54
+ codec = super().for_action_space(name)
55
+ codec._variant_origin = dict(self._variant_origin)
56
+ return codec
57
+
58
+ def fit(self, *args, **kwargs):
59
+ result = super().fit(*args, **kwargs)
60
+ self._variant_origin = {}
61
+ return result
62
+
63
+ def save_pretrained(self, save_directory, push_to_hub=False, **kwargs):
64
+ directory = Path(save_directory)
65
+ if (directory / "variants.json").is_file():
66
+ raise ValueError("save a selected variant to a separate directory, outside the catalog root")
67
+ if not push_to_hub and kwargs:
68
+ raise TypeError(f"unused save_pretrained keyword(s): {', '.join(sorted(kwargs))}")
69
+ files = super().save_pretrained(directory)
70
+ inventory = asset_inventory(directory)
71
+ manifest = directory / "artifact_manifest.json"
72
+ manifest.write_text(json.dumps({
73
+ "format_version": 1,
74
+ "origin": self._variant_origin,
75
+ "files": inventory,
76
+ "fingerprint": inventory_fingerprint(inventory),
77
+ }, indent=2) + "\n", encoding="utf-8")
78
+ files.append(str(manifest))
79
+ if push_to_hub:
80
+ package = type(self.action_tokenizer).__module__.split(".routing.", 1)[0]
81
+ importlib.import_module(f"{package}.integration.hf").upload_pretrained_directory(
82
+ directory, **kwargs
83
+ )
84
+ return files
85
+
86
+
87
+ class ActionCodec2:
88
+ """Hugging Face entry point for catalog and standalone artifacts."""
89
+
90
+ @classmethod
91
+ def register_for_auto_class(cls, auto_class="AutoProcessor"):
92
+ return cls
93
+
94
+ @classmethod
95
+ def from_pretrained(cls, pretrained_model_name_or_path, *, variant=None,
96
+ variant_fingerprint=None, action_space=None, **kwargs):
97
+ source = Path(pretrained_model_name_or_path).expanduser()
98
+ kwargs.pop("_from_auto", None)
99
+ kwargs.pop("trust_remote_code", None)
100
+ subfolder = kwargs.pop("subfolder", "")
101
+ if subfolder:
102
+ raise ValueError("select an ActionCodec2 catalog variant with variant=, not subfolder=")
103
+ allowed = {"cache_dir", "force_download", "local_files_only", "token", "revision", "repo_type"}
104
+ unknown = sorted(set(kwargs) - allowed)
105
+ if unknown:
106
+ raise TypeError(f"unsupported from_pretrained keyword(s): {', '.join(unknown)}")
107
+ if variant is not None and (not isinstance(variant, str) or
108
+ re.fullmatch(r"[a-z0-9]+(?:-[a-z0-9]+)*", variant) is None):
109
+ raise ValueError("variant must be a published variant name")
110
+ if variant_fingerprint is not None and (
111
+ not isinstance(variant_fingerprint, str) or
112
+ re.fullmatch(r"[0-9a-f]{64}", variant_fingerprint) is None
113
+ ):
114
+ raise ValueError("variant_fingerprint must be a SHA-256 fingerprint")
115
+ remote = not source.is_dir()
116
+ if remote and str(pretrained_model_name_or_path).startswith(("/", "./", "../", "~")):
117
+ raise FileNotFoundError(f"ActionCodec2 artifact does not exist: {source}")
118
+ revision = None
119
+ if remote:
120
+ from huggingface_hub import hf_hub_download, snapshot_download
121
+
122
+ code_revision = Path(__file__).parent.name
123
+ if re.fullmatch(r"[0-9a-f]{40}", code_revision):
124
+ requested = kwargs.get("revision")
125
+ if requested and re.fullmatch(r"[0-9a-f]{40}", requested) and requested != code_revision:
126
+ raise ValueError("ActionCodec2 code and assets must use the same revision")
127
+ kwargs["revision"] = code_revision
128
+ metadata_file = Path(hf_hub_download(
129
+ str(pretrained_model_name_or_path), "processor_config.json", **kwargs
130
+ ))
131
+ root = metadata_file.parent
132
+ revision = root.name
133
+ kwargs["revision"] = revision
134
+ else:
135
+ root = source
136
+ metadata_file = root / "processor_config.json"
137
+ metadata = json.loads(metadata_file.read_text(encoding="utf-8"))
138
+ if not isinstance(metadata, dict):
139
+ raise ValueError("invalid ActionCodec2 processor configuration")
140
+ record = None
141
+ origin = {}
142
+ if metadata.get("variant_catalog") == "variants.json":
143
+ catalog_path = (Path(hf_hub_download(
144
+ str(pretrained_model_name_or_path), "variants.json", **kwargs
145
+ )) if remote else root / "variants.json")
146
+ catalog = json.loads(catalog_path.read_text(encoding="utf-8"))
147
+ if (not isinstance(catalog, dict) or catalog.get("format_version") != 1 or
148
+ not isinstance(catalog.get("variants"), dict)):
149
+ raise ValueError("unsupported ActionCodec2 variant catalog")
150
+ variants = catalog["variants"]
151
+ if variant not in variants:
152
+ raise ValueError(f"select variant= from: {', '.join(sorted(variants))}")
153
+ record = variants[variant]
154
+ if not isinstance(record, dict):
155
+ raise ValueError("invalid ActionCodec2 variant record")
156
+ directory = root / "variants" / variant
157
+ if remote:
158
+ root = Path(snapshot_download(
159
+ str(pretrained_model_name_or_path),
160
+ allow_patterns=[*SHARED_PATTERNS, f"variants/{variant}/**"], **kwargs,
161
+ ))
162
+ directory = root / "variants" / variant
163
+ _check_inventory(directory, record)
164
+ origin = {"name": variant, "fingerprint": record["fingerprint"]}
165
+ if remote:
166
+ origin.update(repo_id=str(pretrained_model_name_or_path), revision=revision)
167
+ else:
168
+ if remote:
169
+ root = Path(snapshot_download(
170
+ str(pretrained_model_name_or_path),
171
+ allow_patterns=[*SHARED_PATTERNS, "router_config.yaml", "profiles/**",
172
+ "artifact_manifest.json"], **kwargs,
173
+ ))
174
+ directory = root
175
+ manifest = root / "artifact_manifest.json"
176
+ if manifest.is_file():
177
+ record = json.loads(manifest.read_text(encoding="utf-8"))
178
+ if (not isinstance(record, dict) or record.get("format_version") != 1 or
179
+ not isinstance(record.get("origin"), dict)):
180
+ raise ValueError("unsupported ActionCodec2 saved artifact manifest")
181
+ _check_inventory(directory, record)
182
+ origin = record["origin"]
183
+ if variant is not None and variant != origin.get("name"):
184
+ raise ValueError("saved processor contains a different variant; load the catalog to switch")
185
+ if variant_fingerprint is not None and variant_fingerprint != origin.get("fingerprint"):
186
+ raise ValueError("variant fingerprint differs from the tokenizer bound to this policy")
187
+
188
+ runtime = root / "runtime"
189
+ package_file = runtime / "__init__.py"
190
+ if not package_file.is_file():
191
+ raise FileNotFoundError(f"ActionCodec2 runtime is missing from {root}")
192
+ identity = hashlib.sha256(str(runtime.resolve()).encode()).hexdigest()[:16]
193
+ package_name = f"_actioncodec2_artifact_{identity}"
194
+ if package_name not in sys.modules:
195
+ spec = importlib.util.spec_from_file_location(
196
+ package_name, package_file, submodule_search_locations=[str(runtime)]
197
+ )
198
+ if spec is None or spec.loader is None:
199
+ raise ImportError(f"cannot load ActionCodec2 runtime from {runtime}")
200
+ package = importlib.util.module_from_spec(spec)
201
+ sys.modules[package_name] = package
202
+ try:
203
+ spec.loader.exec_module(package)
204
+ except BaseException:
205
+ del sys.modules[package_name]
206
+ raise
207
+ processor = importlib.import_module(f"{package_name}.processing_actioncodec2")
208
+ if (runtime / "hub_acceleration.py").is_file():
209
+ acceleration = importlib.import_module(f"{package_name}.hub_acceleration")
210
+ acceleration.install(package_name)
211
+ if not hasattr(processor, "_catalog_processor_class"):
212
+ processor._catalog_processor_class = type(
213
+ "ActionCodec2", (_VariantBinding, processor.ActionCodec2),
214
+ {"__module__": processor.__name__},
215
+ )
216
+ codec = processor._catalog_processor_class.from_pretrained(directory, action_space=action_space)
217
+ codec._variant_origin = origin
218
+ codec._source_directory = root
219
+ return codec
pyproject.toml ADDED
@@ -0,0 +1,16 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ [build-system]
2
+ requires = ["setuptools>=77", "wheel", "pybind11>=2.12"]
3
+ build-backend = "setuptools.build_meta"
4
+
5
+ [project]
6
+ name = "actioncodec2-kernels"
7
+ version = "0.1.0"
8
+ description = "Portable C++17 encode/decode kernels for ActionCodec2 tokenizer artifacts"
9
+ requires-python = ">=3.10"
10
+ license = "Apache-2.0"
11
+
12
+ [tool.setuptools]
13
+ packages = ["actioncodec2_kernels"]
14
+
15
+ [tool.setuptools.package-data]
16
+ actioncodec2_kernels = ["*.cpp", "*.hpp"]
requirements.txt ADDED
@@ -0,0 +1,6 @@
 
 
 
 
 
 
 
1
+ numpy
2
+ scipy
3
+ torch>=2.3
4
+ transformers>=4.57,<6
5
+ huggingface-hub
6
+ pyyaml
runtime/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Private inference runtime bundled with this ActionCodec2 artifact."""
runtime/backend/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Internal backend components for ActionCodec2."""
runtime/backend/native_backend.py ADDED
@@ -0,0 +1,39 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Optional CPU accelerator; build with python setup.py build_ext --inplace."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import os
6
+
7
+ __all__ = ["native", "have_native", "require_native", "resolve_backend"]
8
+
9
+ try: # pragma: no cover - exercised by whichever branch the build state picks
10
+ from .. import _native as native
11
+ except ImportError: # pragma: no cover
12
+ native = None
13
+
14
+
15
+ def have_native() -> bool:
16
+ """True when the compiled kernels are importable and not disabled."""
17
+ return native is not None and not os.environ.get("ACTIONCODEC2_NO_NATIVE")
18
+
19
+
20
+ def require_native():
21
+ """Return the native module or explain how to get one."""
22
+ if not have_native():
23
+ raise RuntimeError(
24
+ "the actioncodec2 native extension is not available; build it from "
25
+ "the package native/ directory (python setup.py build_ext --inplace) or "
26
+ "select backend='python'"
27
+ )
28
+ return native
29
+
30
+
31
+ def resolve_backend(backend: str) -> str:
32
+ """Turn ``'auto'`` into a concrete backend name."""
33
+ if backend not in {"auto", "native", "python"}:
34
+ raise ValueError("backend must be 'auto', 'native' or 'python'")
35
+ if backend == "auto":
36
+ return "native" if have_native() else "python"
37
+ if backend == "native":
38
+ require_native()
39
+ return backend
runtime/common/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Internal common components for ActionCodec2."""
runtime/common/_frontend_base.py ADDED
@@ -0,0 +1,94 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """ActionCodec2-only tensor and token-row validation helpers."""
2
+
3
+ from __future__ import annotations
4
+
5
+ from collections.abc import Sequence
6
+
7
+ import numpy as np
8
+ import torch
9
+
10
+ from ._types import ActionIdxBatch, TokenIds
11
+
12
+
13
+ class BaseActionTokenizer:
14
+ """Shared action tensor methods, without policy-specific sentinels."""
15
+
16
+ @property
17
+ def new_action_tokens(self) -> list[str]:
18
+ """Token strings in id order — downstream registers these into its VLM vocab."""
19
+ return list(self._new_action_tokens)
20
+
21
+ def to_token_strs(self, token_ids: TokenIds) -> list[str]:
22
+ "Render one id row as its token strings, e.g. ``<right_arm_0>`` / ``<action0042>``."
23
+ row = self._to_id_row(token_ids).tolist()
24
+ n = len(self._new_action_tokens)
25
+ bad = [i for i in row if not 0 <= i < n]
26
+ if bad:
27
+ raise ValueError(f"token ids {bad[:5]} out of range [0, {n})")
28
+ return [self._new_action_tokens[i] for i in row]
29
+
30
+ def _check_encode_kwargs(self, encode_kwargs: dict | None) -> dict:
31
+
32
+ kw = dict(encode_kwargs or {})
33
+ unknown = sorted(set(kw) - self.encode_kwargs)
34
+ if unknown:
35
+ raise ValueError(
36
+ f"{type(self).__name__} does not consume encode_kwargs {unknown}; it declares {sorted(self.encode_kwargs)}"
37
+ )
38
+ return kw
39
+
40
+ @staticmethod
41
+ def _to_id_row(token_ids: TokenIds) -> torch.Tensor:
42
+
43
+ if isinstance(token_ids, torch.Tensor):
44
+ row = token_ids.detach().cpu()
45
+ if (
46
+ row.dtype == torch.bool
47
+ or row.dtype.is_floating_point
48
+ or row.dtype.is_complex
49
+ ):
50
+ raise ValueError("token ids must be integers")
51
+ row = row.to(torch.long)
52
+ else:
53
+ values = np.asarray(token_ids)
54
+ if values.dtype.kind not in {"i", "u"}:
55
+ raise ValueError("token ids must be integers")
56
+ row = torch.from_numpy(np.asarray(values, dtype=np.int64))
57
+ if row.ndim != 1:
58
+ raise ValueError(f"token id row must be 1D, got {tuple(row.shape)}")
59
+ return row
60
+
61
+ def _checked_decode_row(self, token_ids: TokenIds) -> torch.Tensor:
62
+
63
+ row = self._to_id_row(token_ids)
64
+ invalid = (row < 0) | (row >= int(self._action_vocab_size))
65
+ if torch.any(invalid):
66
+ bad = row[invalid][:5].tolist()
67
+ raise ValueError(
68
+ f"token ids {bad} out of range [0, {int(self._action_vocab_size)})"
69
+ )
70
+ return row
71
+
72
+ def _decode_token_ids_to_actions_batch(
73
+ self,
74
+ action_token_ids: Sequence[TokenIds],
75
+ *,
76
+ time_horizon: int | None = None,
77
+ action_dim: int | None = None,
78
+ decode_kwargs: dict | None = None,
79
+ ) -> torch.Tensor:
80
+
81
+ batch_action_idx: ActionIdxBatch = [
82
+ self._checked_decode_row(a).tolist() for a in action_token_ids
83
+ ]
84
+ (actions_np, _) = self._decode_action_indices(
85
+ batch_action_idx,
86
+ time_horizon=time_horizon,
87
+ action_dim=action_dim,
88
+ decode_kwargs=decode_kwargs,
89
+ )
90
+ return torch.from_numpy(actions_np)
91
+
92
+ @property
93
+ def vocab_size(self) -> int:
94
+ return int(self._action_vocab_size)
runtime/common/_types.py ADDED
@@ -0,0 +1,12 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Shared lightweight types for the frontend layer."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import numpy as np
6
+ import torch
7
+
8
+ ActionIdxBatch = list[list[int]]
9
+ TokenIds = list[int] | np.ndarray | torch.Tensor
10
+
11
+
12
+ __all__ = ["ActionIdxBatch", "TokenIds"]
runtime/common/_validation.py ADDED
@@ -0,0 +1,89 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Shared validation primitives for ActionCodec2 public boundaries."""
2
+
3
+ from __future__ import annotations
4
+
5
+ from collections.abc import Iterable, Sequence
6
+ from typing import TypeVar
7
+
8
+ import numpy as np
9
+
10
+ ErrorT = TypeVar("ErrorT", bound=Exception)
11
+
12
+
13
+ def active_dimensions(
14
+ active_dims: Sequence[int] | None,
15
+ dimension: int,
16
+ *,
17
+ subject: str = "active_dims",
18
+ ) -> tuple[int, ...]:
19
+ """Normalize and validate a non-empty increasing subset of dimensions."""
20
+ dims = (
21
+ tuple(range(dimension))
22
+ if active_dims is None
23
+ else tuple(int(item) for item in active_dims)
24
+ )
25
+ if not dims:
26
+ raise ValueError(f"{subject} must not be empty")
27
+ if any(left >= right for left, right in zip(dims, dims[1:])):
28
+ raise ValueError(f"{subject} must be strictly increasing")
29
+ if dims[0] < 0 or dims[-1] >= dimension:
30
+ raise ValueError(f"{subject} contains a dimension outside the vocabulary")
31
+ return dims
32
+
33
+
34
+ def optional_finite_array(
35
+ value: np.ndarray | None,
36
+ *,
37
+ shape: tuple[int, ...],
38
+ name: str = "current_state",
39
+ ) -> np.ndarray | None:
40
+ """Convert an optional float32 array and enforce its exact shape/finiteness."""
41
+ if value is None:
42
+ return None
43
+ result = np.asarray(value, dtype=np.float32)
44
+ if result.shape != shape:
45
+ rendered = ", ".join(str(item) for item in shape)
46
+ raise ValueError(f"{name} must have shape [{rendered}]")
47
+ if not np.all(np.isfinite(result)):
48
+ raise ValueError(f"{name} contains a non-finite value")
49
+ return result
50
+
51
+
52
+ def broadcast_vector(
53
+ value: object,
54
+ width: int,
55
+ *,
56
+ dtype: np.dtype,
57
+ name: str,
58
+ ) -> np.ndarray:
59
+ """Broadcast a scalar or width-sized value to one numerical vector."""
60
+ array = np.asarray(value, dtype=dtype)
61
+ try:
62
+ return np.broadcast_to(array, (width,)).copy()
63
+ except ValueError as error:
64
+ raise ValueError(f"{name} must be scalar or contain {width} values") from error
65
+
66
+
67
+ def validate_dimension_partition(
68
+ groups: Iterable[tuple[str, Sequence[int]]],
69
+ dimension: int,
70
+ *,
71
+ label: str,
72
+ error_type: type[ErrorT] = ValueError,
73
+ ) -> None:
74
+ """Validate that named dimension groups form one exact non-overlapping partition."""
75
+ owners: dict[int, str] = {}
76
+ for name, dims in groups:
77
+ for index in dims:
78
+ if index < 0 or index >= dimension:
79
+ raise error_type(
80
+ f"{label} {index} used by {name!r} is outside input_dim"
81
+ )
82
+ if index in owners:
83
+ raise error_type(
84
+ f"{label} {index} belongs to both {owners[index]!r} and {name!r}"
85
+ )
86
+ owners[index] = name
87
+ missing = sorted(set(range(dimension)) - set(owners))
88
+ if missing:
89
+ raise error_type(f"{label}(s) {missing} are not assigned")
runtime/frontend/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Internal frontend components for ActionCodec2."""
runtime/frontend/tokenizer.py ADDED
@@ -0,0 +1,342 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Action-space-bound, arbitrary-horizon ActionCodec2 frontend."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import sys
6
+ import warnings
7
+ from collections.abc import Mapping, Sequence
8
+ from pathlib import Path
9
+ from typing import TextIO
10
+
11
+ import numpy as np
12
+ import torch
13
+
14
+ from ..common._frontend_base import BaseActionTokenizer
15
+ from ..common._types import ActionIdxBatch, TokenIds
16
+ from ..routing.presets import PresetConfig
17
+ from ..routing.routing import ActionCodec2Router
18
+ from ..physical.semantic import resampled_horizon
19
+
20
+
21
+ def _numpy(value):
22
+
23
+ return (
24
+ value.detach().cpu().float().numpy()
25
+ if isinstance(value, torch.Tensor)
26
+ else value
27
+ )
28
+
29
+
30
+ class ActionCodec2Tokenizer(BaseActionTokenizer):
31
+ "ActionCodec2 frontend bound to physical tensor semantics, not a horizon."
32
+
33
+ encode_kwargs = frozenset({"fps", "current_state", "previous_first_order"})
34
+
35
+ def __init__(self, backend: ActionCodec2Router, *, action_space: str) -> None:
36
+ if not isinstance(backend, ActionCodec2Router):
37
+ raise TypeError(
38
+ f"ActionCodec2Tokenizer requires ActionCodec2Router, got {type(backend).__name__}"
39
+ )
40
+ preset = backend.preset(action_space)
41
+ self.action_tokenizer = backend
42
+ self._action_space = preset
43
+ self._action_vocab_size = int(backend.vocab_size)
44
+ self._new_action_tokens = list(backend.new_action_tokens)
45
+ self.action_dim = preset.input_dim
46
+ self._warned_default_fps = False
47
+ self.requires_previous_first_order = any(
48
+ physical.primitive_order == 2
49
+ and set(component.active_dims).intersection(physical.dims)
50
+ for component in preset.components
51
+ for physical in backend.profiles[
52
+ component.space
53
+ ].processor.config.physical.components.values()
54
+ )
55
+ self._encoded_action_dim = sum(
56
+ len(component.active_dims) for component in preset.components
57
+ )
58
+
59
+ @classmethod
60
+ def from_pretrained(
61
+ cls,
62
+ pretrained_model_name_or_path: str | Path,
63
+ *,
64
+ action_space: str | Path | PresetConfig | Mapping[str, object],
65
+ **kwargs: object,
66
+ ) -> ActionCodec2Tokenizer:
67
+ "Load a local/Hub router and bind one explicit physical action space."
68
+ router = ActionCodec2Router.from_pretrained(
69
+ pretrained_model_name_or_path, **kwargs
70
+ )
71
+ if isinstance(action_space, (PresetConfig, Mapping, Path)):
72
+ name = router.register_action_space(action_space).name
73
+ elif isinstance(action_space, str):
74
+ candidate = Path(action_space)
75
+ name = (
76
+ router.register_action_space(candidate).name
77
+ if candidate.is_file()
78
+ else action_space
79
+ )
80
+ else:
81
+ raise TypeError(
82
+ "action_space must be a registered name, PresetConfig, mapping, or YAML path"
83
+ )
84
+ return cls(router, action_space=name)
85
+
86
+ def __str__(self) -> str:
87
+
88
+ return self._describe_action_spaces()
89
+
90
+ def _describe_action_spaces(self, action_dim: int | None = None) -> str:
91
+ return self.action_tokenizer.describe_action_spaces(
92
+ selected=self.action_space.name, action_dim=action_dim
93
+ )
94
+
95
+ @property
96
+ def action_space(self) -> PresetConfig:
97
+ """The immutable tensor semantics selected for this tokenizer."""
98
+ return self._action_space
99
+
100
+ @property
101
+ def action_spaces(self) -> Mapping[str, PresetConfig]:
102
+ """Every built-in or user-registered option in the loaded artifact."""
103
+ return self.action_tokenizer.action_spaces
104
+
105
+ def print_action_spaces(
106
+ self, *, action_dim: int | None = None, file: TextIO | None = None
107
+ ) -> None:
108
+ """Print all layouts or candidates matching ``action_dim`` without changing the binding."""
109
+ description = self._describe_action_spaces(action_dim)
110
+ print(description, file=sys.stdout if file is None else file)
111
+
112
+ def register_action_space(
113
+ self,
114
+ preset: PresetConfig | Mapping[str, object] | str | Path,
115
+ ) -> PresetConfig:
116
+ """Register a custom action space without changing this instance's binding."""
117
+ return self.action_tokenizer.register_action_space(preset)
118
+
119
+ def for_action_space(self, name: str) -> ActionCodec2Tokenizer:
120
+ """Return a new tokenizer sharing weights but bound to another option."""
121
+ return type(self)(self.action_tokenizer, action_space=name)
122
+
123
+ def _source_fps(self, fps, preset=None):
124
+ preset = self.action_space if preset is None else preset
125
+ if fps is None:
126
+ if not self._warned_default_fps:
127
+ warnings.warn(
128
+ f"fps was omitted; assuming {preset.source_hz:g} Hz from action_space "
129
+ f"{preset.name!r}. Pass the actual fps explicitly to avoid changing action timing.",
130
+ UserWarning,
131
+ stacklevel=3,
132
+ )
133
+ self._warned_default_fps = True
134
+ return preset.source_hz
135
+ return fps
136
+
137
+ def first_order_template(self, batch_size: int) -> dict[str, np.ndarray]:
138
+ "Explicit zero boundary templates, shaped {component: (B,D_active)}."
139
+ if type(batch_size) is not int or batch_size < 1:
140
+ raise ValueError("batch_size must be a positive integer")
141
+ return {
142
+ c.name: np.zeros((batch_size, len(c.active_dims)), dtype=np.float32)
143
+ for c in self.action_space.components
144
+ if any(
145
+ p.primitive_order == 2 and set(c.active_dims).intersection(p.dims)
146
+ for p in self.action_tokenizer.profiles[
147
+ c.space
148
+ ].processor.config.physical.components.values()
149
+ )
150
+ }
151
+
152
+ def max_tokens_for_horizon(self, horizon: int, *, fps: float | None = None) -> int:
153
+ "Return the token bound after converting a source chunk to the codec clock."
154
+ source_fps = self._source_fps(fps)
155
+ internal = resampled_horizon(
156
+ horizon, source_hz=source_fps, target_hz=self.action_space.target_hz
157
+ )
158
+ return internal * self._encoded_action_dim
159
+
160
+ def grammar(self, horizon: int, *, fps: float | None = None):
161
+ "Build a grammar for the codec steps implied by source chunk geometry."
162
+ source_fps = self._source_fps(fps)
163
+ internal = resampled_horizon(
164
+ horizon, source_hz=source_fps, target_hz=self.action_space.target_hz
165
+ )
166
+ return self.action_tokenizer.grammar(
167
+ preset=self.action_space.name, horizon=internal
168
+ )
169
+
170
+ def __call__(
171
+ self,
172
+ action: torch.Tensor | np.ndarray,
173
+ *,
174
+ fps: float | None = None,
175
+ current_state: torch.Tensor | np.ndarray | None = None,
176
+ previous_first_order: Mapping[str, torch.Tensor | np.ndarray] | None = None,
177
+ encode_kwargs: dict | None = None,
178
+ ) -> ActionIdxBatch:
179
+
180
+ kwargs = self._check_encode_kwargs(encode_kwargs)
181
+ direct = {
182
+ "fps": fps,
183
+ "current_state": current_state,
184
+ "previous_first_order": previous_first_order,
185
+ }
186
+ duplicates = sorted(
187
+ key for key, value in direct.items() if value is not None and key in kwargs
188
+ )
189
+ if duplicates:
190
+ raise ValueError(f"encode arguments provided twice: {duplicates}")
191
+ kwargs.update(
192
+ {key: value for key, value in direct.items() if value is not None}
193
+ )
194
+ reference = kwargs.get("current_state")
195
+ if self.action_space.input == "absolute" and reference is None:
196
+ raise ValueError(
197
+ f"action space {self.action_space.name!r} is absolute; encode requires "
198
+ "current_state with shape [B, D] to preserve the first transition"
199
+ )
200
+ if self.action_space.input == "delta" and reference is not None:
201
+ raise ValueError(
202
+ f"action space {self.action_space.name!r} is delta; omit current_state"
203
+ )
204
+ values = torch.as_tensor(action)
205
+ if values.ndim == 2:
206
+ values = values.unsqueeze(0)
207
+ return self._encode_action_indices(values, encode_kwargs=kwargs)
208
+
209
+ encode = __call__
210
+
211
+ def decode(
212
+ self,
213
+ action_token_ids: Sequence[TokenIds],
214
+ *,
215
+ fps: float | None = None,
216
+ current_state: torch.Tensor | np.ndarray | None = None,
217
+ previous_first_order: Mapping[str, torch.Tensor | np.ndarray] | None = None,
218
+ ) -> torch.Tensor:
219
+ "Decode a batch, deriving sequence length from tokens and output rate."
220
+ return self._decode_token_ids_to_actions_batch(
221
+ action_token_ids,
222
+ action_dim=self.action_dim,
223
+ decode_kwargs={
224
+ "fps": fps,
225
+ "current_state": current_state,
226
+ "previous_first_order": previous_first_order,
227
+ },
228
+ )
229
+
230
+ @staticmethod
231
+ def _numpy_boundary(previous_first_order):
232
+ if previous_first_order is None:
233
+ return None
234
+ if not isinstance(previous_first_order, Mapping):
235
+ raise ValueError(
236
+ "previous_first_order must map component names to [B,D_active] arrays"
237
+ )
238
+ return {name: _numpy(value) for name, value in previous_first_order.items()}
239
+
240
+ def final_first_order(
241
+ self,
242
+ action_token_ids: Sequence[TokenIds],
243
+ *,
244
+ previous_first_order: Mapping[str, torch.Tensor | np.ndarray],
245
+ executed_steps: int | None = None,
246
+ ) -> dict[str, np.ndarray]:
247
+ "Return reconstructed component states after a canonical-step prefix."
248
+ return self.action_tokenizer.final_first_order(
249
+ [self._checked_decode_row(row).tolist() for row in action_token_ids],
250
+ preset=self.action_space.name,
251
+ previous_first_order=self._numpy_boundary(previous_first_order),
252
+ executed_steps=executed_steps,
253
+ )
254
+
255
+ def _encode_action_indices(
256
+ self, action: torch.Tensor, encode_kwargs: dict | None = None
257
+ ) -> ActionIdxBatch:
258
+
259
+ kwargs = dict(encode_kwargs or {})
260
+ fps = float(self._source_fps(kwargs.get("fps")))
261
+ current_state = _numpy(kwargs.get("current_state"))
262
+ values = _numpy(action)
263
+ if values.ndim != 3 or values.shape[2] != self.action_space.input_dim:
264
+ raise ValueError(
265
+ f"actions must have shape [B, T_source, {self.action_space.input_dim}]"
266
+ )
267
+ return self.action_tokenizer.encode_batch(
268
+ values,
269
+ preset=self.action_space.name,
270
+ fps=fps,
271
+ current_state=current_state,
272
+ previous_first_order=self._numpy_boundary(
273
+ kwargs.get("previous_first_order")
274
+ ),
275
+ )
276
+
277
+ def _decode_action_indices(
278
+ self,
279
+ action_indices: ActionIdxBatch,
280
+ *,
281
+ time_horizon: int | None = None,
282
+ action_dim: int | None = None,
283
+ decode_kwargs: dict | None = None,
284
+ ) -> tuple[np.ndarray, list[set[str]]]:
285
+
286
+ if time_horizon is not None:
287
+ raise ValueError(
288
+ "ActionCodec2 decode infers sequence length from tokens; "
289
+ "do not provide time_horizon"
290
+ )
291
+ if action_dim is not None and int(action_dim) != self.action_dim:
292
+ raise ValueError(
293
+ f"action_dim={action_dim} disagrees with action space "
294
+ f"{self.action_space.name!r} (D={self.action_dim})"
295
+ )
296
+ kwargs = dict(decode_kwargs or {})
297
+ unknown = sorted(set(kwargs) - self.encode_kwargs)
298
+ if unknown:
299
+ raise ValueError(
300
+ f"ActionCodec2Tokenizer does not consume decode_kwargs {unknown}"
301
+ )
302
+ raw_fps = kwargs.get("fps")
303
+ fps = float(self._source_fps(raw_fps))
304
+ current_state = _numpy(kwargs.get("current_state"))
305
+ if self.action_space.input == "absolute" and current_state is None:
306
+ raise ValueError(
307
+ f"action space {self.action_space.name!r} is absolute; decode requires "
308
+ "current_state with shape [B, D] to restore its external reference"
309
+ )
310
+ if self.action_space.input == "delta" and current_state is not None:
311
+ raise ValueError(
312
+ f"action space {self.action_space.name!r} is delta; omit current_state "
313
+ "to decode in the registered input semantics"
314
+ )
315
+ decoded = self.action_tokenizer.decode_batch(
316
+ action_indices,
317
+ preset=self.action_space.name,
318
+ fps=fps,
319
+ current_state=current_state,
320
+ previous_first_order=self._numpy_boundary(
321
+ kwargs.get("previous_first_order")
322
+ ),
323
+ )
324
+ actions = self._restore_source_layout(decoded)
325
+ return actions, [set() for _ in range(actions.shape[0])]
326
+
327
+ def _restore_source_layout(self, decoded: Mapping[str, np.ndarray]) -> np.ndarray:
328
+
329
+ first = next(iter(decoded.values()))
330
+ batch, steps = first.shape[:2]
331
+ if any(values.shape[:2] != (batch, steps) for values in decoded.values()):
332
+ raise RuntimeError(
333
+ "decoded components disagree on batch or sequence length"
334
+ )
335
+ actions = np.empty((batch, steps, self.action_dim), dtype=np.float32)
336
+ for component in self.action_space.components:
337
+ values = decoded[component.name]
338
+ motion_width = len(component.source_dims)
339
+ actions[:, :, component.source_dims] = values[:, :, :motion_width]
340
+ if component.gripper_dim is not None:
341
+ actions[:, :, component.gripper_dim] = values[:, :, motion_width]
342
+ return actions
runtime/hub_acceleration.py ADDED
@@ -0,0 +1,333 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Install the shared C++ kernels in a portable ActionCodec2 runtime."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import functools
6
+ import importlib
7
+ from numbers import Integral
8
+ import os
9
+
10
+ import numpy as np
11
+
12
+
13
+ def load_native():
14
+ """Return installed portable or VLA kernels, with a fork-safe thread budget."""
15
+ if os.environ.get("ACTIONCODEC2_NO_NATIVE"):
16
+ return None
17
+ for name in (
18
+ "actioncodec2_kernels._actioncodec2_native",
19
+ "actioncodec2.tokenizers._actioncodec2_native",
20
+ ):
21
+ try:
22
+ native = importlib.import_module(name)
23
+ except ImportError:
24
+ continue
25
+ _configure(native)
26
+ return native
27
+ return None
28
+
29
+
30
+ @functools.cache
31
+ def _configure(native) -> None:
32
+
33
+ configured = os.environ.get("ACTIONCODEC2_NUM_THREADS")
34
+ if configured is not None:
35
+ if not configured.isdigit() or int(configured) < 1:
36
+ raise ValueError("ACTIONCODEC2_NUM_THREADS must be a positive integer")
37
+ native.set_thread_budget(int(configured))
38
+ else:
39
+ native.set_thread_budget(0)
40
+ if hasattr(os, "register_at_fork"):
41
+ os.register_at_fork(after_in_child=lambda: native.set_thread_budget(1))
42
+
43
+
44
+ def install(package: str) -> None:
45
+ """Accelerate an artifact while preserving its profiles and public contracts."""
46
+ backend = importlib.import_module(f"{package}.backend.native_backend")
47
+
48
+ def native_now():
49
+ return backend.native if backend.have_native() else None
50
+
51
+ if not getattr(backend, "_portable_reconstruction_grammar", False):
52
+ numerical = importlib.import_module(f"{package}.physical.physical_codec")
53
+ grammars = importlib.import_module(f"{package}.tokenization.grammar")
54
+ for runtime_class, method, kernel in (
55
+ (numerical._SO3Runtime, "decode_batch", "so3_reconstruct"),
56
+ (numerical._AdditiveRuntime, "_decode_second_order", "additive_reconstruct"),
57
+ ):
58
+ original = getattr(runtime_class, method)
59
+
60
+ @functools.wraps(original)
61
+ def reconstruct(self, cells, previous_first_order=None, *,
62
+ _original=original, _kernel=kernel):
63
+ native = native_now()
64
+ if native is None or not hasattr(native, _kernel):
65
+ return _original(self, cells, previous_first_order)
66
+ if previous_first_order is None:
67
+ if self.config.primitive_order == 2:
68
+ raise ValueError("second-order reconstruction requires previous_first_order")
69
+ previous_first_order = np.zeros((len(cells), len(self.primitives)), np.float32)
70
+ if not hasattr(self, "_portable_reconstruction_table"):
71
+ self._portable_reconstruction_table = self._native_table.astype(np.float64)
72
+ args = (
73
+ self._portable_reconstruction_table, self._native_bins, cells,
74
+ previous_first_order, numerical._scale(self.config), self.config.input_order,
75
+ )
76
+ if _kernel == "additive_reconstruct":
77
+ return native.additive_reconstruct(*args)
78
+ return native.so3_reconstruct(
79
+ *args, self.config.primitive_order, self.left,
80
+ self.config.representation == "rotation_vector",
81
+ getattr(self, "second_order_radius", np.inf),
82
+ )
83
+
84
+ setattr(runtime_class, method, reconstruct)
85
+
86
+ def compiled_grammar(grammar):
87
+ native = native_now()
88
+ kind = "SetGrammar" if hasattr(grammar, "_full") else "PlainGrammar"
89
+ if native is None or not hasattr(native, kind):
90
+ return None
91
+ compiled = getattr(grammar, "_portable_native", None)
92
+ if compiled is None:
93
+ if kind == "SetGrammar":
94
+ compiled = native.SetGrammar(
95
+ grammar.horizon, grammar._full, grammar._root_anchor.tolist(),
96
+ grammar._patterns,
97
+ )
98
+ else:
99
+ compiled = native.PlainGrammar(
100
+ grammar.horizon, grammar._token_cells.tolist(), grammar._phase
101
+ )
102
+ grammar._portable_native = compiled
103
+ return compiled
104
+
105
+ for grammar_class in (grammars.ActionCodec2Grammar, grammars.ActionCodec2BPEGrammar):
106
+ original_advance = grammar_class._advance
107
+ original_mask = grammar_class._next_token_mask_from_state
108
+
109
+ def advance(self, state, token, *, _original=original_advance):
110
+ compiled = compiled_grammar(self)
111
+ if compiled is not None:
112
+ return compiled.advance(state, token)
113
+ if hasattr(self, "_full") and 0 <= token < self.vocab_size:
114
+ if any(support & ~self._full for _, support in self._patterns[token]):
115
+ return None
116
+ return _original(self, state, token)
117
+
118
+ def next_mask(self, state, *, _original=original_mask):
119
+ compiled = compiled_grammar(self)
120
+ if compiled is None:
121
+ mask = _original(self, state)
122
+ if hasattr(self, "_full"):
123
+ scope = getattr(self, "_portable_scope", None)
124
+ if scope is None:
125
+ scope = self._portable_scope = np.asarray([
126
+ all(support & ~self._full == 0 for _, support in pattern)
127
+ for pattern in self._patterns
128
+ ], dtype=bool)
129
+ mask &= scope
130
+ self._mask_cache[state] = mask.copy()
131
+ return mask
132
+ key = state if hasattr(self, "_full") else state[0]
133
+ mask = self._mask_cache.get(key)
134
+ if mask is None:
135
+ mask = compiled.mask(state)
136
+ self._mask_cache[key] = mask
137
+ return mask.copy()
138
+
139
+ grammar_class._advance = advance
140
+ grammar_class._next_token_mask_from_state = next_mask
141
+ backend._portable_reconstruction_grammar = True
142
+
143
+ if getattr(backend, "_portable_acceleration", False):
144
+ return
145
+ backend._portable_acceleration = True
146
+ backend.native = load_native()
147
+ semantic = importlib.import_module(f"{package}.physical.semantic")
148
+ presets = importlib.import_module(f"{package}.routing.presets")
149
+ trajectory = importlib.import_module(f"{package}.physical.trajectory")
150
+ physical = importlib.import_module(f"{package}.physical.physical_processor")
151
+ validation = importlib.import_module(f"{package}.common._validation")
152
+
153
+ def wrap_pose(name, kernel, width):
154
+ original = getattr(semantic, name)
155
+
156
+ @functools.wraps(original)
157
+ def wrapped(values):
158
+ native = native_now()
159
+ array = np.asarray(values)
160
+ if (native is None or array.dtype != np.float32 or array.ndim != 2
161
+ or array.shape[1] != width or len(array) < 1
162
+ or not np.all(np.isfinite(array))):
163
+ return original(values)
164
+ return getattr(native, kernel)(array[None])[0]
165
+
166
+ setattr(semantic, name, wrapped)
167
+ if hasattr(presets, name):
168
+ setattr(presets, name, wrapped)
169
+
170
+ for name, kernel, width in (
171
+ ("integrate_eef_deltas", "integrate_eef_deltas_batch", 6),
172
+ ("eef_pose_absolute", "eef_pose_absolute_batch", 7),
173
+ ("eef_pose_deltas", "eef_pose_deltas_batch", 7),
174
+ ):
175
+ wrap_pose(name, kernel, width)
176
+
177
+ original_resample = semantic.resample_uniform
178
+
179
+ @functools.wraps(original_resample)
180
+ def resample(values, *, source_hz, target_hz, kind="linear", target_length=None,
181
+ method="linear"):
182
+ native = native_now()
183
+ array = np.asarray(values)
184
+ if (native is None or array.dtype != np.float32 or array.ndim != 2
185
+ or len(array) < 1 or not np.all(np.isfinite(array))
186
+ or kind not in {"linear", "hold", "pose_xyzw"}
187
+ or (target_length is not None and (
188
+ isinstance(target_length, bool) or not isinstance(target_length, Integral)
189
+ or target_length < 1))):
190
+ return original_resample(values, source_hz=source_hz, target_hz=target_hz,
191
+ kind=kind, target_length=target_length, method=method)
192
+ return native.resample_uniform_batch(
193
+ array[None], source_hz, target_hz, kind,
194
+ -1 if target_length is None else target_length, method,
195
+ )[0]
196
+
197
+ semantic.resample_uniform = presets.resample_uniform = resample
198
+ original_canonicalize = presets.PresetConfig.canonicalize
199
+
200
+ @functools.wraps(original_canonicalize)
201
+ def canonicalize(self, values, *, source_hz=None, current_state=None):
202
+ native = native_now()
203
+ array = np.asarray(values, dtype=np.float32)
204
+ if (native is None or self.input != "delta" or current_state is not None
205
+ or array.ndim != 3 or min(array.shape) < 1
206
+ or array.shape[2] != self.input_dim or not np.all(np.isfinite(array))
207
+ or any(c.space == "eef" and c.rotation_format != "rotation_vector"
208
+ for c in self.components)):
209
+ return original_canonicalize(self, values, source_hz=source_hz,
210
+ current_state=current_state)
211
+ rate = self.source_hz if source_hz is None else source_hz
212
+ output = {}
213
+ for component in self.components:
214
+ if component.space == "eef":
215
+ motion = np.concatenate((array[..., component.position_dims],
216
+ array[..., component.rotation_dims]), axis=2)
217
+ pose = native.integrate_eef_deltas_batch(motion)
218
+ sampled = native.resample_uniform_batch(
219
+ pose, rate, self.target_hz, "pose_xyzw", -1, self.interpolation
220
+ )
221
+ motion = native.eef_pose_absolute_batch(sampled)
222
+ else:
223
+ motion = np.cumsum(array[..., component.joint_dims], axis=1,
224
+ dtype=np.float32)
225
+ motion = native.resample_uniform_batch(
226
+ motion, rate, self.target_hz, "linear", -1, self.interpolation
227
+ )
228
+ if component.gripper_dim is not None:
229
+ gripper = native.resample_uniform_batch(
230
+ array[..., component.gripper_dim:component.gripper_dim + 1],
231
+ rate, self.target_hz, "hold", -1, self.interpolation,
232
+ )
233
+ gripper = semantic.binarize_gripper_actions(
234
+ gripper.reshape(-1, 1), threshold=component.gripper_threshold,
235
+ open_is_high=component.gripper_open_is_high,
236
+ ).reshape(gripper.shape)
237
+ motion = np.concatenate((motion, gripper), axis=2)
238
+ output[component.name] = [
239
+ trajectory.ActiveTrajectory(row, component.active_dims) for row in motion
240
+ ]
241
+ return output
242
+
243
+ presets.PresetConfig.canonicalize = canonicalize
244
+ original_encode = physical.ActionCodec2Processor.encode_batch
245
+ original_decode = physical.ActionCodec2Processor.decode_batch
246
+
247
+ def serializer(self):
248
+ native = native_now()
249
+ if native is None or self.serializer.scheme != "set_bpe":
250
+ return None
251
+ if not hasattr(self, "_portable_setbpe"):
252
+ saved = self.serializer
253
+ self._portable_setbpe = native.SetBPE(
254
+ saved.bins, saved.lookahead, saved.spatial_merges, saved.temporal_merges
255
+ )
256
+ return self._portable_setbpe
257
+
258
+ @functools.wraps(original_encode)
259
+ def encode_batch(self, actions, *, active_dims=None, previous_first_order=None):
260
+ codec = serializer(self)
261
+ if codec is None:
262
+ return original_encode(self, actions, active_dims=active_dims,
263
+ previous_first_order=previous_first_order)
264
+ values = np.asarray(actions, dtype=np.float32)
265
+ if values.ndim != 3 or min(values.shape) < 1:
266
+ raise ValueError("actions must have non-empty shape [B, T, D_active]")
267
+ dims = validation.active_dimensions(active_dims, self.action_dim)
268
+ if len(dims) != values.shape[2]:
269
+ raise ValueError("active_dims must have one entry per action coordinate")
270
+ dense = np.zeros((*values.shape[:2], self.action_dim), dtype=np.float32)
271
+ dense[..., dims] = values
272
+ previous = self._dense_previous_first_order(previous_first_order, dims, len(values))
273
+ cells = self.quantize_batch(dense, previous_first_order=previous)
274
+ absent = np.ones(self.action_dim, dtype=bool)
275
+ absent[list(dims)] = False
276
+ cells[..., absent] = -1
277
+ return codec.encode_batch(cells)
278
+
279
+ @functools.wraps(original_decode)
280
+ def decode_batch(self, streams, *, active_dims=None, previous_first_order=None):
281
+ codec = serializer(self)
282
+ if codec is None:
283
+ return original_decode(self, streams, active_dims=active_dims,
284
+ previous_first_order=previous_first_order)
285
+ dims = validation.active_dimensions(active_dims, self.action_dim)
286
+ rows = list(streams)
287
+ decoder = getattr(codec, "decode_checked_batch", None)
288
+ if decoder is None:
289
+ for index, stream in enumerate(rows):
290
+ checked = []
291
+ for token in stream:
292
+ if isinstance(token, bool) or not isinstance(token, Integral):
293
+ raise ValueError("tokens must be integers")
294
+ if not 0 <= int(token) < self.vocab_size:
295
+ raise ValueError("token is outside the vocabulary")
296
+ checked.append(int(token))
297
+ rows[index] = checked
298
+ decoder = codec.decode_batch
299
+ cells = decoder(rows, self.action_dim, sum(1 << d for d in dims))
300
+ previous = self._dense_previous_first_order(previous_first_order, dims, len(rows))
301
+ decoded = self.physical.decode_batch(
302
+ self._fill_inactive_cells(cells), previous_first_order=previous
303
+ )
304
+ return np.take(decoded, dims, axis=2)
305
+
306
+ physical.ActionCodec2Processor.encode_batch = encode_batch
307
+ physical.ActionCodec2Processor.decode_batch = decode_batch
308
+
309
+ exporting = importlib.import_module(f"{package}.integration.exporting")
310
+ for name in ("hub_acceleration.py", "hub_entry.py"):
311
+ if name not in exporting.MODULE_FILES:
312
+ exporting.MODULE_FILES.append(name)
313
+ original_export = exporting.ArtifactExporter.export
314
+
315
+ @functools.wraps(original_export)
316
+ def export(self):
317
+ import shutil
318
+
319
+ files = original_export(self)
320
+ source = self.package_root.parent
321
+ for name in ("setup.py", "pyproject.toml", "LICENSE"):
322
+ path = source / name
323
+ target = self.output / name
324
+ if path.is_file() and path.resolve() != target.resolve():
325
+ shutil.copyfile(path, target)
326
+ files.append(str(target))
327
+ kernels = source / "actioncodec2_kernels"
328
+ if kernels.is_dir() and kernels.resolve() != (self.output / kernels.name).resolve():
329
+ shutil.copytree(kernels, self.output / kernels.name, dirs_exist_ok=True,
330
+ ignore=shutil.ignore_patterns("__pycache__", "*.so", "*.pyc"))
331
+ return files
332
+
333
+ exporting.ArtifactExporter.export = export
runtime/hub_entry.py ADDED
@@ -0,0 +1,219 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Load a named ActionCodec2 variant or a standalone saved processor."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import hashlib
6
+ import importlib
7
+ import importlib.util
8
+ import json
9
+ from pathlib import Path
10
+ import re
11
+ import sys
12
+
13
+
14
+ PROFILE_FILES = ("config.yaml", "primitives.npz", "bpe.json", "config.json", "processor_config.json")
15
+ SHARED_PATTERNS = (
16
+ "config.json", "processor_config.json", "processing_actioncodec2.py", "variants.json",
17
+ "runtime/**", "actioncodec2_kernels/**", "setup.py", "pyproject.toml", "LICENSE",
18
+ "README.md", "requirements.txt",
19
+ )
20
+
21
+
22
+ def asset_inventory(directory: Path) -> dict[str, str]:
23
+ """Hash the complete configuration and fitted assets for one variant."""
24
+ names = ["router_config.yaml", "config.json", "processor_config.json"]
25
+ profiles = [name for name in ("eef", "joint") if (directory / "profiles" / name).is_dir()]
26
+ if not profiles:
27
+ raise ValueError("variant has no fitted profiles")
28
+ names.extend(f"profiles/{profile}/{name}" for profile in profiles for name in PROFILE_FILES)
29
+ return {name: hashlib.sha256((directory / name).read_bytes()).hexdigest()
30
+ for name in sorted(names)}
31
+
32
+
33
+ def inventory_fingerprint(files: dict[str, str]) -> str:
34
+ """Return the stable identity of a fitted asset inventory."""
35
+ return hashlib.sha256(json.dumps(files, sort_keys=True, separators=(",", ":")).encode()).hexdigest()
36
+
37
+
38
+ def _check_inventory(directory: Path, record: dict) -> None:
39
+ files = asset_inventory(directory)
40
+ if files != record.get("files") or inventory_fingerprint(files) != record.get("fingerprint"):
41
+ raise ValueError("ActionCodec2 asset fingerprint mismatch; restore the complete selected variant")
42
+
43
+
44
+ class _VariantBinding:
45
+ @property
46
+ def variant(self):
47
+ return self._variant_origin.get("name")
48
+
49
+ @property
50
+ def variant_fingerprint(self):
51
+ return self._variant_origin.get("fingerprint")
52
+
53
+ def for_action_space(self, name):
54
+ codec = super().for_action_space(name)
55
+ codec._variant_origin = dict(self._variant_origin)
56
+ return codec
57
+
58
+ def fit(self, *args, **kwargs):
59
+ result = super().fit(*args, **kwargs)
60
+ self._variant_origin = {}
61
+ return result
62
+
63
+ def save_pretrained(self, save_directory, push_to_hub=False, **kwargs):
64
+ directory = Path(save_directory)
65
+ if (directory / "variants.json").is_file():
66
+ raise ValueError("save a selected variant to a separate directory, outside the catalog root")
67
+ if not push_to_hub and kwargs:
68
+ raise TypeError(f"unused save_pretrained keyword(s): {', '.join(sorted(kwargs))}")
69
+ files = super().save_pretrained(directory)
70
+ inventory = asset_inventory(directory)
71
+ manifest = directory / "artifact_manifest.json"
72
+ manifest.write_text(json.dumps({
73
+ "format_version": 1,
74
+ "origin": self._variant_origin,
75
+ "files": inventory,
76
+ "fingerprint": inventory_fingerprint(inventory),
77
+ }, indent=2) + "\n", encoding="utf-8")
78
+ files.append(str(manifest))
79
+ if push_to_hub:
80
+ package = type(self.action_tokenizer).__module__.split(".routing.", 1)[0]
81
+ importlib.import_module(f"{package}.integration.hf").upload_pretrained_directory(
82
+ directory, **kwargs
83
+ )
84
+ return files
85
+
86
+
87
+ class ActionCodec2:
88
+ """Hugging Face entry point for catalog and standalone artifacts."""
89
+
90
+ @classmethod
91
+ def register_for_auto_class(cls, auto_class="AutoProcessor"):
92
+ return cls
93
+
94
+ @classmethod
95
+ def from_pretrained(cls, pretrained_model_name_or_path, *, variant=None,
96
+ variant_fingerprint=None, action_space=None, **kwargs):
97
+ source = Path(pretrained_model_name_or_path).expanduser()
98
+ kwargs.pop("_from_auto", None)
99
+ kwargs.pop("trust_remote_code", None)
100
+ subfolder = kwargs.pop("subfolder", "")
101
+ if subfolder:
102
+ raise ValueError("select an ActionCodec2 catalog variant with variant=, not subfolder=")
103
+ allowed = {"cache_dir", "force_download", "local_files_only", "token", "revision", "repo_type"}
104
+ unknown = sorted(set(kwargs) - allowed)
105
+ if unknown:
106
+ raise TypeError(f"unsupported from_pretrained keyword(s): {', '.join(unknown)}")
107
+ if variant is not None and (not isinstance(variant, str) or
108
+ re.fullmatch(r"[a-z0-9]+(?:-[a-z0-9]+)*", variant) is None):
109
+ raise ValueError("variant must be a published variant name")
110
+ if variant_fingerprint is not None and (
111
+ not isinstance(variant_fingerprint, str) or
112
+ re.fullmatch(r"[0-9a-f]{64}", variant_fingerprint) is None
113
+ ):
114
+ raise ValueError("variant_fingerprint must be a SHA-256 fingerprint")
115
+ remote = not source.is_dir()
116
+ if remote and str(pretrained_model_name_or_path).startswith(("/", "./", "../", "~")):
117
+ raise FileNotFoundError(f"ActionCodec2 artifact does not exist: {source}")
118
+ revision = None
119
+ if remote:
120
+ from huggingface_hub import hf_hub_download, snapshot_download
121
+
122
+ code_revision = Path(__file__).parent.name
123
+ if re.fullmatch(r"[0-9a-f]{40}", code_revision):
124
+ requested = kwargs.get("revision")
125
+ if requested and re.fullmatch(r"[0-9a-f]{40}", requested) and requested != code_revision:
126
+ raise ValueError("ActionCodec2 code and assets must use the same revision")
127
+ kwargs["revision"] = code_revision
128
+ metadata_file = Path(hf_hub_download(
129
+ str(pretrained_model_name_or_path), "processor_config.json", **kwargs
130
+ ))
131
+ root = metadata_file.parent
132
+ revision = root.name
133
+ kwargs["revision"] = revision
134
+ else:
135
+ root = source
136
+ metadata_file = root / "processor_config.json"
137
+ metadata = json.loads(metadata_file.read_text(encoding="utf-8"))
138
+ if not isinstance(metadata, dict):
139
+ raise ValueError("invalid ActionCodec2 processor configuration")
140
+ record = None
141
+ origin = {}
142
+ if metadata.get("variant_catalog") == "variants.json":
143
+ catalog_path = (Path(hf_hub_download(
144
+ str(pretrained_model_name_or_path), "variants.json", **kwargs
145
+ )) if remote else root / "variants.json")
146
+ catalog = json.loads(catalog_path.read_text(encoding="utf-8"))
147
+ if (not isinstance(catalog, dict) or catalog.get("format_version") != 1 or
148
+ not isinstance(catalog.get("variants"), dict)):
149
+ raise ValueError("unsupported ActionCodec2 variant catalog")
150
+ variants = catalog["variants"]
151
+ if variant not in variants:
152
+ raise ValueError(f"select variant= from: {', '.join(sorted(variants))}")
153
+ record = variants[variant]
154
+ if not isinstance(record, dict):
155
+ raise ValueError("invalid ActionCodec2 variant record")
156
+ directory = root / "variants" / variant
157
+ if remote:
158
+ root = Path(snapshot_download(
159
+ str(pretrained_model_name_or_path),
160
+ allow_patterns=[*SHARED_PATTERNS, f"variants/{variant}/**"], **kwargs,
161
+ ))
162
+ directory = root / "variants" / variant
163
+ _check_inventory(directory, record)
164
+ origin = {"name": variant, "fingerprint": record["fingerprint"]}
165
+ if remote:
166
+ origin.update(repo_id=str(pretrained_model_name_or_path), revision=revision)
167
+ else:
168
+ if remote:
169
+ root = Path(snapshot_download(
170
+ str(pretrained_model_name_or_path),
171
+ allow_patterns=[*SHARED_PATTERNS, "router_config.yaml", "profiles/**",
172
+ "artifact_manifest.json"], **kwargs,
173
+ ))
174
+ directory = root
175
+ manifest = root / "artifact_manifest.json"
176
+ if manifest.is_file():
177
+ record = json.loads(manifest.read_text(encoding="utf-8"))
178
+ if (not isinstance(record, dict) or record.get("format_version") != 1 or
179
+ not isinstance(record.get("origin"), dict)):
180
+ raise ValueError("unsupported ActionCodec2 saved artifact manifest")
181
+ _check_inventory(directory, record)
182
+ origin = record["origin"]
183
+ if variant is not None and variant != origin.get("name"):
184
+ raise ValueError("saved processor contains a different variant; load the catalog to switch")
185
+ if variant_fingerprint is not None and variant_fingerprint != origin.get("fingerprint"):
186
+ raise ValueError("variant fingerprint differs from the tokenizer bound to this policy")
187
+
188
+ runtime = root / "runtime"
189
+ package_file = runtime / "__init__.py"
190
+ if not package_file.is_file():
191
+ raise FileNotFoundError(f"ActionCodec2 runtime is missing from {root}")
192
+ identity = hashlib.sha256(str(runtime.resolve()).encode()).hexdigest()[:16]
193
+ package_name = f"_actioncodec2_artifact_{identity}"
194
+ if package_name not in sys.modules:
195
+ spec = importlib.util.spec_from_file_location(
196
+ package_name, package_file, submodule_search_locations=[str(runtime)]
197
+ )
198
+ if spec is None or spec.loader is None:
199
+ raise ImportError(f"cannot load ActionCodec2 runtime from {runtime}")
200
+ package = importlib.util.module_from_spec(spec)
201
+ sys.modules[package_name] = package
202
+ try:
203
+ spec.loader.exec_module(package)
204
+ except BaseException:
205
+ del sys.modules[package_name]
206
+ raise
207
+ processor = importlib.import_module(f"{package_name}.processing_actioncodec2")
208
+ if (runtime / "hub_acceleration.py").is_file():
209
+ acceleration = importlib.import_module(f"{package_name}.hub_acceleration")
210
+ acceleration.install(package_name)
211
+ if not hasattr(processor, "_catalog_processor_class"):
212
+ processor._catalog_processor_class = type(
213
+ "ActionCodec2", (_VariantBinding, processor.ActionCodec2),
214
+ {"__module__": processor.__name__},
215
+ )
216
+ codec = processor._catalog_processor_class.from_pretrained(directory, action_space=action_space)
217
+ codec._variant_origin = origin
218
+ codec._source_directory = root
219
+ return codec
runtime/integration/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Internal integration components for ActionCodec2."""
runtime/integration/exporting.py ADDED
@@ -0,0 +1,48 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Write the portable runtime and user guide for a saved ActionCodec2 artifact."""
2
+ import shutil
3
+ from pathlib import Path
4
+ MODULE_FILES: list[str]
5
+ SUPPORT_FILES: dict[str, str]
6
+ USER_GUIDE: str
7
+
8
+ class ArtifactExporter:
9
+ """Copy portable code alongside numerical artifacts."""
10
+
11
+ def __init__(self, directory: str | Path, *, source_directory: str | Path | None=None) -> None:
12
+ self.output = Path(directory)
13
+ self.package_root = Path(__file__).resolve().parent.parent
14
+ self.source = Path(source_directory) if source_directory else self.package_root
15
+
16
+ def export(self) -> list[str]:
17
+ self.output.mkdir(parents=True, exist_ok=True)
18
+ files = []
19
+ runtime = self.output / 'runtime'
20
+ runtime.mkdir(parents=True, exist_ok=True)
21
+ for name in MODULE_FILES:
22
+ source = self.package_root / name
23
+ if not source.is_file():
24
+ source = self.source / name
25
+ target = runtime / name
26
+ target.parent.mkdir(parents=True, exist_ok=True)
27
+ if source.resolve() != target.resolve():
28
+ shutil.copyfile(source, target)
29
+ files.append(str(target))
30
+ (runtime / '__init__.py').write_text('"""Private inference runtime bundled with this ActionCodec2 artifact."""\n', encoding='utf-8')
31
+ entry = self.package_root / 'hub_entry.py'
32
+ if not entry.is_file():
33
+ entry = self.source / 'runtime' / 'hub_entry.py'
34
+ target = self.output / 'processing_actioncodec2.py'
35
+ if entry.resolve() != target.resolve():
36
+ shutil.copyfile(entry, target)
37
+ files.append(str(target))
38
+ for name in ('requirements.txt',):
39
+ content = SUPPORT_FILES[name]
40
+ target = self.output / name
41
+ target.write_text(content, encoding='utf-8')
42
+ files.append(str(target))
43
+ if not (self.output / 'README.md').exists():
44
+ (self.output / 'README.md').write_text(USER_GUIDE, encoding='utf-8')
45
+ return files
46
+ USER_GUIDE = '# Saved ActionCodec2 processor\n\nLoad this directory with `AutoProcessor.from_pretrained(path, trust_remote_code=True, local_files_only=True)`. Keep all files together.\nThe action-space binding and selected fitted assets are preserved.\nSecond-order motion requires `previous_first_order`; absolute layouts require `current_state`.\n'
47
+ SUPPORT_FILES = {'requirements.txt': 'numpy\nscipy\ntorch>=2.3\ntransformers>=4.57,<6\nhuggingface-hub\npyyaml\n'}
48
+ MODULE_FILES = ['backend/__init__.py', 'backend/native_backend.py', 'common/__init__.py', 'common/_frontend_base.py', 'common/_types.py', 'common/_validation.py', 'frontend/__init__.py', 'frontend/tokenizer.py', 'integration/__init__.py', 'integration/exporting.py', 'integration/hf.py', 'physical/__init__.py', 'physical/_so3.py', 'physical/boundary.py', 'physical/configuration.py', 'physical/physical_codec.py', 'physical/physical_processor.py', 'physical/primitives.py', 'physical/semantic.py', 'physical/trajectory.py', 'routing/__init__.py', 'routing/cache.py', 'routing/defaults.py', 'routing/presets.py', 'routing/routing.py', 'tokenization/__init__.py', 'tokenization/bpe.py', 'tokenization/bpe_training.py', 'tokenization/bpe_vocabulary.py', 'tokenization/generation.py', 'tokenization/grammar.py', 'tokenization/setbpe_spatial.py', 'tokenization/setbpe_spatial_training.py', 'tokenization/setbpe_temporal.py', 'tokenization/setbpe_temporal_training.py', 'training/__init__.py', 'training/fitting.py', 'training/training.py', 'processing_actioncodec2.py']
runtime/integration/hf.py ADDED
@@ -0,0 +1,189 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ "Hugging Face AutoProcessor integration."
2
+
3
+ from __future__ import annotations
4
+
5
+ import json
6
+ from pathlib import Path
7
+ from typing import TYPE_CHECKING, Any
8
+
9
+ from transformers import AutoConfig, AutoProcessor, PretrainedConfig
10
+
11
+ from ..physical.configuration import MODEL_TYPE
12
+ from .exporting import USER_GUIDE
13
+
14
+ if TYPE_CHECKING:
15
+ from ..physical.physical_processor import ActionCodec2Processor
16
+ from ..routing.routing import ActionCodec2Router
17
+
18
+ HF_CONFIG_NAME = "config.json"
19
+ PROCESSOR_CONFIG_NAME = "processor_config.json"
20
+ PROCESSOR_CLASS_NAME = "ActionCodec2Processor"
21
+ ROUTER_MODEL_TYPE = "actioncodec2_router"
22
+ ROUTER_PROCESSOR_CLASS_NAME = "ActionCodec2Router"
23
+ ROUTER_CONFIG_NAME = "router_config.yaml"
24
+ ROUTER_ALLOW_PATTERNS = [
25
+ ROUTER_CONFIG_NAME,
26
+ HF_CONFIG_NAME,
27
+ PROCESSOR_CONFIG_NAME,
28
+ "profiles/**",
29
+ "fit_report.json",
30
+ "README.md",
31
+ "*.py",
32
+ "requirements.txt",
33
+ "runtime/**", "actioncodec2_kernels/**", "setup.py", "pyproject.toml", "LICENSE",
34
+
35
+ "native/*",
36
+ ]
37
+
38
+
39
+ class ActionCodec2FrontendHFConfig(PretrainedConfig):
40
+ """AutoConfig discovery key for the action-space-bound public processor."""
41
+
42
+ model_type = "actioncodec2"
43
+
44
+ def __init__(self, **kwargs: Any) -> None:
45
+ kwargs.setdefault("processor_class", "ActionCodec2")
46
+ super().__init__(**kwargs)
47
+
48
+
49
+ class ActionCodec2HFConfig(PretrainedConfig):
50
+ """Minimal AutoConfig key that dispatches to :class:`ActionCodec2Processor`."""
51
+
52
+ model_type = MODEL_TYPE
53
+
54
+ def __init__(self, **kwargs: Any) -> None:
55
+ kwargs.setdefault("processor_class", PROCESSOR_CLASS_NAME)
56
+ super().__init__(**kwargs)
57
+
58
+
59
+ class ActionCodec2RouterHFConfig(PretrainedConfig):
60
+ """Minimal AutoConfig key that dispatches to :class:`ActionCodec2Router`."""
61
+
62
+ model_type = ROUTER_MODEL_TYPE
63
+
64
+ def __init__(self, **kwargs: Any) -> None:
65
+ kwargs.setdefault("processor_class", ROUTER_PROCESSOR_CLASS_NAME)
66
+ super().__init__(**kwargs)
67
+
68
+
69
+ def save_huggingface_metadata(
70
+ directory: str | Path,
71
+ *,
72
+ model_type: str = MODEL_TYPE,
73
+ processor_class: str = PROCESSOR_CLASS_NAME,
74
+ **processor_config: Any,
75
+ ) -> tuple[Path, Path]:
76
+ """Write standard local/Hub discovery files for ``AutoProcessor``."""
77
+ root = Path(directory)
78
+ root.mkdir(parents=True, exist_ok=True)
79
+ config_path = root / HF_CONFIG_NAME
80
+ processor_path = root / PROCESSOR_CONFIG_NAME
81
+ config_path.write_text(
82
+ json.dumps(
83
+ {
84
+ "model_type": model_type,
85
+ "processor_class": processor_class,
86
+ },
87
+ indent=2,
88
+ )
89
+ + "\n",
90
+ encoding="utf-8",
91
+ )
92
+ processor_path.write_text(
93
+ json.dumps({"processor_class": processor_class, **processor_config}, indent=2)
94
+ + "\n",
95
+ encoding="utf-8",
96
+ )
97
+ return config_path, processor_path
98
+
99
+
100
+ def resolve_pretrained_directory(
101
+ pretrained_model_name_or_path: str | Path,
102
+ *,
103
+ allow_patterns: list[str] | None = None,
104
+ **kwargs: Any,
105
+ ) -> Path:
106
+ """Resolve a local directory or download one immutable Hub snapshot."""
107
+ source = Path(pretrained_model_name_or_path)
108
+ kwargs.pop("_from_auto", None)
109
+ kwargs.pop("trust_remote_code", None)
110
+ subfolder = str(kwargs.pop("subfolder", ""))
111
+ allowed = {
112
+ "cache_dir",
113
+ "force_download",
114
+ "local_files_only",
115
+ "token",
116
+ "revision",
117
+ "repo_type",
118
+ }
119
+ unknown = sorted(set(kwargs) - allowed)
120
+ if unknown:
121
+ raise TypeError(f"unsupported from_pretrained keyword(s): {', '.join(unknown)}")
122
+ if source.is_dir():
123
+ return source / subfolder
124
+
125
+ try:
126
+ from huggingface_hub import snapshot_download
127
+ except ImportError as error:
128
+ raise ImportError(
129
+ "loading a Hub artifact requires huggingface_hub (installed with Transformers)"
130
+ ) from error
131
+ patterns = allow_patterns or [
132
+ "config.yaml",
133
+ "primitives.npz",
134
+ "bpe.json",
135
+ HF_CONFIG_NAME,
136
+ PROCESSOR_CONFIG_NAME,
137
+ ]
138
+ if subfolder:
139
+ prefix = subfolder.rstrip("/")
140
+ patterns = [f"{prefix}/{pattern}" for pattern in patterns]
141
+ snapshot = snapshot_download(
142
+ repo_id=str(pretrained_model_name_or_path),
143
+ allow_patterns=patterns,
144
+ **kwargs,
145
+ )
146
+ return Path(snapshot) / subfolder
147
+
148
+
149
+ def upload_pretrained_directory(directory: Path, **kwargs: Any) -> None:
150
+ """Upload a complete, already-saved processor artifact to the Hub."""
151
+ from huggingface_hub import HfApi
152
+
153
+ repo_id = kwargs.pop("repo_id", directory.name)
154
+ commit_message = kwargs.pop("commit_message", "Upload ActionCodec2 processor")
155
+ api = HfApi(token=kwargs.pop("token", None))
156
+ api.create_repo(repo_id=repo_id, exist_ok=True, **kwargs)
157
+ api.upload_folder(
158
+ repo_id=repo_id,
159
+ folder_path=str(directory),
160
+ commit_message=commit_message,
161
+ )
162
+
163
+
164
+ def register_actioncodec2_auto_processors(
165
+ processor_class: type[ActionCodec2Processor],
166
+ router_class: type[ActionCodec2Router],
167
+ ) -> bool:
168
+ "Register both installed ActionCodec2 artifact types with Transformers."
169
+ AutoConfig.register(MODEL_TYPE, ActionCodec2HFConfig, exist_ok=True)
170
+ AutoProcessor.register(ActionCodec2HFConfig, processor_class, exist_ok=True)
171
+ AutoConfig.register(ROUTER_MODEL_TYPE, ActionCodec2RouterHFConfig, exist_ok=True)
172
+ AutoProcessor.register(ActionCodec2RouterHFConfig, router_class, exist_ok=True)
173
+ return True
174
+
175
+
176
+ def save_model_card(directory: str | Path, description: str) -> Path:
177
+ """Create an informative Hub card on first save, preserving existing user cards."""
178
+ path = Path(directory) / "README.md"
179
+ if not path.exists():
180
+ path.parent.mkdir(parents=True, exist_ok=True)
181
+ guide = USER_GUIDE
182
+ path.write_text(
183
+ guide
184
+ + "\n\n## Artifact-specific report\n\n```text\n"
185
+ + description
186
+ + "\n```\n",
187
+ encoding="utf-8",
188
+ )
189
+ return path
runtime/physical/__init__.py ADDED
@@ -0,0 +1 @@
 
 
1
+ """Internal physical components for ActionCodec2."""
runtime/physical/_so3.py ADDED
@@ -0,0 +1,99 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Shared NumPy quaternion operations using the xyzw convention."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import numpy as np
6
+
7
+
8
+ def normalize_quaternion(values: np.ndarray) -> np.ndarray:
9
+ """Normalize quaternion rows and reject an undefined zero rotation."""
10
+ quaternions = np.asarray(values, dtype=np.float64)
11
+ norms = np.linalg.norm(quaternions, axis=-1, keepdims=True)
12
+ if np.any(norms < 1e-12):
13
+ raise ValueError("a pose contains a zero quaternion")
14
+ return quaternions / norms
15
+
16
+
17
+ def multiply_quaternions(left: np.ndarray, right: np.ndarray) -> np.ndarray:
18
+ """Return the Hamilton product of broadcastable xyzw arrays."""
19
+ first = np.asarray(left, dtype=np.float64)
20
+ second = np.asarray(right, dtype=np.float64)
21
+ first_vector, first_scalar = first[..., :3], first[..., 3:4]
22
+ second_vector, second_scalar = second[..., :3], second[..., 3:4]
23
+ return normalize_quaternion(
24
+ np.concatenate(
25
+ (
26
+ first_scalar * second_vector
27
+ + second_scalar * first_vector
28
+ + np.cross(first_vector, second_vector),
29
+ first_scalar * second_scalar
30
+ - np.sum(first_vector * second_vector, axis=-1, keepdims=True),
31
+ ),
32
+ axis=-1,
33
+ )
34
+ )
35
+
36
+
37
+ def conjugate_quaternion(quaternion: np.ndarray) -> np.ndarray:
38
+ """Return the conjugate of an xyzw quaternion array."""
39
+ result = np.asarray(quaternion, dtype=np.float64).copy()
40
+ result[..., :3] *= -1.0
41
+ return result
42
+
43
+
44
+ def rotvec_to_quaternion(rotvec: np.ndarray) -> np.ndarray:
45
+ """Convert rotation vectors to xyzw quaternions."""
46
+ values = np.asarray(rotvec, dtype=np.float64)
47
+ angle = np.linalg.norm(values, axis=-1)
48
+ scale = np.full_like(angle, 0.5)
49
+ regular = angle >= 1e-12
50
+ scale[regular] = np.sin(0.5 * angle[regular]) / angle[regular]
51
+ return normalize_quaternion(
52
+ np.concatenate(
53
+ (values * scale[..., None], np.cos(0.5 * angle)[..., None]), axis=-1
54
+ )
55
+ )
56
+
57
+
58
+ def quaternion_to_rotvec(quaternion: np.ndarray) -> np.ndarray:
59
+ """Convert xyzw quaternions to shortest-path rotation vectors."""
60
+ values = normalize_quaternion(quaternion).copy()
61
+ values[values[..., 3] < 0.0] *= -1.0
62
+ vector_norm = np.linalg.norm(values[..., :3], axis=-1)
63
+ angle = 2.0 * np.arctan2(vector_norm, np.clip(values[..., 3], 0.0, 1.0))
64
+ scale = np.full_like(angle, 2.0)
65
+ regular = vector_norm >= 1e-12
66
+ scale[regular] = angle[regular] / vector_norm[regular]
67
+ return values[..., :3] * scale[..., None]
68
+
69
+
70
+ def euler_xyz_to_quaternion(euler: np.ndarray) -> np.ndarray:
71
+ """Convert ``(N,3)`` XYZ Euler angles to xyzw quaternions."""
72
+ values = np.asarray(euler, dtype=np.float64)
73
+ if values.ndim != 2 or values.shape[1] != 3:
74
+ raise ValueError("XYZ Euler angles must have shape [N, 3]")
75
+ half = 0.5 * values
76
+ sin_roll, sin_pitch, sin_yaw = np.sin(half).T
77
+ cos_roll, cos_pitch, cos_yaw = np.cos(half).T
78
+ result = np.empty((len(values), 4), dtype=np.float64)
79
+ result[:, 0] = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw
80
+ result[:, 1] = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw
81
+ result[:, 2] = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw
82
+ result[:, 3] = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw
83
+ return normalize_quaternion(result)
84
+
85
+
86
+ def quaternion_to_euler_xyz(quaternion: np.ndarray) -> np.ndarray:
87
+ """Convert ``(N,4)`` xyzw quaternions to XYZ Euler angles."""
88
+ values = normalize_quaternion(quaternion)
89
+ if values.ndim != 2 or values.shape[1] != 4:
90
+ raise ValueError("xyzw quaternions must have shape [N, 4]")
91
+ x, y, z, w = values.T
92
+ return np.stack(
93
+ (
94
+ np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y)),
95
+ np.arcsin(np.clip(2.0 * (w * y - z * x), -1.0, 1.0)),
96
+ np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)),
97
+ ),
98
+ axis=1,
99
+ )
runtime/physical/boundary.py ADDED
@@ -0,0 +1,134 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Explicit measured-context preparation for stateful trajectory fitting."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import numpy as np
6
+
7
+ from .configuration import ActionCodec2Config
8
+ from .physical_codec import _scale, _SO3Runtime
9
+ from .trajectory import (
10
+ ActiveTrajectory,
11
+ TrajectoryLike,
12
+ prepare_trajectory,
13
+ trajectory_boundary,
14
+ )
15
+
16
+
17
+ def source_differences(
18
+ trajectory: TrajectoryLike, config: ActionCodec2Config
19
+ ) -> dict[str, tuple[tuple[int, ...], np.ndarray, np.ndarray]]:
20
+ "Measure physical first/second differences with the declared group law."
21
+ values, dims = prepare_trajectory(
22
+ trajectory, dimension=config.physical.input_dim, index=0
23
+ )
24
+ boundary = trajectory_boundary(trajectory, config, dims)
25
+ output = {}
26
+ for name, component in config.physical.components.items():
27
+ present = tuple(d for d in component.dims if d in dims)
28
+ if not present or component.primitive_order == 0:
29
+ continue
30
+ local = [dims.index(d) for d in present]
31
+ indices = [component.dims.index(d) for d in present]
32
+ scale = np.take(_scale(component), indices)
33
+ raw = values[:, local].astype(np.float64) * scale
34
+ previous = (
35
+ None
36
+ if boundary is None
37
+ else boundary[list(present)].astype(np.float64) * scale
38
+ )
39
+ if component.space == "additive":
40
+ if component.input_order == 0:
41
+ first = np.diff(raw, axis=0, prepend=np.zeros((1, len(present))))
42
+ elif component.input_order == 1:
43
+ first = raw
44
+ else:
45
+ if previous is None:
46
+ raise ValueError(
47
+ "order-2 input statistics require previous_first_order"
48
+ )
49
+ first = np.cumsum(raw, axis=0) + previous
50
+ second = (
51
+ np.diff(first, axis=0)
52
+ if previous is None
53
+ else np.diff(first, axis=0, prepend=previous[None])
54
+ )
55
+ else:
56
+ if len(present) != 3:
57
+ raise ValueError(
58
+ "differential statistics require complete SO(3) components"
59
+ )
60
+ runtime = _SO3Runtime(component, (np.zeros(1),) * 3)
61
+ quaternion = runtime._from_representation(raw)
62
+ if component.input_order == 0:
63
+ anchors = np.concatenate(
64
+ (np.array([[0.0, 0.0, 0.0, 1.0]]), quaternion[:-1])
65
+ )
66
+ increments = runtime._residual(quaternion, anchors)
67
+ else:
68
+ increments = quaternion
69
+ first = runtime._to_representation(increments)
70
+ if previous is None:
71
+ changes = runtime._residual(increments[1:], increments[:-1])
72
+ else:
73
+ anchors = np.concatenate(
74
+ (runtime._from_representation(previous[None]), increments[:-1])
75
+ )
76
+ changes = runtime._residual(increments, anchors)
77
+ second = runtime._to_representation(changes)
78
+ output[name] = present, first, second
79
+ return output
80
+
81
+
82
+ def bootstrap_second_order_trajectory(
83
+ trajectory: ActiveTrajectory, config: ActionCodec2Config
84
+ ) -> ActiveTrajectory:
85
+ "Use two observed positions as context and encode the remaining suffix."
86
+ values, dims = prepare_trajectory(
87
+ trajectory, dimension=config.physical.input_dim, index=0
88
+ )
89
+ if len(values) < 3:
90
+ raise ValueError(
91
+ "second-order bootstrap requires two context frames and at least one target"
92
+ )
93
+ if trajectory.previous_first_order is not None:
94
+ raise ValueError(
95
+ "trajectory already has previous_first_order; do not bootstrap it again"
96
+ )
97
+ result = values[2:].copy()
98
+ previous = np.zeros(len(dims), dtype=np.float32)
99
+ for component in config.physical.components.values():
100
+ local = [dims.index(d) for d in component.dims if d in dims]
101
+ if not local:
102
+ continue
103
+ if component.input_order != 0:
104
+ raise ValueError("measured-context bootstrap requires input_order=0")
105
+ if component.primitive_order == 0:
106
+ continue
107
+ if component.space == "additive":
108
+ result[:, local] -= values[1, local]
109
+ previous[local] = values[1, local] - values[0, local]
110
+ else:
111
+ if len(local) != 3:
112
+ raise ValueError("bootstrap requires complete SO(3) components")
113
+ runtime = _SO3Runtime(component, (np.zeros(1),) * 3)
114
+ quaternion = runtime._from_representation(
115
+ values[:, local] * _scale(component)
116
+ )
117
+ increment = runtime._residual(quaternion[1:2], quaternion[:1])
118
+
119
+
120
+ from ._so3 import conjugate_quaternion, multiply_quaternions
121
+
122
+ anchor = conjugate_quaternion(quaternion[1:2])
123
+ local_path = (
124
+ multiply_quaternions(quaternion[2:], anchor)
125
+ if runtime.left
126
+ else multiply_quaternions(anchor, quaternion[2:])
127
+ )
128
+ result[:, local] = runtime._to_representation(local_path) / _scale(
129
+ component
130
+ )
131
+ previous[local] = runtime._to_representation(increment)[0] / _scale(
132
+ component
133
+ )
134
+ return ActiveTrajectory(result, dims, previous_first_order=previous)
runtime/physical/configuration.py ADDED
@@ -0,0 +1,746 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Validated ``config.yaml`` model contract for ActionCodec2."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import math
6
+ import re
7
+ from collections.abc import Mapping, Sequence
8
+ from dataclasses import dataclass
9
+ from pathlib import Path
10
+ from types import MappingProxyType
11
+
12
+ import yaml
13
+
14
+ from ..common._validation import validate_dimension_partition
15
+
16
+ CONFIG_NAME = "config.yaml"
17
+
18
+ MODEL_TYPE = "actioncodec2_unified"
19
+ CONFIG_VERSION = 1
20
+ _SPACES = ("additive", "so3")
21
+ _SO3_REPRESENTATIONS = ("rotation_vector", "euler_xyz")
22
+ _SO3_MULTIPLICATIONS = ("left", "right")
23
+ _PRIMITIVE_SELECTIONS = ("nearest", "smooth")
24
+ _ORDER_KEY = re.compile(r"order_(0|[1-9][0-9]*)\Z")
25
+
26
+ NumberOrVector = float | tuple[float, ...]
27
+
28
+
29
+ class ConfigError(ValueError):
30
+ """The serialized model definition is incomplete or inconsistent."""
31
+
32
+
33
+ class _UniqueKeyLoader(yaml.SafeLoader):
34
+ pass
35
+
36
+
37
+ def _construct_unique_mapping(
38
+ loader: _UniqueKeyLoader, node: yaml.MappingNode, deep: bool = False
39
+ ) -> dict[object, object]:
40
+ loader.flatten_mapping(node)
41
+ output: dict[object, object] = {}
42
+ for key_node, value_node in node.value:
43
+ key = loader.construct_object(key_node, deep=deep)
44
+ try:
45
+ duplicate = key in output
46
+ except TypeError as error:
47
+ raise ConfigError("config.yaml contains an invalid mapping key") from error
48
+ if duplicate:
49
+ raise ConfigError(f"config.yaml contains duplicate field {key!r}")
50
+ output[key] = loader.construct_object(value_node, deep=deep)
51
+ return output
52
+
53
+
54
+ _UniqueKeyLoader.add_constructor(
55
+ yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
56
+ _construct_unique_mapping,
57
+ )
58
+
59
+
60
+ def _mapping(value: object, path: str) -> Mapping[str, object]:
61
+ if not isinstance(value, Mapping):
62
+ raise ConfigError(f"{path} must be a mapping")
63
+ if not all(isinstance(key, str) for key in value):
64
+ raise ConfigError(f"every field in {path} must be a string")
65
+ return value
66
+
67
+
68
+ def _reject_unknown(
69
+ payload: Mapping[str, object], allowed: set[str], path: str
70
+ ) -> None:
71
+ unknown = sorted(set(payload) - allowed)
72
+ if unknown:
73
+ rendered = ", ".join(unknown)
74
+ raise ConfigError(f"{path} contains unknown field(s): {rendered}")
75
+
76
+
77
+ def _required(payload: Mapping[str, object], key: str, path: str) -> object:
78
+ if key not in payload:
79
+ raise ConfigError(f"{path} is missing required field {key!r}")
80
+ return payload[key]
81
+
82
+
83
+ def _integer(value: object, path: str, *, positive: bool = False) -> int:
84
+ if isinstance(value, bool) or not isinstance(value, int):
85
+ raise ConfigError(f"{path} must be an integer")
86
+ minimum = 1 if positive else 0
87
+ if value < minimum:
88
+ qualifier = "positive" if positive else "non-negative"
89
+ raise ConfigError(f"{path} must be {qualifier}")
90
+ return int(value)
91
+
92
+
93
+ def _number(value: object, path: str) -> float:
94
+ if isinstance(value, bool) or not isinstance(value, (int, float)):
95
+ raise ConfigError(f"{path} must be a number")
96
+ result = float(value)
97
+ if not math.isfinite(result):
98
+ raise ConfigError(f"{path} must be finite")
99
+ return result
100
+
101
+
102
+ def _number_or_vector(value: object, width: int, path: str) -> NumberOrVector:
103
+ if isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)):
104
+ result = tuple(
105
+ _number(item, f"{path}[{index}]") for index, item in enumerate(value)
106
+ )
107
+ if len(result) != width:
108
+ raise ConfigError(f"{path} must be a scalar or contain {width} values")
109
+ return result
110
+ return _number(value, path)
111
+
112
+
113
+ def _expanded(value: NumberOrVector, width: int) -> tuple[float, ...]:
114
+ if isinstance(value, tuple):
115
+ return value
116
+ return (value,) * width
117
+
118
+
119
+ def _serialize_number_or_vector(value: NumberOrVector) -> float | list[float]:
120
+ return list(value) if isinstance(value, tuple) else value
121
+
122
+
123
+ def _parse_dims(value: object, input_dim: int, path: str) -> tuple[int, ...]:
124
+ if isinstance(value, Mapping):
125
+ payload = _mapping(value, path)
126
+ _reject_unknown(payload, {"start", "stop"}, path)
127
+ start = _integer(_required(payload, "start", path), f"{path}.start")
128
+ stop = _integer(_required(payload, "stop", path), f"{path}.stop")
129
+ if stop <= start:
130
+ raise ConfigError(f"{path}.stop must be greater than {path}.start")
131
+ dims = tuple(range(start, stop))
132
+ elif isinstance(value, Sequence) and not isinstance(value, (str, bytes, bytearray)):
133
+ dims = tuple(
134
+ _integer(item, f"{path}[{index}]") for index, item in enumerate(value)
135
+ )
136
+ if not dims:
137
+ raise ConfigError(f"{path} must not be empty")
138
+ else:
139
+ raise ConfigError(f"{path} must be a list or a start/stop range")
140
+
141
+ if len(set(dims)) != len(dims):
142
+ raise ConfigError(f"{path} contains a repeated dimension")
143
+ outside = [dimension for dimension in dims if dimension >= input_dim]
144
+ if outside:
145
+ raise ConfigError(
146
+ f"{path} contains dimension {outside[0]}, outside input_dim={input_dim}"
147
+ )
148
+ return dims
149
+
150
+
151
+ def _serialize_dims(dims: tuple[int, ...]) -> object:
152
+ if len(dims) > 1 and dims == tuple(range(dims[0], dims[-1] + 1)):
153
+ return {"start": dims[0], "stop": dims[-1] + 1}
154
+ return list(dims)
155
+
156
+
157
+ @dataclass(frozen=True)
158
+ class ConstraintConfig:
159
+ """Allowed inclusive interval for one differential order."""
160
+
161
+ minimum: NumberOrVector
162
+ maximum: NumberOrVector
163
+
164
+ @classmethod
165
+ def from_dict(cls, payload: object, *, width: int, path: str) -> ConstraintConfig:
166
+ values = _mapping(payload, path)
167
+ _reject_unknown(values, {"min", "max"}, path)
168
+ minimum = _number_or_vector(
169
+ _required(values, "min", path), width, f"{path}.min"
170
+ )
171
+ maximum = _number_or_vector(
172
+ _required(values, "max", path), width, f"{path}.max"
173
+ )
174
+ if any(
175
+ lower > upper
176
+ for lower, upper in zip(
177
+ _expanded(minimum, width), _expanded(maximum, width), strict=True
178
+ )
179
+ ):
180
+ raise ConfigError(f"{path}.min must not exceed {path}.max")
181
+ return cls(minimum=minimum, maximum=maximum)
182
+
183
+ def to_dict(self) -> dict[str, object]:
184
+ return {
185
+ "min": _serialize_number_or_vector(self.minimum),
186
+ "max": _serialize_number_or_vector(self.maximum),
187
+ }
188
+
189
+
190
+ @dataclass(frozen=True)
191
+ class InputTransformConfig:
192
+ """Input-unit conversion and optional source-side clipping."""
193
+
194
+ scale: NumberOrVector = 1.0
195
+ clip: bool = False
196
+
197
+ @classmethod
198
+ def from_dict(
199
+ cls, payload: object | None, *, width: int, path: str
200
+ ) -> InputTransformConfig:
201
+ if payload is None:
202
+ return cls()
203
+ values = _mapping(payload, path)
204
+ _reject_unknown(values, {"scale", "clip"}, path)
205
+ scale = _number_or_vector(values.get("scale", 1.0), width, f"{path}.scale")
206
+ if any(value <= 0.0 for value in _expanded(scale, width)):
207
+ raise ConfigError(f"{path}.scale must be positive")
208
+ clip = values.get("clip", False)
209
+ if not isinstance(clip, bool):
210
+ raise ConfigError(f"{path}.clip must be true or false")
211
+ return cls(scale=scale, clip=clip)
212
+
213
+ def to_dict(self) -> dict[str, object]:
214
+ return {
215
+ "scale": _serialize_number_or_vector(self.scale),
216
+ "clip": self.clip,
217
+ }
218
+
219
+
220
+ def _parse_component_geometry(
221
+ values: Mapping[str, object], *, input_dim: int, path: str
222
+ ) -> tuple[tuple[int, ...], str, int, int, str | None, str | None]:
223
+ dims = _parse_dims(_required(values, "dims", path), input_dim, f"{path}.dims")
224
+ space = _required(values, "space", path)
225
+ if space not in _SPACES:
226
+ raise ConfigError(f"{path}.space must be one of {_SPACES}")
227
+ input_order = _integer(
228
+ _required(values, "input_order", path), f"{path}.input_order"
229
+ )
230
+ primitive_order = _integer(
231
+ _required(values, "primitive_order", path), f"{path}.primitive_order"
232
+ )
233
+ representation = values.get("representation")
234
+ multiplication = values.get("multiplication")
235
+ if space == "so3":
236
+ if len(dims) != 3:
237
+ raise ConfigError(f"{path} with space='so3' must contain exactly 3 dims")
238
+ if input_order not in {0, 1} or primitive_order not in {1, 2}:
239
+ raise ConfigError(
240
+ f"{path} with space='so3' supports input_order 0 or 1 and primitive_order 1 or 2"
241
+ )
242
+ if representation not in _SO3_REPRESENTATIONS:
243
+ raise ConfigError(
244
+ f"{path}.representation must be one of {_SO3_REPRESENTATIONS}"
245
+ )
246
+ if multiplication not in _SO3_MULTIPLICATIONS:
247
+ raise ConfigError(
248
+ f"{path}.multiplication must be one of {_SO3_MULTIPLICATIONS}"
249
+ )
250
+ else:
251
+ if input_order not in {0, 1, 2} or primitive_order not in {0, 1, 2}:
252
+ raise ConfigError(
253
+ f"{path} with space='additive' supports input_order and primitive_order 0, 1, or 2"
254
+ )
255
+ if representation is not None or multiplication is not None:
256
+ raise ConfigError(
257
+ f"{path}.representation and multiplication are only valid for space='so3'"
258
+ )
259
+ return (
260
+ dims,
261
+ str(space),
262
+ input_order,
263
+ primitive_order,
264
+ representation,
265
+ multiplication,
266
+ )
267
+
268
+
269
+ def _parse_component_constraints(
270
+ values: Mapping[str, object], *, width: int, path: str
271
+ ) -> dict[str, ConstraintConfig]:
272
+ raw = _mapping(_required(values, "constraints", path), f"{path}.constraints")
273
+ if not raw:
274
+ raise ConfigError(f"{path}.constraints must not be empty")
275
+ constraints: dict[str, ConstraintConfig] = {}
276
+ for key, constraint in raw.items():
277
+ if _ORDER_KEY.fullmatch(key) is None:
278
+ raise ConfigError(
279
+ f"{path}.constraints field {key!r} must be named order_0, order_1, ..."
280
+ )
281
+ constraints[key] = ConstraintConfig.from_dict(
282
+ constraint, width=width, path=f"{path}.constraints.{key}"
283
+ )
284
+ return constraints
285
+
286
+
287
+ @dataclass(frozen=True)
288
+ class ComponentConfig:
289
+ "One orthogonal physical component of the action vector."
290
+
291
+ dims: tuple[int, ...]
292
+ space: str
293
+ input_order: int
294
+ primitive_order: int
295
+ input_transform: InputTransformConfig
296
+ constraints: Mapping[str, ConstraintConfig]
297
+ distortion_limit: NumberOrVector
298
+ primitive_selection: str
299
+ representation: str | None = None
300
+ multiplication: str | None = None
301
+
302
+ @classmethod
303
+ def from_dict(
304
+ cls,
305
+ payload: object,
306
+ *,
307
+ input_dim: int,
308
+ path: str,
309
+ ) -> ComponentConfig:
310
+ values = _mapping(payload, path)
311
+ allowed = {
312
+ "dims",
313
+ "space",
314
+ "input_order",
315
+ "primitive_order",
316
+ "input_transform",
317
+ "constraints",
318
+ "distortion_limit",
319
+ "primitive_selection",
320
+ "representation",
321
+ "multiplication",
322
+ }
323
+ _reject_unknown(values, allowed, path)
324
+ dims, space, input_order, primitive_order, representation, multiplication = (
325
+ _parse_component_geometry(values, input_dim=input_dim, path=path)
326
+ )
327
+ width = len(dims)
328
+ constraints = _parse_component_constraints(values, width=width, path=path)
329
+
330
+ input_transform = InputTransformConfig.from_dict(
331
+ values.get("input_transform"),
332
+ width=width,
333
+ path=f"{path}.input_transform",
334
+ )
335
+ if input_transform.clip and f"order_{input_order}" not in constraints:
336
+ raise ConfigError(
337
+ f"{path}.input_transform.clip requires constraints.order_{input_order}"
338
+ )
339
+
340
+ distortion_limit = _number_or_vector(
341
+ _required(values, "distortion_limit", path),
342
+ width,
343
+ f"{path}.distortion_limit",
344
+ )
345
+ if any(value <= 0.0 for value in _expanded(distortion_limit, width)):
346
+ raise ConfigError(f"{path}.distortion_limit must be positive")
347
+ if space == "so3" and isinstance(distortion_limit, tuple):
348
+ raise ConfigError(
349
+ f"{path}.distortion_limit must be one scalar geodesic angle for SO(3)"
350
+ )
351
+
352
+ primitive_selection = _required(values, "primitive_selection", path)
353
+ if primitive_selection not in _PRIMITIVE_SELECTIONS:
354
+ raise ConfigError(f"{path}.primitive_selection must be nearest or smooth")
355
+ if (
356
+ primitive_selection == "smooth"
357
+ and space == "additive"
358
+ and primitive_order not in {1, 2}
359
+ ):
360
+ raise ConfigError(
361
+ f"{path}.primitive_selection='smooth' requires first- or second-order additive primitives"
362
+ )
363
+ if primitive_order == 2 and "order_2" not in constraints:
364
+ raise ConfigError(
365
+ f"{path} second-order primitives require constraints.order_2"
366
+ )
367
+ return cls(
368
+ dims=dims,
369
+ space=space,
370
+ input_order=input_order,
371
+ primitive_order=primitive_order,
372
+ input_transform=input_transform,
373
+ constraints=MappingProxyType(constraints),
374
+ distortion_limit=distortion_limit,
375
+ primitive_selection=str(primitive_selection),
376
+ representation=None if representation is None else str(representation),
377
+ multiplication=None if multiplication is None else str(multiplication),
378
+ )
379
+
380
+ def to_dict(self) -> dict[str, object]:
381
+ payload: dict[str, object] = {
382
+ "dims": _serialize_dims(self.dims),
383
+ "space": self.space,
384
+ "input_order": self.input_order,
385
+ "primitive_order": self.primitive_order,
386
+ "input_transform": self.input_transform.to_dict(),
387
+ "constraints": {
388
+ key: constraint.to_dict()
389
+ for key, constraint in self.constraints.items()
390
+ },
391
+ "distortion_limit": _serialize_number_or_vector(self.distortion_limit),
392
+ "primitive_selection": self.primitive_selection,
393
+ }
394
+ if self.space == "so3":
395
+ payload["representation"] = self.representation
396
+ payload["multiplication"] = self.multiplication
397
+ return payload
398
+
399
+
400
+ @dataclass(frozen=True)
401
+ class PhysicalConfig:
402
+ """Complete, non-overlapping component partition of the input's last axis."""
403
+
404
+ input_dim: int
405
+ components: Mapping[str, ComponentConfig]
406
+
407
+ @classmethod
408
+ def from_dict(cls, payload: object, *, path: str = "physical") -> PhysicalConfig:
409
+ values = _mapping(payload, path)
410
+ _reject_unknown(values, {"input_dim", "components"}, path)
411
+ input_dim = _integer(
412
+ _required(values, "input_dim", path), f"{path}.input_dim", positive=True
413
+ )
414
+ raw_components = _mapping(
415
+ _required(values, "components", path), f"{path}.components"
416
+ )
417
+ if not raw_components:
418
+ raise ConfigError(f"{path}.components must not be empty")
419
+
420
+ components: dict[str, ComponentConfig] = {}
421
+ for name, component_payload in raw_components.items():
422
+ if not name.strip():
423
+ raise ConfigError(f"{path}.components contains an empty component name")
424
+ component = ComponentConfig.from_dict(
425
+ component_payload,
426
+ input_dim=input_dim,
427
+ path=f"{path}.components.{name}",
428
+ )
429
+ components[name] = component
430
+
431
+ validate_dimension_partition(
432
+ ((name, component.dims) for name, component in components.items()),
433
+ input_dim,
434
+ label="physical dimension",
435
+ error_type=ConfigError,
436
+ )
437
+ return cls(input_dim=input_dim, components=MappingProxyType(components))
438
+
439
+ def to_dict(self) -> dict[str, object]:
440
+ return {
441
+ "input_dim": self.input_dim,
442
+ "components": {
443
+ name: component.to_dict() for name, component in self.components.items()
444
+ },
445
+ }
446
+
447
+
448
+ TOKENIZATION_SCHEMES = ("set_bpe", "bpe")
449
+
450
+
451
+ @dataclass(frozen=True)
452
+ class SetBPEConfig:
453
+ "Runtime settings of the tokenization stage."
454
+
455
+ lookahead: int
456
+ allow_cross_component_merge: bool = False
457
+ scheme: str = "set_bpe"
458
+
459
+ @classmethod
460
+ def from_dict(cls, payload: object, *, path: str = "set_bpe") -> SetBPEConfig:
461
+ values = _mapping(payload, path)
462
+ _reject_unknown(
463
+ values, {"lookahead", "allow_cross_component_merge", "scheme"}, path
464
+ )
465
+ lookahead = _integer(
466
+ _required(values, "lookahead", path), f"{path}.lookahead", positive=True
467
+ )
468
+ allow_cross = values.get("allow_cross_component_merge", False)
469
+ if not isinstance(allow_cross, bool):
470
+ raise ConfigError(
471
+ f"{path}.allow_cross_component_merge must be true or false"
472
+ )
473
+ scheme = values.get("scheme", "set_bpe")
474
+ if scheme not in TOKENIZATION_SCHEMES:
475
+ raise ConfigError(
476
+ f"{path}.scheme must be one of {', '.join(TOKENIZATION_SCHEMES)}"
477
+ )
478
+ return cls(
479
+ lookahead=lookahead, allow_cross_component_merge=allow_cross, scheme=scheme
480
+ )
481
+
482
+ def to_dict(self) -> dict[str, object]:
483
+ payload: dict[str, object] = {
484
+ "lookahead": self.lookahead,
485
+ "allow_cross_component_merge": self.allow_cross_component_merge,
486
+ }
487
+
488
+
489
+ if self.scheme != "set_bpe":
490
+ payload["scheme"] = self.scheme
491
+ return payload
492
+
493
+
494
+ @dataclass(frozen=True)
495
+ class PrimitiveTrainingConfig:
496
+ "Serialized config-v1 metadata retained until artifacts are versioned again."
497
+
498
+ zero_primitive_components: tuple[str, ...]
499
+
500
+ @classmethod
501
+ def from_dict(
502
+ cls,
503
+ payload: object,
504
+ *,
505
+ physical: PhysicalConfig,
506
+ path: str = "train.primitives",
507
+ ) -> PrimitiveTrainingConfig:
508
+ values = _mapping(payload, path)
509
+ _reject_unknown(values, {"zero_primitive_components"}, path)
510
+ raw_names = _required(values, "zero_primitive_components", path)
511
+ if not isinstance(raw_names, Sequence) or isinstance(
512
+ raw_names, (str, bytes, bytearray)
513
+ ):
514
+ raise ConfigError(f"{path}.zero_primitive_components must be a list")
515
+ names = tuple(raw_names)
516
+ if not all(isinstance(name, str) and name for name in names):
517
+ raise ConfigError(
518
+ f"{path}.zero_primitive_components must contain non-empty component names"
519
+ )
520
+ if len(set(names)) != len(names):
521
+ raise ConfigError(f"{path}.zero_primitive_components contains a duplicate")
522
+ unknown = sorted(set(names) - set(physical.components))
523
+ if unknown:
524
+ raise ConfigError(
525
+ f"{path}.zero_primitive_components contains unknown component {unknown[0]!r}"
526
+ )
527
+
528
+ selected = set(names)
529
+ for name, component in physical.components.items():
530
+ if component.space == "additive":
531
+ if component.primitive_order not in {0, 1, 2}:
532
+ raise ConfigError(
533
+ f"{path} cannot train additive primitive_order="
534
+ f"{component.primitive_order} for component {name!r}"
535
+ )
536
+ domain_order = component.primitive_order
537
+ if name in selected and component.primitive_order == 0:
538
+ raise ConfigError(
539
+ f"{path}.zero_primitive_components cannot contain absolute "
540
+ f"component {name!r}"
541
+ )
542
+ else:
543
+ domain_order = component.primitive_order
544
+ if f"order_{domain_order}" not in component.constraints:
545
+ raise ConfigError(
546
+ f"physical component {name!r} needs constraints.order_{domain_order} "
547
+ "to train its primitives"
548
+ )
549
+ return cls(zero_primitive_components=names)
550
+
551
+ def to_dict(self) -> dict[str, object]:
552
+ return {"zero_primitive_components": list(self.zero_primitive_components)}
553
+
554
+
555
+ @dataclass(frozen=True)
556
+ class BPETrainingConfig:
557
+ "Spatial and final vocabulary budgets used for whole-trajectory BPE."
558
+
559
+ spatial_vocab_size: int
560
+ vocab_size: int
561
+
562
+ @classmethod
563
+ def from_dict(
564
+ cls, payload: object, *, path: str = "train.bpe"
565
+ ) -> BPETrainingConfig:
566
+ values = _mapping(payload, path)
567
+ _reject_unknown(values, {"spatial_vocab_size", "vocab_size"}, path)
568
+ spatial_vocab_size = _integer(
569
+ _required(values, "spatial_vocab_size", path),
570
+ f"{path}.spatial_vocab_size",
571
+ positive=True,
572
+ )
573
+ vocab_size = _integer(
574
+ _required(values, "vocab_size", path), f"{path}.vocab_size", positive=True
575
+ )
576
+ if spatial_vocab_size > vocab_size:
577
+ raise ConfigError(f"{path}.spatial_vocab_size must not exceed vocab_size")
578
+ return cls(spatial_vocab_size=spatial_vocab_size, vocab_size=vocab_size)
579
+
580
+ def to_dict(self) -> dict[str, object]:
581
+ return {
582
+ "spatial_vocab_size": self.spatial_vocab_size,
583
+ "vocab_size": self.vocab_size,
584
+ }
585
+
586
+
587
+ @dataclass(frozen=True)
588
+ class QualityControlConfig:
589
+ "Pretraining-only policy for extreme first-order discontinuities."
590
+
591
+ hard_order_1_scale: float
592
+ min_segment_length: int
593
+
594
+ @classmethod
595
+ def from_dict(
596
+ cls, payload: object, *, path: str = "train.quality_control"
597
+ ) -> QualityControlConfig:
598
+ values = _mapping(payload, path)
599
+ _reject_unknown(values, {"hard_order_1_scale", "min_segment_length"}, path)
600
+ scale = _number(
601
+ _required(values, "hard_order_1_scale", path),
602
+ f"{path}.hard_order_1_scale",
603
+ )
604
+ if scale <= 1.0:
605
+ raise ConfigError(f"{path}.hard_order_1_scale must be greater than 1")
606
+ return cls(
607
+ hard_order_1_scale=scale,
608
+ min_segment_length=_integer(
609
+ _required(values, "min_segment_length", path),
610
+ f"{path}.min_segment_length",
611
+ positive=True,
612
+ ),
613
+ )
614
+
615
+ def to_dict(self) -> dict[str, object]:
616
+ return {
617
+ "hard_order_1_scale": self.hard_order_1_scale,
618
+ "min_segment_length": self.min_segment_length,
619
+ }
620
+
621
+
622
+ @dataclass(frozen=True)
623
+ class TrainConfig:
624
+ "Non-derivable fit choices and BPE budgets."
625
+
626
+ primitives: PrimitiveTrainingConfig
627
+ bpe: BPETrainingConfig
628
+ quality_control: QualityControlConfig | None = None
629
+
630
+ @classmethod
631
+ def from_dict(
632
+ cls,
633
+ payload: object,
634
+ *,
635
+ physical: PhysicalConfig,
636
+ path: str = "train",
637
+ ) -> TrainConfig:
638
+ values = _mapping(payload, path)
639
+ _reject_unknown(values, {"primitives", "bpe", "quality_control"}, path)
640
+ raw_quality_control = values.get("quality_control")
641
+ return cls(
642
+ primitives=PrimitiveTrainingConfig.from_dict(
643
+ _required(values, "primitives", path), physical=physical
644
+ ),
645
+ bpe=BPETrainingConfig.from_dict(_required(values, "bpe", path)),
646
+ quality_control=(
647
+ None
648
+ if raw_quality_control is None
649
+ else QualityControlConfig.from_dict(raw_quality_control)
650
+ ),
651
+ )
652
+
653
+ def to_dict(self) -> dict[str, object]:
654
+ payload = {"primitives": self.primitives.to_dict(), "bpe": self.bpe.to_dict()}
655
+ if self.quality_control is not None:
656
+ payload["quality_control"] = self.quality_control.to_dict()
657
+ return payload
658
+
659
+
660
+ @dataclass(frozen=True)
661
+ class ActionCodec2Config:
662
+ "Strict YAML configuration for an ActionCodec2 processor artifact."
663
+
664
+ physical: PhysicalConfig
665
+ set_bpe: SetBPEConfig
666
+ train: TrainConfig | None = None
667
+ model_type: str = MODEL_TYPE
668
+ config_version: int = CONFIG_VERSION
669
+
670
+ @classmethod
671
+ def from_dict(cls, payload: object) -> ActionCodec2Config:
672
+ values = _mapping(payload, "config")
673
+ _reject_unknown(
674
+ values,
675
+ {"model_type", "config_version", "physical", "set_bpe", "train"},
676
+ "config",
677
+ )
678
+ model_type = _required(values, "model_type", "config")
679
+ if model_type != MODEL_TYPE:
680
+ raise ConfigError(f"config.model_type must be {MODEL_TYPE!r}")
681
+ config_version = _integer(
682
+ _required(values, "config_version", "config"), "config.config_version"
683
+ )
684
+ if config_version != CONFIG_VERSION:
685
+ raise ConfigError(
686
+ f"unsupported config_version {config_version}; expected {CONFIG_VERSION}"
687
+ )
688
+ physical = PhysicalConfig.from_dict(_required(values, "physical", "config"))
689
+ raw_train = values.get("train")
690
+ return cls(
691
+ physical=physical,
692
+ set_bpe=SetBPEConfig.from_dict(_required(values, "set_bpe", "config")),
693
+ train=(
694
+ None
695
+ if raw_train is None
696
+ else TrainConfig.from_dict(raw_train, physical=physical)
697
+ ),
698
+ model_type=str(model_type),
699
+ config_version=config_version,
700
+ )
701
+
702
+ @classmethod
703
+ def from_yaml(cls, path: str | Path) -> ActionCodec2Config:
704
+ source = Path(path)
705
+ try:
706
+ payload = yaml.load(
707
+ source.read_text(encoding="utf-8"), Loader=_UniqueKeyLoader
708
+ )
709
+ except OSError:
710
+ raise
711
+ except yaml.YAMLError as error:
712
+ raise ConfigError(f"cannot parse {source}: {error}") from error
713
+ return cls.from_dict(payload)
714
+
715
+ @classmethod
716
+ def from_pretrained(cls, directory: str | Path) -> ActionCodec2Config:
717
+ """Load the strict numerical configuration from ``config.yaml``."""
718
+ return cls.from_yaml(Path(directory) / CONFIG_NAME)
719
+
720
+ def to_dict(self) -> dict[str, object]:
721
+ payload: dict[str, object] = {
722
+ "model_type": self.model_type,
723
+ "config_version": self.config_version,
724
+ "physical": self.physical.to_dict(),
725
+ "set_bpe": self.set_bpe.to_dict(),
726
+ }
727
+ if self.train is not None:
728
+ payload["train"] = self.train.to_dict()
729
+ return payload
730
+
731
+ def save_yaml(self, path: str | Path) -> None:
732
+ destination = Path(path)
733
+ destination.parent.mkdir(parents=True, exist_ok=True)
734
+ destination.write_text(
735
+ yaml.safe_dump(
736
+ self.to_dict(),
737
+ sort_keys=False,
738
+ allow_unicode=True,
739
+ default_flow_style=False,
740
+ ),
741
+ encoding="utf-8",
742
+ )
743
+
744
+ def save_pretrained(self, directory: str | Path) -> None:
745
+ """Write ``config.yaml`` without embedding primitive or BPE arrays."""
746
+ self.save_yaml(Path(directory) / CONFIG_NAME)
runtime/physical/physical_codec.py ADDED
@@ -0,0 +1,909 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ "Configuration-driven physical quantization."
2
+
3
+ from __future__ import annotations
4
+
5
+ import itertools
6
+ import os
7
+ from typing import Protocol
8
+
9
+ import numpy as np
10
+
11
+ from ._so3 import conjugate_quaternion as _conjugate
12
+ from ._so3 import euler_xyz_to_quaternion as _euler_xyz_to_quaternion
13
+ from ._so3 import multiply_quaternions as _multiply
14
+ from ._so3 import quaternion_to_euler_xyz as _quaternion_to_euler_xyz
15
+ from ._so3 import quaternion_to_rotvec as _quaternion_to_rotvec
16
+ from ._so3 import rotvec_to_quaternion as _rotvec_to_quaternion
17
+ from ..common._validation import broadcast_vector
18
+ from .configuration import ActionCodec2Config, ComponentConfig, ConfigError
19
+ from ..backend.native_backend import require_native, resolve_backend
20
+ from .primitives import PrimitiveStore
21
+
22
+ _FLOAT32_SLACK = np.float32(8 * np.finfo(np.float32).eps)
23
+ _IDENTITY = np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64)
24
+
25
+
26
+ def _constraint(
27
+ component: ComponentConfig, order: int
28
+ ) -> tuple[np.ndarray, np.ndarray] | None:
29
+ value = component.constraints.get(f"order_{order}")
30
+ if value is None:
31
+ return None
32
+ return (
33
+ np.asarray(value.minimum, dtype=np.float32),
34
+ np.asarray(value.maximum, dtype=np.float32),
35
+ )
36
+
37
+
38
+ def _scale(component: ComponentConfig) -> np.ndarray:
39
+ return broadcast_vector(
40
+ component.input_transform.scale,
41
+ len(component.dims),
42
+ dtype=np.float32,
43
+ name="input_transform.scale",
44
+ )
45
+
46
+
47
+ def _transform(component: ComponentConfig, actions: np.ndarray) -> np.ndarray:
48
+
49
+ values = np.asarray(actions, dtype=np.float32) * _scale(component)
50
+ if component.input_transform.clip:
51
+ bounds = _constraint(component, component.input_order)
52
+ if bounds is None:
53
+ raise ConfigError(
54
+ f"clipping requires constraints.order_{component.input_order}"
55
+ )
56
+ values = np.clip(values, bounds[0], bounds[1])
57
+ return np.asarray(values, dtype=np.float32)
58
+
59
+
60
+ def _integrate_additive(values: np.ndarray, order: int) -> np.ndarray:
61
+
62
+ result = np.asarray(values, dtype=np.float32)
63
+ for _ in range(order):
64
+ result = np.cumsum(result, axis=-2, dtype=np.float32)
65
+ return result
66
+
67
+
68
+ def _difference_additive(values: np.ndarray, order: int) -> np.ndarray:
69
+
70
+ result = np.asarray(values, dtype=np.float32)
71
+ for _ in range(order):
72
+ previous = np.concatenate(
73
+ (np.zeros_like(result[..., :1, :]), result[..., :-1, :]), axis=-2
74
+ )
75
+ result = result - previous
76
+ return result
77
+
78
+
79
+ class _ComponentRuntime(Protocol):
80
+
81
+
82
+ def encode_batch(
83
+ self, actions: np.ndarray, previous_first_order: np.ndarray | None = None
84
+ ) -> np.ndarray: ...
85
+
86
+ def decode_batch(
87
+ self, cells: np.ndarray, previous_first_order: np.ndarray | None = None
88
+ ) -> np.ndarray: ...
89
+
90
+
91
+ class _AdditiveRuntime:
92
+
93
+
94
+ def __init__(
95
+ self, component: ComponentConfig, primitives: tuple[np.ndarray, ...]
96
+ ) -> None:
97
+ self.config = component
98
+ self.primitives = tuple(
99
+ np.asarray(value, dtype=np.float32) for value in primitives
100
+ )
101
+ self.width = len(component.dims)
102
+ self.order = component.primitive_order
103
+ self._second_order_axes = (
104
+ tuple(axis.astype(np.float64) for axis in self.primitives)
105
+ if self.order == 2
106
+ else ()
107
+ )
108
+ self._second_order_sorted = tuple(
109
+ bool(np.all(axis[1:] > axis[:-1])) for axis in self._second_order_axes
110
+ )
111
+ self._second_order_zero_bounds = tuple(
112
+ (
113
+ int(np.searchsorted(axis, 0)),
114
+ int(np.searchsorted(axis, 0, side="right")) - 1,
115
+ )
116
+ if ordered
117
+ else (0, 0)
118
+ for axis, ordered in zip(
119
+ self._second_order_axes, self._second_order_sorted, strict=True
120
+ )
121
+ )
122
+ raw_position_bounds = _constraint(component, 0)
123
+ self.position_bounds = (
124
+ None
125
+ if raw_position_bounds is None
126
+ else (
127
+ np.broadcast_to(raw_position_bounds[0], (self.width,)).copy(),
128
+ np.broadcast_to(raw_position_bounds[1], (self.width,)).copy(),
129
+ )
130
+ )
131
+ self.limit = broadcast_vector(
132
+ component.distortion_limit,
133
+ self.width,
134
+ dtype=np.float32,
135
+ name="distortion_limit",
136
+ )
137
+ table_width = max(len(values) for values in self.primitives)
138
+ table = np.full((self.width, table_width), np.inf, dtype=np.float32)
139
+ for coordinate, values in enumerate(self.primitives):
140
+ table[coordinate, : len(values)] = values
141
+ self._native_table = np.ascontiguousarray(table)
142
+ self._native_bins = np.asarray(
143
+ [len(values) for values in self.primitives], dtype=np.int32
144
+ )
145
+ self._native_binary = np.zeros(self.width, dtype=np.uint8)
146
+
147
+ def _pick_batch(self, request: np.ndarray, direction: np.ndarray) -> np.ndarray:
148
+
149
+ flat_request = np.asarray(request, dtype=np.float32).reshape(-1, self.width)
150
+ flat_direction = np.asarray(direction, dtype=np.float32).reshape(-1, self.width)
151
+ choice = np.empty(flat_request.shape, dtype=np.int64)
152
+ for coordinate, primitives in enumerate(self.primitives):
153
+ distance = np.abs(flat_request[:, coordinate, None] - primitives[None, :])
154
+ nearest = np.argmin(distance, axis=1)
155
+ if self.config.primitive_selection == "nearest":
156
+ choice[:, coordinate] = nearest
157
+ continue
158
+ feasible = distance <= self.limit[coordinate] + _FLOAT32_SLACK
159
+ preferred = feasible & (
160
+ np.sign(primitives)[None, :] * flat_direction[:, coordinate, None]
161
+ >= 0.0
162
+ )
163
+ candidates = np.where(
164
+ np.any(preferred, axis=1, keepdims=True), preferred, feasible
165
+ )
166
+ smooth = np.argmin(np.where(candidates, distance, np.inf), axis=1)
167
+ choice[:, coordinate] = np.where(np.any(feasible, axis=1), smooth, nearest)
168
+ return choice.reshape(request.shape)
169
+
170
+ def _advance_batch(self, state: np.ndarray, primitive: np.ndarray) -> None:
171
+
172
+ higher = primitive
173
+ for level in range(self.order - 1, -1, -1):
174
+ state[:, level] += higher
175
+ higher = state[:, level]
176
+ if self.order == 1 and self.position_bounds is not None:
177
+ state[:, 0] = np.clip(
178
+ state[:, 0], self.position_bounds[0], self.position_bounds[1]
179
+ )
180
+
181
+ def encode_batch(
182
+ self, actions: np.ndarray, previous_first_order: np.ndarray | None = None
183
+ ) -> np.ndarray:
184
+ """Quantize ``[B,T,d]`` actions, vectorizing all trajectories per step."""
185
+ if self.order == 2:
186
+ return self._encode_second_order(actions, previous_first_order)
187
+ transformed = _transform(self.config, actions)
188
+ target = _integrate_additive(transformed, self.config.input_order)
189
+ batch, steps, _ = target.shape
190
+ if self.order == 0:
191
+ return self._pick_batch(target, np.zeros_like(target))
192
+ if self.position_bounds is not None and resolve_backend("auto") == "native":
193
+ starts = np.zeros((batch, self.width), dtype=np.float32)
194
+ cells = require_native().physical_encode_batch(
195
+ self._native_table,
196
+ self._native_bins,
197
+ self._native_binary,
198
+ np.ascontiguousarray(self.position_bounds[0], dtype=np.float32),
199
+ np.ascontiguousarray(self.position_bounds[1], dtype=np.float32),
200
+ np.ascontiguousarray(target, dtype=np.float32),
201
+ starts,
202
+ starts,
203
+ self.order,
204
+ min(batch, os.cpu_count() or 1),
205
+ np.ascontiguousarray(self.limit, dtype=np.float32),
206
+ self.config.primitive_selection == "smooth",
207
+ )
208
+ return np.asarray(cells, dtype=np.int64)
209
+
210
+ cells = np.empty(target.shape, dtype=np.int64)
211
+ state = np.zeros((batch, self.order, self.width), dtype=np.float32)
212
+ direction = np.zeros((batch, self.width), dtype=np.float32)
213
+ zero = np.zeros((batch, self.width), dtype=np.float32)
214
+ for step in range(steps):
215
+ baseline = state.copy()
216
+ self._advance_batch(baseline, zero)
217
+ request = target[:, step] - baseline[:, 0]
218
+ choice = self._pick_batch(request, direction)
219
+ primitive = np.stack(
220
+ [
221
+ self.primitives[index][choice[:, index]]
222
+ for index in range(self.width)
223
+ ],
224
+ axis=1,
225
+ )
226
+ self._advance_batch(state, primitive)
227
+ moved = primitive != 0.0
228
+ direction = np.where(moved, np.sign(primitive), direction)
229
+ cells[:, step] = choice
230
+ return cells
231
+
232
+ def decode_batch(
233
+ self, cells: np.ndarray, previous_first_order: np.ndarray | None = None
234
+ ) -> np.ndarray:
235
+ """Decode ``[B,T,d]`` cells to actions in the component's input units."""
236
+ if self.order == 2:
237
+ return self._decode_second_order(cells, previous_first_order)
238
+ indices = np.asarray(cells, dtype=np.int64)
239
+ if self.order == 0:
240
+ positions = np.stack(
241
+ [
242
+ self.primitives[index][indices[:, :, index]]
243
+ for index in range(self.width)
244
+ ],
245
+ axis=2,
246
+ )
247
+ elif self.position_bounds is not None and resolve_backend("auto") == "native":
248
+ batch = len(indices)
249
+ starts = np.zeros((batch, self.width), dtype=np.float32)
250
+ positions = require_native().physical_decode_batch(
251
+ self._native_table,
252
+ self._native_bins,
253
+ self._native_binary,
254
+ np.ascontiguousarray(self.position_bounds[0], dtype=np.float32),
255
+ np.ascontiguousarray(self.position_bounds[1], dtype=np.float32),
256
+ np.ascontiguousarray(indices, dtype=np.int32),
257
+ starts,
258
+ starts,
259
+ self.order,
260
+ min(batch, os.cpu_count() or 1),
261
+ )
262
+ else:
263
+ picked = np.stack(
264
+ [
265
+ self.primitives[index][indices[:, :, index]]
266
+ for index in range(self.width)
267
+ ],
268
+ axis=2,
269
+ )
270
+ batch, steps, _ = indices.shape
271
+ positions = np.empty(indices.shape, dtype=np.float32)
272
+ state = np.zeros((batch, self.order, self.width), dtype=np.float32)
273
+ for step in range(steps):
274
+ self._advance_batch(state, picked[:, step])
275
+ positions[:, step] = state[:, 0]
276
+ actions = _difference_additive(positions, self.config.input_order)
277
+ return np.asarray(actions / _scale(self.config), dtype=np.float32)
278
+
279
+ def _encode_second_order(
280
+ self, actions: np.ndarray, previous_first_order: np.ndarray
281
+ ) -> np.ndarray:
282
+
283
+ transformed = _transform(self.config, actions).astype(np.float64)
284
+ velocity = previous_first_order.astype(np.float64) * _scale(self.config)
285
+ target = transformed
286
+ if self.config.input_order == 2:
287
+ target = np.cumsum(target, axis=1) + velocity[:, None]
288
+ if self.config.input_order >= 1:
289
+ target = np.cumsum(target, axis=1)
290
+ if resolve_backend("auto") == "native" and hasattr(
291
+ require_native(), "additive_second_order_encode_batch"
292
+ ):
293
+ return np.asarray(
294
+ require_native().additive_second_order_encode_batch(
295
+ self._native_table,
296
+ self._native_bins,
297
+ np.ascontiguousarray(target),
298
+ np.ascontiguousarray(velocity),
299
+ self.limit,
300
+ self.config.primitive_selection == "smooth",
301
+ min(
302
+ len(actions),
303
+ max(
304
+ 1,
305
+ int(os.environ.get("ACTIONCODEC2_PHYSICAL_THREADS", "16")),
306
+ ),
307
+ ),
308
+ ),
309
+ dtype=np.int64,
310
+ )
311
+ position = np.zeros_like(velocity)
312
+ direction = np.zeros_like(velocity)
313
+ cells = np.empty(actions.shape, dtype=np.int64)
314
+ for step in range(actions.shape[1]):
315
+ request = target[:, step] - (position + velocity)
316
+
317
+
318
+ choice = self._pick_second_order(request, direction)
319
+ primitive = np.stack(
320
+ [axis[choice[:, j]] for j, axis in enumerate(self.primitives)], axis=1
321
+ )
322
+ velocity += primitive
323
+ position += velocity
324
+ direction = np.where(primitive != 0, np.sign(primitive), direction)
325
+ cells[:, step] = choice
326
+ return cells
327
+
328
+ def _pick_second_order(
329
+ self, request: np.ndarray, direction: np.ndarray
330
+ ) -> np.ndarray:
331
+ choice = np.empty(request.shape, dtype=np.int64)
332
+ for j, axis in enumerate(self._second_order_axes):
333
+
334
+ if self._second_order_sorted[j] and (
335
+ self.config.primitive_selection == "smooth" or len(axis) > 32
336
+ ):
337
+ right = np.searchsorted(axis, request[:, j])
338
+ left = np.clip(right - 1, 0, len(axis) - 1)
339
+ right = np.minimum(right, len(axis) - 1)
340
+ nearest = np.where(
341
+ np.abs(request[:, j] - axis[left])
342
+ <= np.abs(request[:, j] - axis[right]),
343
+ left,
344
+ right,
345
+ )
346
+
347
+
348
+ tied = (nearest > 0) & (
349
+ np.abs(request[:, j] - axis[np.maximum(nearest - 1, 0)])
350
+ == np.abs(request[:, j] - axis[nearest])
351
+ )
352
+ if np.any(tied):
353
+ nearest[tied] = np.argmin(
354
+ np.abs(request[tied, j, None] - axis), axis=1
355
+ )
356
+ if self.config.primitive_selection == "smooth":
357
+ nonnegative, nonpositive = self._second_order_zero_bounds[j]
358
+ preferred = np.where(
359
+ direction[:, j] > 0,
360
+ np.maximum(nearest, min(nonnegative, len(axis) - 1)),
361
+ np.where(
362
+ direction[:, j] < 0,
363
+ np.minimum(nearest, max(nonpositive, 0)),
364
+ nearest,
365
+ ),
366
+ )
367
+ admissible = (np.sign(axis[preferred]) * direction[:, j] >= 0) & (
368
+ np.abs(request[:, j] - axis[preferred]) <= self.limit[j]
369
+ )
370
+ nearest = np.where(admissible, preferred, nearest)
371
+ choice[:, j] = nearest
372
+ continue
373
+
374
+ distance = np.abs(request[:, j, None] - axis)
375
+ nearest = np.argmin(distance, axis=1)
376
+ if self.config.primitive_selection == "nearest":
377
+ choice[:, j] = nearest
378
+ continue
379
+ feasible = distance <= self.limit[j]
380
+ preferred = feasible & (np.sign(axis)[None] * direction[:, j, None] >= 0)
381
+ candidates = np.where(
382
+ preferred.any(axis=1, keepdims=True), preferred, feasible
383
+ )
384
+ smooth = np.argmin(np.where(candidates, distance, np.inf), axis=1)
385
+ choice[:, j] = np.where(feasible.any(axis=1), smooth, nearest)
386
+ return choice
387
+
388
+ def _decode_second_order(
389
+ self, cells: np.ndarray, previous_first_order: np.ndarray
390
+ ) -> np.ndarray:
391
+
392
+ picked = np.stack(
393
+ [axis[cells[:, :, j]] for j, axis in enumerate(self.primitives)], axis=2
394
+ ).astype(np.float64)
395
+ if self.config.input_order == 2:
396
+ result = picked
397
+ else:
398
+ result = np.cumsum(picked, axis=1) + previous_first_order[:, None] * _scale(
399
+ self.config
400
+ )
401
+ if self.config.input_order == 0:
402
+ result = np.cumsum(result, axis=1)
403
+ return np.asarray(result / _scale(self.config), dtype=np.float32)
404
+
405
+
406
+ class _SO3Runtime:
407
+
408
+
409
+ def __init__(
410
+ self, component: ComponentConfig, primitives: tuple[np.ndarray, ...]
411
+ ) -> None:
412
+ self.config = component
413
+ self.primitives = tuple(
414
+ np.asarray(value, dtype=np.float64) for value in primitives
415
+ )
416
+ self.left = component.multiplication == "left"
417
+ self.limit = float(component.distortion_limit)
418
+ if component.primitive_order == 2:
419
+ bounds = component.constraints["order_2"]
420
+ corner = np.maximum(np.abs(bounds.minimum), np.abs(bounds.maximum))
421
+ corner = np.broadcast_to(corner, (3,))
422
+ radius = (
423
+ np.linalg.norm(corner)
424
+ if component.representation == "rotation_vector"
425
+ else np.sum(corner)
426
+ )
427
+ self.second_order_radius = min(np.pi, float(radius) + 3 * self.limit)
428
+ table_width = max(len(values) for values in self.primitives)
429
+ table = np.full((3, table_width), np.inf, dtype=np.float64)
430
+ for coordinate, values in enumerate(self.primitives):
431
+ table[coordinate, : len(values)] = values
432
+ self._native_table = table
433
+ self._native_bins = np.asarray(
434
+ [len(values) for values in self.primitives], dtype=np.int32
435
+ )
436
+
437
+ def _from_representation(self, values: np.ndarray) -> np.ndarray:
438
+ shape = values.shape[:-1]
439
+ flat = np.asarray(values).reshape(-1, 3)
440
+ converted = (
441
+ _rotvec_to_quaternion(flat)
442
+ if self.config.representation == "rotation_vector"
443
+ else _euler_xyz_to_quaternion(flat)
444
+ )
445
+ return converted.reshape(*shape, 4)
446
+
447
+ def _to_representation(self, values: np.ndarray) -> np.ndarray:
448
+ shape = values.shape[:-1]
449
+ flat = np.asarray(values).reshape(-1, 4)
450
+ converted = (
451
+ _quaternion_to_rotvec(flat)
452
+ if self.config.representation == "rotation_vector"
453
+ else _quaternion_to_euler_xyz(flat)
454
+ )
455
+ return converted.reshape(*shape, 3)
456
+
457
+ def _residual(self, target: np.ndarray, reconstruction: np.ndarray) -> np.ndarray:
458
+ return (
459
+ _multiply(target, _conjugate(reconstruction))
460
+ if self.left
461
+ else _multiply(_conjugate(reconstruction), target)
462
+ )
463
+
464
+ def _apply(self, state: np.ndarray, increment: np.ndarray) -> np.ndarray:
465
+ return _multiply(increment, state) if self.left else _multiply(state, increment)
466
+
467
+ def _nearest_batch(self, request: np.ndarray) -> np.ndarray:
468
+ return np.stack(
469
+ [
470
+ np.argmin(np.abs(request[:, index, None] - values[None, :]), axis=1)
471
+ for index, values in enumerate(self.primitives)
472
+ ],
473
+ axis=1,
474
+ ).astype(np.int64, copy=False)
475
+
476
+ def _smooth(
477
+ self,
478
+ request: np.ndarray,
479
+ reconstruction: np.ndarray,
480
+ target: np.ndarray,
481
+ direction: np.ndarray,
482
+ nearest: np.ndarray,
483
+ ) -> np.ndarray:
484
+
485
+ axes = []
486
+ for coordinate, values in enumerate(self.primitives):
487
+ candidates = np.flatnonzero(
488
+ np.abs(values - request[coordinate]) <= 2.0 * self.limit + 1e-12
489
+ )
490
+ if nearest[coordinate] not in candidates:
491
+ candidates = np.append(candidates, nearest[coordinate])
492
+ axes.append(candidates)
493
+
494
+ best_any: tuple[float, tuple[int, int, int]] | None = None
495
+ best_preferred: tuple[float, tuple[int, int, int]] | None = None
496
+ for raw_choice in itertools.product(*axes):
497
+ choice = tuple(int(value) for value in raw_choice)
498
+ rotvec = np.asarray(
499
+ [self.primitives[index][choice[index]] for index in range(3)]
500
+ )
501
+ rebuilt = self._apply(reconstruction, _rotvec_to_quaternion(rotvec))
502
+ error = float(
503
+ np.linalg.norm(_quaternion_to_rotvec(self._residual(target, rebuilt)))
504
+ )
505
+ if error > self.limit + 1e-12:
506
+ continue
507
+ candidate = (error, choice)
508
+ if best_any is None or candidate < best_any:
509
+ best_any = candidate
510
+
511
+
512
+ if np.dot(rotvec, direction) >= -1e-15 and (
513
+ best_preferred is None or candidate < best_preferred
514
+ ):
515
+ best_preferred = candidate
516
+ selected = best_preferred or best_any
517
+ return nearest if selected is None else np.asarray(selected[1], dtype=np.int64)
518
+
519
+ def encode_batch(
520
+ self, actions: np.ndarray, previous_first_order: np.ndarray | None = None
521
+ ) -> np.ndarray:
522
+ """Quantize ``[B,T,3]`` rotations with a vectorized batch state."""
523
+ if self.config.primitive_order == 2:
524
+ return self._encode_second_order(actions, previous_first_order)
525
+ transformed = _transform(self.config, actions).astype(np.float64)
526
+ if resolve_backend("auto") == "native":
527
+ cells = require_native().so3_encode_batch(
528
+ self._native_table,
529
+ self._native_bins,
530
+ np.ascontiguousarray(transformed, dtype=np.float32),
531
+ self.config.input_order,
532
+ self.config.representation == "rotation_vector",
533
+ self.left,
534
+ self.limit,
535
+ self.config.primitive_selection == "smooth",
536
+ min(len(transformed), os.cpu_count() or 1),
537
+ )
538
+ return np.asarray(cells, dtype=np.int64)
539
+ converted = self._from_representation(transformed)
540
+ batch, steps = transformed.shape[:2]
541
+ true = np.broadcast_to(_IDENTITY, (batch, 4)).copy()
542
+ reconstruction = true.copy()
543
+ direction = np.zeros((batch, 3), dtype=np.float64)
544
+ cells = np.empty((batch, steps, 3), dtype=np.int64)
545
+ for step in range(steps):
546
+ value = converted[:, step]
547
+ true = value if self.config.input_order == 0 else self._apply(true, value)
548
+ residual = _quaternion_to_rotvec(self._residual(true, reconstruction))
549
+ choice = self._nearest_batch(residual)
550
+ if self.config.primitive_selection == "smooth":
551
+ choice = np.stack(
552
+ [
553
+ self._smooth(
554
+ residual[index],
555
+ reconstruction[index],
556
+ true[index],
557
+ direction[index],
558
+ choice[index],
559
+ )
560
+ for index in range(batch)
561
+ ]
562
+ )
563
+ primitive = np.stack(
564
+ [self.primitives[index][choice[:, index]] for index in range(3)], axis=1
565
+ )
566
+ reconstruction = self._apply(
567
+ reconstruction, _rotvec_to_quaternion(primitive)
568
+ )
569
+ moved = np.linalg.norm(primitive, axis=1) > 0.0
570
+ direction[moved] = primitive[moved]
571
+ cells[:, step] = choice
572
+ return cells
573
+
574
+ def decode_batch(
575
+ self, cells: np.ndarray, previous_first_order: np.ndarray | None = None
576
+ ) -> np.ndarray:
577
+ """Decode ``[B,T,3]`` cells in the configured boundary representation."""
578
+ if self.config.primitive_order == 2:
579
+ return self._decode_second_order(cells, previous_first_order)
580
+ indices = np.asarray(cells, dtype=np.int64)
581
+ rotvec = np.stack(
582
+ [self.primitives[index][indices[:, :, index]] for index in range(3)], axis=2
583
+ )
584
+ if self.config.input_order == 1:
585
+ represented = (
586
+ rotvec
587
+ if self.config.representation == "rotation_vector"
588
+ else self._to_representation(
589
+ _rotvec_to_quaternion(rotvec.astype(np.float32))
590
+ )
591
+ )
592
+ actions = np.asarray(represented / _scale(self.config), dtype=np.float32)
593
+
594
+
595
+ return _difference_additive(np.cumsum(actions, axis=1, dtype=np.float32), 1)
596
+
597
+ batch, steps = indices.shape[:2]
598
+ reconstruction = np.broadcast_to(_IDENTITY, (batch, 4)).copy()
599
+ quaternion = np.empty((batch, steps, 4), dtype=np.float64)
600
+ primitive_quaternion = _rotvec_to_quaternion(rotvec)
601
+ for step in range(steps):
602
+ reconstruction = self._apply(reconstruction, primitive_quaternion[:, step])
603
+ quaternion[:, step] = reconstruction
604
+ represented = self._to_representation(quaternion)
605
+ return np.asarray(represented / _scale(self.config), dtype=np.float32)
606
+
607
+ def _encode_second_order(
608
+ self, actions: np.ndarray, previous_first_order: np.ndarray
609
+ ) -> np.ndarray:
610
+
611
+ converted = self._from_representation(
612
+ _transform(self.config, actions).astype(np.float64)
613
+ )
614
+ increment = self._from_representation(
615
+ previous_first_order.astype(np.float64) * _scale(self.config)
616
+ )
617
+ if resolve_backend("auto") == "native" and hasattr(
618
+ require_native(), "so3_second_order_encode_batch"
619
+ ):
620
+ return np.asarray(
621
+ require_native().so3_second_order_encode_batch(
622
+ self._native_table,
623
+ self._native_bins,
624
+ np.ascontiguousarray(converted),
625
+ np.ascontiguousarray(increment),
626
+ self.config.input_order,
627
+ self.left,
628
+ self.limit,
629
+ self.second_order_radius,
630
+ self.config.primitive_selection == "smooth",
631
+ min(
632
+ len(actions),
633
+ max(
634
+ 1,
635
+ int(os.environ.get("ACTIONCODEC2_PHYSICAL_THREADS", "16")),
636
+ ),
637
+ ),
638
+ ),
639
+ dtype=np.int64,
640
+ )
641
+ batch, steps = actions.shape[:2]
642
+ true = np.broadcast_to(_IDENTITY, (batch, 4)).copy()
643
+ reconstruction = true.copy()
644
+ direction = np.zeros((batch, 3), dtype=np.float64)
645
+ cells = np.empty((batch, steps, 3), dtype=np.int64)
646
+ for step in range(steps):
647
+ true = (
648
+ converted[:, step]
649
+ if self.config.input_order == 0
650
+ else self._apply(true, converted[:, step])
651
+ )
652
+ desired = self._residual(true, reconstruction)
653
+ correction = self._residual(desired, increment)
654
+ request = _quaternion_to_rotvec(correction)
655
+ choice = self._nearest_batch(request)
656
+ if self.config.primitive_selection == "smooth":
657
+ choice = self._smooth_second_order_batch(
658
+ request, correction, direction, choice
659
+ )
660
+ primitive = np.stack(
661
+ [axis[choice[:, j]] for j, axis in enumerate(self.primitives)], axis=1
662
+ )
663
+ primitive = self._second_order_primitive(primitive)
664
+ increment = self._apply(increment, _rotvec_to_quaternion(primitive))
665
+ reconstruction = self._apply(reconstruction, increment)
666
+ moved = np.linalg.norm(primitive, axis=1) > 0
667
+ direction[moved] = primitive[moved]
668
+ cells[:, step] = choice
669
+ return cells
670
+
671
+ def _second_order_primitive(self, rotvec: np.ndarray) -> np.ndarray:
672
+
673
+ norm = np.linalg.norm(rotvec, axis=-1, keepdims=True)
674
+ return rotvec * np.minimum(
675
+ 1.0, self.second_order_radius / np.maximum(norm, np.finfo(float).tiny)
676
+ )
677
+
678
+ def _smooth_second_order_batch(
679
+ self,
680
+ request: np.ndarray,
681
+ correction: np.ndarray,
682
+ direction: np.ndarray,
683
+ nearest: np.ndarray,
684
+ ) -> np.ndarray:
685
+
686
+ if len(request) > 64:
687
+ return np.concatenate(
688
+ [
689
+ self._smooth_second_order_batch(
690
+ request[i : i + 64],
691
+ correction[i : i + 64],
692
+ direction[i : i + 64],
693
+ nearest[i : i + 64],
694
+ )
695
+ for i in range(0, len(request), 64)
696
+ ]
697
+ )
698
+ axes, counts = [], []
699
+ for j, axis in enumerate(self.primitives):
700
+ mask = np.abs(axis[None] - request[:, j, None]) <= 2 * self.limit + 1e-12
701
+ mask[np.arange(len(request)), nearest[:, j]] = True
702
+ count = mask.sum(axis=1)
703
+ rows, columns = np.nonzero(mask)
704
+ slots = np.arange(len(rows)) - np.repeat(np.cumsum(count) - count, count)
705
+ candidates = np.zeros((len(request), int(count.max())), dtype=np.int64)
706
+ candidates[rows, slots] = columns
707
+ axes.append(candidates)
708
+ counts.append(count)
709
+ counts = np.stack(counts, axis=1)
710
+ widths = counts.max(axis=0)
711
+ total = int(np.prod(widths))
712
+ best = nearest.copy()
713
+ best_error = np.full(len(request), np.inf)
714
+ best_preferred = np.zeros(len(request), dtype=bool)
715
+ for offset in range(0, total, 4096):
716
+ lattice = np.stack(
717
+ np.unravel_index(
718
+ np.arange(offset, min(total, offset + 4096)), tuple(widths)
719
+ ),
720
+ axis=1,
721
+ )
722
+ valid = np.all(lattice[None] < counts[:, None], axis=2)
723
+ choices = np.stack(
724
+ [
725
+ axis[
726
+ np.arange(len(request))[:, None],
727
+ np.minimum(lattice[None, :, j], counts[:, j, None] - 1),
728
+ ]
729
+ for j, axis in enumerate(axes)
730
+ ],
731
+ axis=2,
732
+ )
733
+ rotvec = np.stack(
734
+ [axis[choices[:, :, j]] for j, axis in enumerate(self.primitives)],
735
+ axis=2,
736
+ )
737
+ rotvec = self._second_order_primitive(rotvec)
738
+ primitive = _rotvec_to_quaternion(rotvec)
739
+ residual = _multiply(_conjugate(primitive), correction[:, None])
740
+ error = np.linalg.norm(_quaternion_to_rotvec(residual), axis=2)
741
+ valid &= error <= self.limit + 1e-12
742
+ preferred = valid & (np.sum(rotvec * direction[:, None], axis=2) >= -1e-15)
743
+ has_preferred = preferred.any(axis=1)
744
+ eligible = np.where(has_preferred[:, None], preferred, valid)
745
+ scores = np.where(eligible, error, np.inf)
746
+ index = np.argmin(scores, axis=1)
747
+ score = scores[np.arange(len(request)), index]
748
+ improve = np.isfinite(score) & (
749
+ (has_preferred & ~best_preferred)
750
+ | ((has_preferred == best_preferred) & (score < best_error))
751
+ )
752
+ best[improve] = choices[np.arange(len(request)), index][improve]
753
+ best_error[improve] = score[improve]
754
+ best_preferred[improve] = has_preferred[improve]
755
+ return best
756
+
757
+ def _decode_second_order(
758
+ self, cells: np.ndarray, previous_first_order: np.ndarray
759
+ ) -> np.ndarray:
760
+
761
+ rotvec = np.stack(
762
+ [axis[cells[:, :, j]] for j, axis in enumerate(self.primitives)], axis=2
763
+ )
764
+ rotvec = self._second_order_primitive(rotvec)
765
+ changes = _rotvec_to_quaternion(rotvec)
766
+ increment = self._from_representation(
767
+ previous_first_order.astype(np.float64) * _scale(self.config)
768
+ )
769
+ reconstruction = np.broadcast_to(_IDENTITY, increment.shape).copy()
770
+ output = np.empty((*cells.shape[:2], 4), dtype=np.float64)
771
+ for step in range(cells.shape[1]):
772
+ increment = self._apply(increment, changes[:, step])
773
+ reconstruction = self._apply(reconstruction, increment)
774
+ output[:, step] = (
775
+ increment if self.config.input_order == 1 else reconstruction
776
+ )
777
+ return np.asarray(
778
+ self._to_representation(output) / _scale(self.config), dtype=np.float32
779
+ )
780
+
781
+
782
+ class PhysicalCodec:
783
+ "Compose independent physical components in global action-dimension order."
784
+
785
+ def __init__(self, config: ActionCodec2Config, primitives: PrimitiveStore) -> None:
786
+ if primitives.dimension != config.physical.input_dim:
787
+ raise ConfigError(
788
+ f"primitive dimension {primitives.dimension} does not match input_dim "
789
+ f"{config.physical.input_dim}"
790
+ )
791
+ self.config = config
792
+ self.primitives = primitives
793
+ runtimes: list[tuple[ComponentConfig, _ComponentRuntime]] = []
794
+ for component in config.physical.components.values():
795
+ component_primitives = tuple(
796
+ primitives.values[index] for index in component.dims
797
+ )
798
+ runtime: _ComponentRuntime = (
799
+ _AdditiveRuntime(component, component_primitives)
800
+ if component.space == "additive"
801
+ else _SO3Runtime(component, component_primitives)
802
+ )
803
+ runtimes.append((component, runtime))
804
+ self._runtimes = tuple(runtimes)
805
+ self.bins = primitives.bins
806
+ self.requires_previous_first_order = any(
807
+ c.primitive_order == 2 for c, _ in runtimes
808
+ )
809
+
810
+ def _check_previous_first_order(
811
+ self, value: np.ndarray | None, batch: int
812
+ ) -> np.ndarray | None:
813
+ if value is None:
814
+ if self.requires_previous_first_order:
815
+ raise ValueError(
816
+ "second-order tokenization requires previous_first_order; no zero initialization is inferred"
817
+ )
818
+ return None
819
+ array = np.asarray(value, dtype=np.float32)
820
+ shape = (batch, self.config.physical.input_dim)
821
+ if array.shape != shape or not np.all(np.isfinite(array)):
822
+ raise ValueError(f"previous_first_order must be finite with shape {shape}")
823
+ return array
824
+
825
+ def _check_actions(self, actions: np.ndarray) -> np.ndarray:
826
+ values = np.asarray(actions, dtype=np.float32)
827
+ expected = self.config.physical.input_dim
828
+ if values.ndim != 3 or values.shape[2] != expected or values.shape[1] < 1:
829
+ raise ValueError(f"actions must have non-empty shape [B, T, {expected}]")
830
+ if not np.all(np.isfinite(values)):
831
+ raise ValueError("actions must contain only finite values")
832
+ return values
833
+
834
+ def final_first_order(
835
+ self, cells: np.ndarray, *, previous_first_order: np.ndarray
836
+ ) -> np.ndarray:
837
+ "Return ``[B,D]`` state after these cells, without position differencing."
838
+ values = self._check_cells(cells)
839
+ previous = self._check_previous_first_order(previous_first_order, len(values))
840
+ result = np.zeros(
841
+ (len(values), self.config.physical.input_dim), dtype=np.float32
842
+ )
843
+ for component, runtime in self._runtimes:
844
+ if component.primitive_order != 2:
845
+ continue
846
+ local = np.take(values, component.dims, axis=2)
847
+ boundary = np.take(previous, component.dims, axis=1)
848
+ picked = np.stack(
849
+ [axis[local[:, :, j]] for j, axis in enumerate(runtime.primitives)],
850
+ axis=2,
851
+ )
852
+ if component.space == "additive":
853
+ final = boundary.astype(np.float64) + np.sum(
854
+ picked.astype(np.float64), axis=1
855
+ ) / _scale(component)
856
+ else:
857
+ increment = runtime._from_representation(
858
+ boundary.astype(np.float64) * _scale(component)
859
+ )
860
+ changes = _rotvec_to_quaternion(runtime._second_order_primitive(picked))
861
+ for step in range(local.shape[1]):
862
+ increment = runtime._apply(increment, changes[:, step])
863
+ final = runtime._to_representation(increment) / _scale(component)
864
+ result[:, component.dims] = final
865
+ return result
866
+
867
+ def _check_cells(self, cells: np.ndarray) -> np.ndarray:
868
+ values = np.asarray(cells, dtype=np.int64)
869
+ expected = self.config.physical.input_dim
870
+ if values.ndim != 3 or values.shape[2] != expected or values.shape[1] < 1:
871
+ raise ValueError(f"cells must have non-empty shape [B, T, {expected}]")
872
+ counts = np.asarray(self.bins, dtype=np.int64)
873
+ if values.size and (
874
+ np.any(values < 0) or np.any(values >= counts[None, None, :])
875
+ ):
876
+ raise ValueError("a cell index is outside its dimension's primitive set")
877
+ return values
878
+
879
+ def encode_batch(
880
+ self, actions: np.ndarray, *, previous_first_order: np.ndarray | None = None
881
+ ) -> np.ndarray:
882
+ "Quantize ``[B,T,D]`` with a required ``[B,D]`` boundary for order 2."
883
+ values = self._check_actions(actions)
884
+ previous = self._check_previous_first_order(previous_first_order, len(values))
885
+ cells = np.empty(values.shape, dtype=np.int64)
886
+ for component, runtime in self._runtimes:
887
+ component_cells = runtime.encode_batch(
888
+ np.take(values, component.dims, axis=2),
889
+ None if previous is None else np.take(previous, component.dims, axis=1),
890
+ )
891
+ for local, global_dimension in enumerate(component.dims):
892
+ cells[:, :, global_dimension] = component_cells[:, :, local]
893
+ return cells
894
+
895
+ def decode_batch(
896
+ self, cells: np.ndarray, *, previous_first_order: np.ndarray | None = None
897
+ ) -> np.ndarray:
898
+ """Decode ``[B,T,D]`` using the same ``[B,D]`` boundary as encoding."""
899
+ values = self._check_cells(cells)
900
+ previous = self._check_previous_first_order(previous_first_order, len(values))
901
+ actions = np.empty(values.shape, dtype=np.float32)
902
+ for component, runtime in self._runtimes:
903
+ component_actions = runtime.decode_batch(
904
+ np.take(values, component.dims, axis=2),
905
+ None if previous is None else np.take(previous, component.dims, axis=1),
906
+ )
907
+ for local, global_dimension in enumerate(component.dims):
908
+ actions[:, :, global_dimension] = component_actions[:, :, local]
909
+ return actions
runtime/physical/physical_processor.py ADDED
@@ -0,0 +1,338 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ "Public ActionCodec2 processor and constrained-generation grammars."
2
+
3
+ from __future__ import annotations
4
+
5
+ import hashlib
6
+ import json
7
+ from collections.abc import Sequence
8
+ from numbers import Integral
9
+ from pathlib import Path
10
+ from typing import Any
11
+
12
+ import numpy as np
13
+ from transformers.processing_utils import ProcessorMixin
14
+
15
+ from ..common._validation import active_dimensions
16
+ from ..tokenization.bpe import (
17
+ BPE_NAME,
18
+ BPESerializer,
19
+ SetBPESerializer,
20
+ serializer_class,
21
+ )
22
+ from ..routing.cache import BoundedCache
23
+ from .configuration import CONFIG_NAME, MODEL_TYPE, ActionCodec2Config
24
+ from ..tokenization.grammar import ActionCodec2BPEGrammar as ActionCodec2BPEGrammar
25
+ from ..tokenization.grammar import ActionCodec2Grammar as ActionCodec2Grammar
26
+ from ..integration.hf import (
27
+ resolve_pretrained_directory,
28
+ save_huggingface_metadata,
29
+ upload_pretrained_directory,
30
+ )
31
+ from .physical_codec import PhysicalCodec
32
+ from .primitives import PRIMITIVES_NAME, PrimitiveStore
33
+
34
+
35
+ class ActionCodec2Processor(ProcessorMixin):
36
+ "Hugging Face-style processor for continuous robot action trajectories."
37
+
38
+ model_type = MODEL_TYPE
39
+ name = "actioncodec2"
40
+ is_exact = False
41
+ set_bpe_is_lossless = True
42
+ model_input_names = ["input_ids"]
43
+ attributes = []
44
+
45
+ def __init__(
46
+ self,
47
+ config: ActionCodec2Config,
48
+ primitives: PrimitiveStore,
49
+ serializer: SetBPESerializer | BPESerializer,
50
+ ) -> None:
51
+ if serializer.scheme != config.set_bpe.scheme:
52
+ raise ValueError("serializer scheme does not match config.yaml")
53
+ if tuple(serializer.bins) != tuple(primitives.bins):
54
+ raise ValueError("BPE bins do not match primitives.npz")
55
+ if serializer.lookahead != config.set_bpe.lookahead:
56
+ raise ValueError("BPE lookahead does not match config.yaml")
57
+
58
+
59
+ self.config = config
60
+ self.primitives = primitives
61
+ self.serializer = serializer
62
+ self._grammar_cache = BoundedCache(8)
63
+ self.physical = PhysicalCodec(config, primitives)
64
+ self.action_dim = config.physical.input_dim
65
+ self.bins = primitives.bins
66
+ self.vocab_size = serializer.vocab_size
67
+ self.lookahead = serializer.lookahead
68
+ self.requires_previous_first_order = self.physical.requires_previous_first_order
69
+ zero_cells = []
70
+ for dimension, axis in enumerate(primitives.values):
71
+ matches = np.flatnonzero(np.asarray(axis) == 0)
72
+ if not len(matches):
73
+ raise ValueError(
74
+ "ActionCodec2 artifacts require an exact zero primitive in every axis; "
75
+ f"dimension {dimension} has none"
76
+ )
77
+ zero_cells.append(int(matches[0]))
78
+ self._zero_cells = np.asarray(zero_cells, dtype=np.int64)
79
+
80
+ @classmethod
81
+ def from_pretrained(
82
+ cls,
83
+ pretrained_model_name_or_path: str | Path,
84
+ **kwargs: Any,
85
+ ) -> ActionCodec2Processor:
86
+ "Load a local directory or Hugging Face Hub snapshot."
87
+ directory = resolve_pretrained_directory(
88
+ pretrained_model_name_or_path, **kwargs
89
+ )
90
+ config = ActionCodec2Config.from_pretrained(directory)
91
+ primitives = PrimitiveStore.from_pretrained(
92
+ directory, input_dim=config.physical.input_dim
93
+ )
94
+ serializer = serializer_class(config).from_pretrained(
95
+ directory,
96
+ bins=primitives.bins,
97
+ lookahead=config.set_bpe.lookahead,
98
+ )
99
+ return cls(config, primitives, serializer)
100
+
101
+ def save_pretrained(
102
+ self,
103
+ save_directory: str | Path,
104
+ push_to_hub: bool = False,
105
+ **kwargs: Any,
106
+ ) -> list[str]:
107
+ """Save numerical artifacts plus standard AutoProcessor metadata."""
108
+ if not push_to_hub and kwargs:
109
+ raise TypeError(
110
+ f"unused save_pretrained keyword(s): {', '.join(sorted(kwargs))}"
111
+ )
112
+ directory = Path(save_directory)
113
+ self.config.save_pretrained(directory)
114
+ self.primitives.save_pretrained(directory)
115
+ self.serializer.save_pretrained(directory)
116
+ metadata = save_huggingface_metadata(directory)
117
+ files = [
118
+ directory / CONFIG_NAME,
119
+ directory / PRIMITIVES_NAME,
120
+ directory / BPE_NAME,
121
+ *metadata,
122
+ ]
123
+ if push_to_hub:
124
+ upload_pretrained_directory(directory, **kwargs)
125
+ return [str(path) for path in files]
126
+
127
+ def to_dict(self, legacy_serialization: bool = True) -> dict[str, object]:
128
+ "Return metadata without embedding numerical artifact arrays."
129
+ del legacy_serialization
130
+ return {
131
+ "processor_class": self.__class__.__name__,
132
+ "model_type": self.model_type,
133
+ "action_dim": self.action_dim,
134
+ "vocab_size": self.vocab_size,
135
+ }
136
+
137
+ @property
138
+ def artifact_fingerprint(self) -> str:
139
+ """Content hash over config, primitive axes, and ordered BPE rules."""
140
+ digest = hashlib.sha256()
141
+ digest.update(
142
+ json.dumps(
143
+ self.config.to_dict(), sort_keys=True, separators=(",", ":")
144
+ ).encode()
145
+ )
146
+ for axis in self.primitives.values:
147
+ values = np.ascontiguousarray(axis)
148
+ digest.update(str(values.dtype).encode())
149
+ digest.update(np.asarray(values.shape, dtype=np.int64).tobytes())
150
+ digest.update(values.tobytes())
151
+ digest.update(
152
+ json.dumps(
153
+ self.serializer.to_dict(), sort_keys=True, separators=(",", ":")
154
+ ).encode()
155
+ )
156
+ return digest.hexdigest()
157
+
158
+ def quantize_batch(
159
+ self, actions: np.ndarray, *, previous_first_order: np.ndarray | None = None
160
+ ) -> np.ndarray:
161
+ """Quantize ``[B,T,D]`` using a ``[B,D]`` first-order boundary if required."""
162
+ return self.physical.encode_batch(
163
+ actions, previous_first_order=previous_first_order
164
+ )
165
+
166
+ def _dense_previous_first_order(
167
+ self, previous_first_order: np.ndarray | None, dims: tuple[int, ...], batch: int
168
+ ) -> np.ndarray | None:
169
+
170
+ active = set(dims)
171
+ for component in self.config.physical.components.values():
172
+ present = active.intersection(component.dims)
173
+ if (
174
+ component.primitive_order == 2
175
+ and component.space == "so3"
176
+ and present
177
+ and present != set(component.dims)
178
+ ):
179
+ raise ValueError(
180
+ "second-order SO(3) requires all three active component dimensions"
181
+ )
182
+ if previous_first_order is None:
183
+ if any(
184
+ c.primitive_order == 2 and active.intersection(c.dims)
185
+ for c in self.config.physical.components.values()
186
+ ):
187
+ raise ValueError(
188
+ f"second-order tokenization requires previous_first_order with shape {(batch, len(dims))} in canonical input units"
189
+ )
190
+ return (
191
+ np.zeros((batch, self.action_dim), dtype=np.float32)
192
+ if self.requires_previous_first_order
193
+ else None
194
+ )
195
+ values = np.asarray(previous_first_order, dtype=np.float32)
196
+ if values.shape != (batch, len(dims)) or not np.all(np.isfinite(values)):
197
+ raise ValueError(
198
+ f"previous_first_order must be finite with shape {(batch, len(dims))}"
199
+ )
200
+ dense = np.zeros((batch, self.action_dim), dtype=np.float32)
201
+ dense[:, dims] = values
202
+ return dense
203
+
204
+ def tokens_from_cells(
205
+ self,
206
+ cells: np.ndarray,
207
+ *,
208
+ active_dims: Sequence[int] | None = None,
209
+ ) -> list[int]:
210
+ """Apply the lossless tokenizer to one compact or dense cell trajectory."""
211
+ return self.serializer.encode(cells, active_dims=active_dims)
212
+
213
+ def cells_from_tokens(
214
+ self,
215
+ tokens: Sequence[int],
216
+ *,
217
+ active_dims: Sequence[int] | None = None,
218
+ ) -> np.ndarray:
219
+ """Losslessly recover dense cells with ``-1`` in absent dimensions."""
220
+ return self.serializer.decode(tokens, active_dims=active_dims)
221
+
222
+ def _fill_inactive_cells(self, cells: np.ndarray) -> np.ndarray:
223
+
224
+ values = np.asarray(cells, dtype=np.int64)
225
+ return np.where(values >= 0, values, self._zero_cells)
226
+
227
+ def encode_batch(
228
+ self,
229
+ actions: np.ndarray,
230
+ *,
231
+ active_dims: Sequence[int] | None = None,
232
+ previous_first_order: np.ndarray | None = None,
233
+ ) -> list[list[int]]:
234
+ "Encode a dense or compact ``[B,T,D]`` batch into ragged streams."
235
+ values = np.asarray(actions, dtype=np.float32)
236
+ if (
237
+ values.ndim != 3
238
+ or len(values) < 1
239
+ or values.shape[1] < 1
240
+ or values.shape[2] < 1
241
+ ):
242
+ raise ValueError("actions must have non-empty shape [B, T, D_active]")
243
+ dims = active_dimensions(active_dims, self.action_dim)
244
+ if len(dims) != values.shape[2]:
245
+ raise ValueError("active_dims must have one entry per action coordinate")
246
+ full_width = dims == tuple(range(self.action_dim))
247
+ if full_width:
248
+ dense = values
249
+ else:
250
+ dense = np.zeros(
251
+ (len(values), values.shape[1], self.action_dim), dtype=np.float32
252
+ )
253
+ dense[:, :, dims] = values
254
+ previous = self._dense_previous_first_order(
255
+ previous_first_order, dims, len(values)
256
+ )
257
+ cells = self.quantize_batch(dense, previous_first_order=previous)
258
+ if not full_width:
259
+ cells = np.take(cells, dims, axis=2)
260
+ return [self.tokens_from_cells(row, active_dims=dims) for row in cells]
261
+
262
+ def decode_batch(
263
+ self,
264
+ streams: Sequence[Sequence[int]],
265
+ *,
266
+ active_dims: Sequence[int] | None = None,
267
+ previous_first_order: np.ndarray | None = None,
268
+ ) -> np.ndarray:
269
+ "Decode equal-length streams into one batched physical array."
270
+ dims = active_dimensions(active_dims, self.action_dim)
271
+ cells = [self.cells_from_tokens(stream, active_dims=dims) for stream in streams]
272
+ if not cells:
273
+ raise ValueError("streams must not be empty")
274
+ lengths = {len(value) for value in cells}
275
+ if len(lengths) != 1:
276
+ raise ValueError(
277
+ "all token streams in a batch must decode to the same length"
278
+ )
279
+ stacked_cells = np.stack(cells)
280
+ dense_cells = self._fill_inactive_cells(stacked_cells)
281
+ previous = self._dense_previous_first_order(
282
+ previous_first_order, dims, len(cells)
283
+ )
284
+ decoded = self.physical.decode_batch(dense_cells, previous_first_order=previous)
285
+ return np.take(decoded, dims, axis=2)
286
+
287
+ def final_first_order(
288
+ self,
289
+ streams: Sequence[Sequence[int]],
290
+ *,
291
+ previous_first_order: np.ndarray,
292
+ active_dims: Sequence[int] | None = None,
293
+ executed_steps: int | None = None,
294
+ ) -> np.ndarray:
295
+ "Return the next compact ``[B,D_active]`` reconstructed boundary."
296
+ dims = active_dimensions(active_dims, self.action_dim)
297
+ cells = [self.cells_from_tokens(stream, active_dims=dims) for stream in streams]
298
+ if not cells or len({len(row) for row in cells}) != 1:
299
+ raise ValueError(
300
+ "streams must be non-empty and have equal decoded horizons"
301
+ )
302
+ if executed_steps is not None:
303
+ if (
304
+ isinstance(executed_steps, bool)
305
+ or not isinstance(executed_steps, Integral)
306
+ or not 1 <= executed_steps <= len(cells[0])
307
+ ):
308
+ raise ValueError("executed_steps must be within the decoded horizon")
309
+ cells = [row[:executed_steps] for row in cells]
310
+ previous = self._dense_previous_first_order(
311
+ previous_first_order, dims, len(cells)
312
+ )
313
+ result = self.physical.final_first_order(
314
+ self._fill_inactive_cells(np.stack(cells)), previous_first_order=previous
315
+ )
316
+ return np.take(result, dims, axis=1)
317
+
318
+ def grammar(
319
+ self,
320
+ horizon: int,
321
+ *,
322
+ active_dims: Sequence[int] | None = None,
323
+ ) -> ActionCodec2Grammar | ActionCodec2BPEGrammar:
324
+ """Build a constrained grammar for one horizon and route support."""
325
+ if isinstance(horizon, bool) or not isinstance(horizon, int) or horizon < 1:
326
+ raise ValueError("horizon must be a positive integer")
327
+ dims = active_dimensions(active_dims, self.action_dim)
328
+ key = (horizon, dims)
329
+ grammar = self._grammar_cache.get(key)
330
+ if grammar is None:
331
+ grammar_class = (
332
+ ActionCodec2BPEGrammar
333
+ if isinstance(self.serializer, BPESerializer)
334
+ else ActionCodec2Grammar
335
+ )
336
+ grammar = grammar_class(self.serializer, horizon=horizon, active_dims=dims)
337
+ self._grammar_cache[key] = grammar
338
+ return grammar
runtime/physical/primitives.py ADDED
@@ -0,0 +1,79 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Numerical primitive parameters stored in ``primitives.npz``."""
2
+
3
+ from __future__ import annotations
4
+
5
+ from collections.abc import Sequence
6
+ from pathlib import Path
7
+
8
+ import numpy as np
9
+
10
+ from .configuration import ConfigError
11
+
12
+ PRIMITIVES_NAME = "primitives.npz"
13
+
14
+
15
+ def _key(dimension: int) -> str:
16
+ return f"dimension_{dimension:04d}"
17
+
18
+
19
+ class PrimitiveStore:
20
+ "Immutable numerical primitive axes in global action-dimension order."
21
+
22
+ def __init__(self, values: Sequence[np.ndarray | Sequence[float]]) -> None:
23
+ if not values:
24
+ raise ConfigError("primitives must contain at least one dimension")
25
+ arrays = []
26
+ for dimension, raw in enumerate(values):
27
+ array = np.asarray(raw)
28
+ if array.dtype.hasobject:
29
+ raise ConfigError(
30
+ f"primitive dimension {dimension} must not use object dtype"
31
+ )
32
+ if array.ndim != 1 or not len(array):
33
+ raise ConfigError(
34
+ f"primitive dimension {dimension} must be a non-empty vector"
35
+ )
36
+ real_numeric = np.issubdtype(array.dtype, np.integer) or np.issubdtype(
37
+ array.dtype, np.floating
38
+ )
39
+ if not real_numeric or not np.all(np.isfinite(array)):
40
+ raise ConfigError(
41
+ f"primitive dimension {dimension} must contain finite numbers"
42
+ )
43
+ arrays.append(np.array(array, copy=True))
44
+ self.values = tuple(arrays)
45
+ self.bins = tuple(len(array) for array in arrays)
46
+
47
+ @property
48
+ def dimension(self) -> int:
49
+ return len(self.values)
50
+
51
+ @classmethod
52
+ def from_pretrained(
53
+ cls, directory: str | Path, *, input_dim: int
54
+ ) -> PrimitiveStore:
55
+ """Load exactly one safe numeric vector per expected global dimension."""
56
+ path = Path(directory) / PRIMITIVES_NAME
57
+ with np.load(path, allow_pickle=False) as payload:
58
+ expected = {_key(index) for index in range(input_dim)}
59
+ actual = set(payload.files)
60
+ missing = sorted(expected - actual)
61
+ unknown = sorted(actual - expected)
62
+ if missing:
63
+ raise ConfigError(
64
+ f"{PRIMITIVES_NAME} is missing array(s): {', '.join(missing)}"
65
+ )
66
+ if unknown:
67
+ raise ConfigError(
68
+ f"{PRIMITIVES_NAME} contains unknown array(s): {', '.join(unknown)}"
69
+ )
70
+ values = [payload[_key(index)] for index in range(input_dim)]
71
+ return cls(values)
72
+
73
+ def save_pretrained(self, directory: str | Path) -> None:
74
+ """Write ``primitives.npz`` without pickled objects or path metadata."""
75
+ path = Path(directory) / PRIMITIVES_NAME
76
+ path.parent.mkdir(parents=True, exist_ok=True)
77
+ np.savez(
78
+ path, **{_key(index): value for index, value in enumerate(self.values)}
79
+ )