Instructions to use ZibinDong/ActionCodec2 with libraries, inference providers, notebooks, and local apps. Follow these links to get started.
- Libraries
- Transformers
How to use ZibinDong/ActionCodec2 with Transformers:
# pip install -U transformers accelerate # Load model directly from transformers import AutoModel model = AutoModel.from_pretrained("ZibinDong/ActionCodec2", trust_remote_code=True, device_map="auto") - Notebooks
- Google Colab
- Kaggle
Publish audited ActionCodec2 release files
Browse files- LICENSE +202 -0
- actioncodec2-0.1.0.tar.gz +3 -0
- actioncodec2_kernels/__init__.py +1 -0
- actioncodec2_kernels/grammar.cpp +162 -0
- actioncodec2_kernels/module.cpp +88 -0
- actioncodec2_kernels/parallel.hpp +88 -0
- actioncodec2_kernels/physical.cpp +457 -0
- actioncodec2_kernels/quaternion.hpp +46 -0
- actioncodec2_kernels/reconstruct.cpp +146 -0
- actioncodec2_kernels/resample.cpp +794 -0
- actioncodec2_kernels/setbpe.cpp +423 -0
- actioncodec2_kernels/so3.cpp +311 -0
- config.json +4 -0
- processing_actioncodec2.py +219 -0
- pyproject.toml +16 -0
- requirements.txt +6 -0
- runtime/__init__.py +1 -0
- runtime/backend/__init__.py +1 -0
- runtime/backend/native_backend.py +39 -0
- runtime/common/__init__.py +1 -0
- runtime/common/_frontend_base.py +94 -0
- runtime/common/_types.py +12 -0
- runtime/common/_validation.py +89 -0
- runtime/frontend/__init__.py +1 -0
- runtime/frontend/tokenizer.py +342 -0
- runtime/hub_acceleration.py +333 -0
- runtime/hub_entry.py +219 -0
- runtime/integration/__init__.py +1 -0
- runtime/integration/exporting.py +48 -0
- runtime/integration/hf.py +189 -0
- runtime/physical/__init__.py +1 -0
- runtime/physical/_so3.py +99 -0
- runtime/physical/boundary.py +134 -0
- runtime/physical/configuration.py +746 -0
- runtime/physical/physical_codec.py +909 -0
- runtime/physical/physical_processor.py +338 -0
- runtime/physical/primitives.py +79 -0
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 |
+
)
|