archeone 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- archeone/__init__.py +44 -0
- archeone/_vendor/__init__.py +12 -0
- archeone/_vendor/jaxls/LICENSE +21 -0
- archeone/_vendor/jaxls/__init__.py +103 -0
- archeone/_vendor/jaxls/_analyzed_cost.py +420 -0
- archeone/_vendor/jaxls/_augmented_lagrangian.py +368 -0
- archeone/_vendor/jaxls/_cost.py +374 -0
- archeone/_vendor/jaxls/_covariance.py +179 -0
- archeone/_vendor/jaxls/_lie_group_variables.py +35 -0
- archeone/_vendor/jaxls/_preconditioning.py +131 -0
- archeone/_vendor/jaxls/_problem.py +1053 -0
- archeone/_vendor/jaxls/_py310/__init__.py +0 -0
- archeone/_vendor/jaxls/_py310/_analyzed_cost.py +297 -0
- archeone/_vendor/jaxls/_py310/_augmented_lagrangian.py +268 -0
- archeone/_vendor/jaxls/_py310/_cost.py +185 -0
- archeone/_vendor/jaxls/_py310/_covariance.py +96 -0
- archeone/_vendor/jaxls/_py310/_lie_group_variables.py +56 -0
- archeone/_vendor/jaxls/_py310/_preconditioning.py +114 -0
- archeone/_vendor/jaxls/_py310/_problem.py +722 -0
- archeone/_vendor/jaxls/_py310/_schur.py +914 -0
- archeone/_vendor/jaxls/_py310/_solvers.py +849 -0
- archeone/_vendor/jaxls/_py310/_sparse_matrices.py +198 -0
- archeone/_vendor/jaxls/_py310/_variables.py +292 -0
- archeone/_vendor/jaxls/_py310/_visualization.py +533 -0
- archeone/_vendor/jaxls/_py310/utils.py +113 -0
- archeone/_vendor/jaxls/_schur.py +1210 -0
- archeone/_vendor/jaxls/_solvers.py +1210 -0
- archeone/_vendor/jaxls/_sparse_matrices.py +257 -0
- archeone/_vendor/jaxls/_variables.py +408 -0
- archeone/_vendor/jaxls/_visualization.py +596 -0
- archeone/_vendor/jaxls/utils.py +155 -0
- archeone/_vendor/pyroki/LICENSE +21 -0
- archeone/_vendor/pyroki/__init__.py +4 -0
- archeone/_vendor/pyroki/_residuals/__init__.py +32 -0
- archeone/_vendor/pyroki/_residuals/_pose_residual_analytic_jac.py +258 -0
- archeone/_vendor/pyroki/_residuals/_pose_residual_numerical_jac.py +94 -0
- archeone/_vendor/pyroki/_residuals/_residuals.py +350 -0
- archeone/_vendor/pyroki/_robot.py +178 -0
- archeone/_vendor/pyroki/_robot_urdf_parser.py +462 -0
- archeone/_vendor/pyroki/collision/__init__.py +11 -0
- archeone/_vendor/pyroki/collision/_collision.py +160 -0
- archeone/_vendor/pyroki/collision/_geometry.py +740 -0
- archeone/_vendor/pyroki/collision/_geometry_pairs.py +337 -0
- archeone/_vendor/pyroki/collision/_robot_collision.py +554 -0
- archeone/_vendor/pyroki/collision/_utils.py +113 -0
- archeone/_vendor/pyroki/costs.py +66 -0
- archeone/_vendor/pyroki/utils.py +28 -0
- archeone/_vendor/pyroki/viewer/__init__.py +3 -0
- archeone/_vendor/pyroki/viewer/_batched_urdf.py +175 -0
- archeone/_vendor/pyroki/viewer/_manipulability_ellipse.py +197 -0
- archeone/_vendor/pyroki/viewer/_weight_tuner.py +112 -0
- archeone/client.py +354 -0
- archeone/credentials.py +74 -0
- archeone/errors.py +150 -0
- archeone/ik/__init__.py +6 -0
- archeone/ik/dream_ik/__init__.py +19 -0
- archeone/ik/dream_ik/assets/piper_dual_left.urdf +179 -0
- archeone/ik/dream_ik/assets/piper_dual_right.urdf +179 -0
- archeone/ik/dream_ik/assets/piper_right.urdf +179 -0
- archeone/ik/dream_ik/assets/yam.urdf +221 -0
- archeone/ik/dream_ik/decode.py +194 -0
- archeone/ik/dream_ik/solver.py +923 -0
- archeone/ik/dream_ik/specs.py +655 -0
- archeone/ik/solver.py +528 -0
- archeone/observation.py +124 -0
- archeone/pacing.py +42 -0
- archeone/py.typed +1 -0
- archeone/seam_join.py +182 -0
- archeone/smoothing.py +226 -0
- archeone/transport.py +227 -0
- archeone/window.py +140 -0
- archeone-0.1.0.data/data/share/archeone/docs/quickstart.md +412 -0
- archeone-0.1.0.dist-info/METADATA +181 -0
- archeone-0.1.0.dist-info/RECORD +81 -0
- archeone-0.1.0.dist-info/WHEEL +5 -0
- archeone-0.1.0.dist-info/licenses/LICENSE +202 -0
- archeone-0.1.0.dist-info/licenses/licenses/dreamlabs-LICENSE +202 -0
- archeone-0.1.0.dist-info/licenses/licenses/dreamlabs-NOTICE +60 -0
- archeone-0.1.0.dist-info/licenses/licenses/jaxls-LICENSE +21 -0
- archeone-0.1.0.dist-info/licenses/licenses/pyroki-LICENSE +21 -0
- archeone-0.1.0.dist-info/top_level.txt +1 -0
archeone/__init__.py
ADDED
|
@@ -0,0 +1,44 @@
|
|
|
1
|
+
"""Public API for archeone."""
|
|
2
|
+
|
|
3
|
+
from .client import ArcheoneClient
|
|
4
|
+
from .errors import (
|
|
5
|
+
ActionError,
|
|
6
|
+
ArcheoneError,
|
|
7
|
+
AuthError,
|
|
8
|
+
BadRequest,
|
|
9
|
+
CapacityError,
|
|
10
|
+
ConfigError,
|
|
11
|
+
ModelError,
|
|
12
|
+
ObservationError,
|
|
13
|
+
ProtocolError,
|
|
14
|
+
ServerError,
|
|
15
|
+
SessionReplaced,
|
|
16
|
+
Timeout,
|
|
17
|
+
TransportError,
|
|
18
|
+
)
|
|
19
|
+
from .pacing import SYNC, Pacer
|
|
20
|
+
from .window import Window
|
|
21
|
+
|
|
22
|
+
__version__ = "0.1.0"
|
|
23
|
+
|
|
24
|
+
__all__ = [
|
|
25
|
+
"ActionError",
|
|
26
|
+
"ArcheoneClient",
|
|
27
|
+
"ArcheoneError",
|
|
28
|
+
"AuthError",
|
|
29
|
+
"BadRequest",
|
|
30
|
+
"CapacityError",
|
|
31
|
+
"ConfigError",
|
|
32
|
+
"ModelError",
|
|
33
|
+
"ObservationError",
|
|
34
|
+
"Pacer",
|
|
35
|
+
"ProtocolError",
|
|
36
|
+
"SYNC",
|
|
37
|
+
"ServerError",
|
|
38
|
+
"SessionReplaced",
|
|
39
|
+
"Timeout",
|
|
40
|
+
"TransportError",
|
|
41
|
+
"Window",
|
|
42
|
+
"__version__",
|
|
43
|
+
]
|
|
44
|
+
|
|
@@ -0,0 +1,12 @@
|
|
|
1
|
+
"""Vendored IK dependencies (jaxls, pyroki), kept under their original licenses."""
|
|
2
|
+
|
|
3
|
+
import sys
|
|
4
|
+
|
|
5
|
+
# pyroki and dream-style solvers import these by their upstream names. Bind the
|
|
6
|
+
# vendored copies before any installed top-level package can shadow them.
|
|
7
|
+
from . import jaxls, pyroki
|
|
8
|
+
|
|
9
|
+
sys.modules.setdefault("jaxls", jaxls)
|
|
10
|
+
sys.modules.setdefault("pyroki", pyroki)
|
|
11
|
+
|
|
12
|
+
__all__ = ["jaxls", "pyroki"]
|
|
@@ -0,0 +1,21 @@
|
|
|
1
|
+
MIT License
|
|
2
|
+
|
|
3
|
+
Copyright (c) 2025 Brent Yi
|
|
4
|
+
|
|
5
|
+
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
6
|
+
of this software and associated documentation files (the "Software"), to deal
|
|
7
|
+
in the Software without restriction, including without limitation the rights
|
|
8
|
+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
9
|
+
copies of the Software, and to permit persons to whom the Software is
|
|
10
|
+
furnished to do so, subject to the following conditions:
|
|
11
|
+
|
|
12
|
+
The above copyright notice and this permission notice shall be included in all
|
|
13
|
+
copies or substantial portions of the Software.
|
|
14
|
+
|
|
15
|
+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
16
|
+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
17
|
+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
18
|
+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
19
|
+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
20
|
+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
21
|
+
SOFTWARE.
|
|
@@ -0,0 +1,103 @@
|
|
|
1
|
+
import sys
|
|
2
|
+
from typing import TYPE_CHECKING
|
|
3
|
+
|
|
4
|
+
if TYPE_CHECKING or sys.version_info >= (3, 12):
|
|
5
|
+
from . import utils as utils
|
|
6
|
+
from ._augmented_lagrangian import (
|
|
7
|
+
AugmentedLagrangianConfig as AugmentedLagrangianConfig,
|
|
8
|
+
)
|
|
9
|
+
from ._cost import Cost as Cost
|
|
10
|
+
from ._covariance import CovarianceEstimator as CovarianceEstimator
|
|
11
|
+
from ._covariance import (
|
|
12
|
+
LinearSolverCovarianceEstimatorConfig as LinearSolverCovarianceEstimatorConfig,
|
|
13
|
+
)
|
|
14
|
+
from ._lie_group_variables import SE2Var as SE2Var
|
|
15
|
+
from ._lie_group_variables import SE3Var as SE3Var
|
|
16
|
+
from ._lie_group_variables import SO2Var as SO2Var
|
|
17
|
+
from ._lie_group_variables import SO3Var as SO3Var
|
|
18
|
+
from ._problem import AnalyzedLeastSquaresProblem as AnalyzedLeastSquaresProblem
|
|
19
|
+
from ._problem import LeastSquaresProblem as LeastSquaresProblem
|
|
20
|
+
from ._solvers import ConjugateGradientConfig as ConjugateGradientConfig
|
|
21
|
+
from ._solvers import record_iteration_times as record_iteration_times
|
|
22
|
+
from ._solvers import SolveSummary as SolveSummary
|
|
23
|
+
from ._solvers import TerminationConfig as TerminationConfig
|
|
24
|
+
from ._solvers import TrustRegionConfig as TrustRegionConfig
|
|
25
|
+
from ._variables import Var as Var
|
|
26
|
+
from ._variables import VarValues as VarValues
|
|
27
|
+
elif sys.version_info >= (3, 10):
|
|
28
|
+
from ._py310 import utils as utils
|
|
29
|
+
from ._py310._augmented_lagrangian import (
|
|
30
|
+
AugmentedLagrangianConfig as AugmentedLagrangianConfig,
|
|
31
|
+
)
|
|
32
|
+
from ._py310._cost import Cost as Cost
|
|
33
|
+
from ._py310._covariance import CovarianceEstimator as CovarianceEstimator
|
|
34
|
+
from ._py310._covariance import (
|
|
35
|
+
LinearSolverCovarianceEstimatorConfig as LinearSolverCovarianceEstimatorConfig,
|
|
36
|
+
)
|
|
37
|
+
from ._py310._lie_group_variables import SE2Var as SE2Var
|
|
38
|
+
from ._py310._lie_group_variables import SE3Var as SE3Var
|
|
39
|
+
from ._py310._lie_group_variables import SO2Var as SO2Var
|
|
40
|
+
from ._py310._lie_group_variables import SO3Var as SO3Var
|
|
41
|
+
from ._py310._problem import (
|
|
42
|
+
AnalyzedLeastSquaresProblem as AnalyzedLeastSquaresProblem,
|
|
43
|
+
)
|
|
44
|
+
from ._py310._problem import LeastSquaresProblem as LeastSquaresProblem
|
|
45
|
+
from ._py310._solvers import ConjugateGradientConfig as ConjugateGradientConfig
|
|
46
|
+
from ._py310._solvers import record_iteration_times as record_iteration_times
|
|
47
|
+
from ._py310._solvers import SolveSummary as SolveSummary
|
|
48
|
+
from ._py310._solvers import TerminationConfig as TerminationConfig
|
|
49
|
+
from ._py310._solvers import TrustRegionConfig as TrustRegionConfig
|
|
50
|
+
from ._py310._variables import Var as Var
|
|
51
|
+
from ._py310._variables import VarValues as VarValues
|
|
52
|
+
else:
|
|
53
|
+
assert False
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
__version__ = "0.0.0"
|
|
57
|
+
|
|
58
|
+
# Some shims for backwards compatibility.
|
|
59
|
+
if not TYPE_CHECKING:
|
|
60
|
+
import sys
|
|
61
|
+
from typing import Any
|
|
62
|
+
|
|
63
|
+
# Create a descriptor for Factor that shows a deprecation warning when accessed
|
|
64
|
+
class _FactorDescriptor:
|
|
65
|
+
def __get__(self, obj: Any, objtype: Any = None) -> Any:
|
|
66
|
+
utils.print_deprecation_warning(
|
|
67
|
+
"Import detected for [yellow]jaxls.Factor[/yellow], which has been renamed:",
|
|
68
|
+
"[yellow]jaxls.Factor[/yellow] has been renamed to [green]jaxls.Cost[/green]. "
|
|
69
|
+
"See migration guide at [blue]https://github.com/brentyi/jaxls/pull/36[/blue] for details. "
|
|
70
|
+
"To suppress this warning, you can also downgrade jaxls: [bold]pip install git+https://github.com/brentyi/jaxls.git@21219e08676e13bb481230907e1cbc486ee284c7[/bold].",
|
|
71
|
+
)
|
|
72
|
+
|
|
73
|
+
# Standard warning is now handled by print_deprecation_warning
|
|
74
|
+
return Cost
|
|
75
|
+
|
|
76
|
+
# Create a descriptor for FactorGraph that shows a deprecation warning when accessed
|
|
77
|
+
class _FactorGraphDescriptor:
|
|
78
|
+
def __get__(self, obj: Any, objtype: Any = None) -> Any:
|
|
79
|
+
utils.print_deprecation_warning(
|
|
80
|
+
"Import detected for [yellow]jaxls.FactorGraph[/yellow], which has been renamed:",
|
|
81
|
+
"[yellow]jaxls.FactorGraph.make(...)[/yellow] has been replaced with [green]jaxls.LeastSquaresProblem(...).analyze()[/green]. "
|
|
82
|
+
"See migration guide at [blue]https://github.com/brentyi/jaxls/pull/36[/blue] for details. "
|
|
83
|
+
"To suppress this warning, you can also downgrade jaxls: [bold]pip install git+https://github.com/brentyi/jaxls.git@21219e08676e13bb481230907e1cbc486ee284c7[/bold].",
|
|
84
|
+
)
|
|
85
|
+
|
|
86
|
+
class FactorGraph:
|
|
87
|
+
@staticmethod
|
|
88
|
+
def make(factors, variables, use_onp=False):
|
|
89
|
+
return LeastSquaresProblem(
|
|
90
|
+
costs=factors, variables=variables
|
|
91
|
+
).analyze(use_onp)
|
|
92
|
+
|
|
93
|
+
return FactorGraph
|
|
94
|
+
|
|
95
|
+
# Use the descriptors to trigger warnings when Factor or FactorGraph are accessed
|
|
96
|
+
sys.modules[__name__].__class__ = type(
|
|
97
|
+
sys.modules[__name__].__class__.__name__,
|
|
98
|
+
(sys.modules[__name__].__class__,),
|
|
99
|
+
{
|
|
100
|
+
"Factor": _FactorDescriptor(),
|
|
101
|
+
"FactorGraph": _FactorGraphDescriptor(),
|
|
102
|
+
},
|
|
103
|
+
)
|
|
@@ -0,0 +1,420 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
3
|
+
from typing import TYPE_CHECKING, Any, Callable, cast
|
|
4
|
+
|
|
5
|
+
import jax
|
|
6
|
+
import jax_dataclasses as jdc
|
|
7
|
+
import numpy as onp
|
|
8
|
+
from jax import numpy as jnp
|
|
9
|
+
from jax.nn import relu
|
|
10
|
+
|
|
11
|
+
from ._cost import Cost, CustomJacobianCache
|
|
12
|
+
from ._variables import Var, VarTypeOrdering, VarValues, sort_and_stack_vars
|
|
13
|
+
|
|
14
|
+
if TYPE_CHECKING:
|
|
15
|
+
pass
|
|
16
|
+
|
|
17
|
+
|
|
18
|
+
@jdc.pytree_dataclass
|
|
19
|
+
class AugmentedLagrangianParams[*Args]:
|
|
20
|
+
"""Parameters for a single augmented constraint cost.
|
|
21
|
+
|
|
22
|
+
Each augmented cost gets its own params with arrays for just that constraint.
|
|
23
|
+
The original cost args are bundled here for type-safe access.
|
|
24
|
+
"""
|
|
25
|
+
|
|
26
|
+
lagrange_multipliers: jax.Array
|
|
27
|
+
"""Lagrange multipliers for this constraint. Shape: (constraint_flat_dim,)."""
|
|
28
|
+
|
|
29
|
+
penalty_params: jax.Array
|
|
30
|
+
"""Penalty parameter for this constraint instance. Shape: () (scalar)."""
|
|
31
|
+
|
|
32
|
+
original_args: tuple[*Args]
|
|
33
|
+
"""The original cost args to pass to compute_residual."""
|
|
34
|
+
|
|
35
|
+
constraint_index: jdc.Static[int]
|
|
36
|
+
"""Index used to group constraints with the same structure for vectorized updates.
|
|
37
|
+
|
|
38
|
+
Constraints with the same structure (pytree shape, array shapes) are assigned the
|
|
39
|
+
same constraint_index so the AL solver can update their parameters together.
|
|
40
|
+
"""
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
@jdc.pytree_dataclass(kw_only=True)
|
|
44
|
+
class _AnalyzedCost[*Args](Cost[*Args]):
|
|
45
|
+
"""Analyzed cost ready for optimization.
|
|
46
|
+
|
|
47
|
+
Extends Cost with precomputed metadata needed for efficient Jacobian
|
|
48
|
+
computation and sparse matrix assembly. Used for both regular costs
|
|
49
|
+
and constraint-mode costs internally.
|
|
50
|
+
"""
|
|
51
|
+
|
|
52
|
+
num_variables: jdc.Static[int]
|
|
53
|
+
sorted_ids_from_var_type: dict[type[Var[Any]], jax.Array]
|
|
54
|
+
residual_flat_dim: jdc.Static[int] = 0
|
|
55
|
+
|
|
56
|
+
# For constraint-mode costs, stores the original constraint function
|
|
57
|
+
# (before augmented Lagrangian transformation). None for regular costs.
|
|
58
|
+
compute_residual_original: jdc.Static[Callable[..., jax.Array] | None] = None
|
|
59
|
+
"""For constraint-mode costs, computes the raw constraint value (not augmented).
|
|
60
|
+
Takes (vals, al_params) and returns the original constraint value."""
|
|
61
|
+
|
|
62
|
+
def compute_residual_flat(
|
|
63
|
+
self, vals: VarValues, *args: *Args
|
|
64
|
+
) -> jax.Array | tuple[jax.Array, CustomJacobianCache]:
|
|
65
|
+
"""Compute residual and flatten to 1D array.
|
|
66
|
+
|
|
67
|
+
Calls the underlying compute_residual function and flattens the output.
|
|
68
|
+
If compute_residual returns a tuple (residual, cache), the flattened
|
|
69
|
+
residual and cache are returned together.
|
|
70
|
+
|
|
71
|
+
Args:
|
|
72
|
+
vals: Variable values to evaluate the residual at.
|
|
73
|
+
*args: Additional arguments passed to compute_residual.
|
|
74
|
+
|
|
75
|
+
Returns:
|
|
76
|
+
Either a flattened residual array, or a tuple of (flattened_residual, cache)
|
|
77
|
+
if the compute_residual function returns a Jacobian cache.
|
|
78
|
+
"""
|
|
79
|
+
out = self.compute_residual(vals, *args)
|
|
80
|
+
|
|
81
|
+
# Flatten residual vector.
|
|
82
|
+
if isinstance(out, tuple):
|
|
83
|
+
assert len(out) == 2
|
|
84
|
+
out = (out[0].flatten(), out[1])
|
|
85
|
+
else:
|
|
86
|
+
out = out.flatten()
|
|
87
|
+
|
|
88
|
+
return out
|
|
89
|
+
|
|
90
|
+
@staticmethod
|
|
91
|
+
@jdc.jit
|
|
92
|
+
def _make[*Args_](
|
|
93
|
+
cost: Cost[*Args_],
|
|
94
|
+
) -> (
|
|
95
|
+
_AnalyzedCost[*Args_] | _AnalyzedCost[tuple[AugmentedLagrangianParams[*Args_]]]
|
|
96
|
+
):
|
|
97
|
+
"""Construct an analyzed cost from Cost.
|
|
98
|
+
|
|
99
|
+
For constraint-mode costs (constraint is not None), this converts to augmented
|
|
100
|
+
Lagrangian form with placeholder AL params.
|
|
101
|
+
"""
|
|
102
|
+
|
|
103
|
+
# Handle constraint modes -> augmented _AnalyzedCost conversion
|
|
104
|
+
if cost.kind != "l2_squared":
|
|
105
|
+
return _augment_constraint_cost(cost)
|
|
106
|
+
|
|
107
|
+
# Handle regular cost -> _AnalyzedCost conversion
|
|
108
|
+
variables = cost._get_variables()
|
|
109
|
+
assert len(variables) > 0
|
|
110
|
+
|
|
111
|
+
# Support batch axis.
|
|
112
|
+
if not isinstance(variables[0].id, int):
|
|
113
|
+
batch_axes = variables[0].id.shape
|
|
114
|
+
assert len(batch_axes) in (0, 1)
|
|
115
|
+
for var in variables[1:]:
|
|
116
|
+
assert (
|
|
117
|
+
() if isinstance(var.id, int) else var.id.shape
|
|
118
|
+
) == batch_axes, "Batch axes of variables do not match."
|
|
119
|
+
if len(batch_axes) == 1:
|
|
120
|
+
return jax.vmap(_AnalyzedCost._make)(cost)
|
|
121
|
+
|
|
122
|
+
# Same as `compute_residual`, but removes Jacobian cache if present.
|
|
123
|
+
def _residual_no_cache(*args) -> jax.Array:
|
|
124
|
+
residual_out = cost.compute_residual(*args) # type: ignore
|
|
125
|
+
if isinstance(residual_out, tuple):
|
|
126
|
+
assert len(residual_out) == 2
|
|
127
|
+
return residual_out[0]
|
|
128
|
+
else:
|
|
129
|
+
return residual_out
|
|
130
|
+
|
|
131
|
+
# Cache the residual dimension for this cost.
|
|
132
|
+
dummy_vals = jax.eval_shape(VarValues.make, variables)
|
|
133
|
+
residual_dim = int(
|
|
134
|
+
onp.prod(jax.eval_shape(_residual_no_cache, dummy_vals, *cost.args).shape)
|
|
135
|
+
)
|
|
136
|
+
|
|
137
|
+
return _AnalyzedCost(
|
|
138
|
+
compute_residual=cost.compute_residual,
|
|
139
|
+
args=cost.args,
|
|
140
|
+
kind=cost.kind,
|
|
141
|
+
jac_mode=cost.jac_mode,
|
|
142
|
+
jac_batch_size=cost.jac_batch_size,
|
|
143
|
+
jac_custom_fn=cost.jac_custom_fn,
|
|
144
|
+
jac_custom_with_cache_fn=cost.jac_custom_with_cache_fn,
|
|
145
|
+
name=cost.name,
|
|
146
|
+
num_variables=len(variables),
|
|
147
|
+
sorted_ids_from_var_type=sort_and_stack_vars(variables),
|
|
148
|
+
residual_flat_dim=residual_dim,
|
|
149
|
+
)
|
|
150
|
+
|
|
151
|
+
def _compute_block_sparse_jac_indices(
|
|
152
|
+
self,
|
|
153
|
+
tangent_ordering: VarTypeOrdering,
|
|
154
|
+
sorted_ids_from_var_type: dict[type[Var[Any]], jax.Array],
|
|
155
|
+
tangent_start_from_var_type: dict[type[Var[Any]], int],
|
|
156
|
+
) -> tuple[jax.Array, jax.Array]:
|
|
157
|
+
"""Compute row and column indices for block-sparse Jacobian.
|
|
158
|
+
|
|
159
|
+
Computes the sparsity pattern for a Jacobian of shape
|
|
160
|
+
(residual_dim, total_tangent_dim). The indices map each residual
|
|
161
|
+
element to its corresponding tangent space elements.
|
|
162
|
+
|
|
163
|
+
Args:
|
|
164
|
+
tangent_ordering: Ordering of variable types for consistent indexing.
|
|
165
|
+
sorted_ids_from_var_type: Sorted variable IDs for each type.
|
|
166
|
+
tangent_start_from_var_type: Starting column index for each variable type.
|
|
167
|
+
|
|
168
|
+
Returns:
|
|
169
|
+
Tuple of (row_indices, col_indices) for the sparse Jacobian.
|
|
170
|
+
Row indices start at 0 for this cost's residual block.
|
|
171
|
+
"""
|
|
172
|
+
col_indices = list[jax.Array]()
|
|
173
|
+
for var_type, ids in tangent_ordering.ordered_dict_items(
|
|
174
|
+
self.sorted_ids_from_var_type
|
|
175
|
+
):
|
|
176
|
+
var_indices = jnp.searchsorted(sorted_ids_from_var_type[var_type], ids)
|
|
177
|
+
tangent_start = tangent_start_from_var_type[var_type]
|
|
178
|
+
tangent_indices = (
|
|
179
|
+
onp.arange(tangent_start, tangent_start + var_type.tangent_dim)[None, :]
|
|
180
|
+
+ var_indices[:, None] * var_type.tangent_dim
|
|
181
|
+
)
|
|
182
|
+
assert tangent_indices.shape == (
|
|
183
|
+
var_indices.shape[0],
|
|
184
|
+
var_type.tangent_dim,
|
|
185
|
+
)
|
|
186
|
+
col_indices.append(cast(jax.Array, tangent_indices).flatten())
|
|
187
|
+
rows, cols = jnp.meshgrid(
|
|
188
|
+
jnp.arange(self.residual_flat_dim),
|
|
189
|
+
jnp.concatenate(col_indices, axis=0),
|
|
190
|
+
indexing="ij",
|
|
191
|
+
)
|
|
192
|
+
return rows, cols
|
|
193
|
+
|
|
194
|
+
|
|
195
|
+
def _augment_constraint_cost[*Args](
|
|
196
|
+
cost: Cost[*Args], constraint_index: int = 0
|
|
197
|
+
) -> _AnalyzedCost[tuple[AugmentedLagrangianParams[*Args]]]:
|
|
198
|
+
"""Augment a constraint-mode cost and convert it to augmented _AnalyzedCost form.
|
|
199
|
+
|
|
200
|
+
This creates an _AnalyzedCost that wraps the cost with the augmented
|
|
201
|
+
Lagrangian formulation, with placeholder AL params that will be updated
|
|
202
|
+
during optimization.
|
|
203
|
+
|
|
204
|
+
For equality constraints (kind="constraint_eq_zero"): h(x) = 0
|
|
205
|
+
r = sqrt(rho) * (h(x) + lambda/rho)
|
|
206
|
+
|
|
207
|
+
For inequality constraints (kind="constraint_leq_zero"): g(x) <= 0
|
|
208
|
+
r = sqrt(rho) * max(0, g(x) + lambda/rho)
|
|
209
|
+
|
|
210
|
+
For inequality constraints (kind="constraint_geq_zero"): g(x) >= 0
|
|
211
|
+
Internally converted to -g(x) <= 0, then treated as leq_zero.
|
|
212
|
+
|
|
213
|
+
Args:
|
|
214
|
+
cost: The constraint-mode cost to analyze.
|
|
215
|
+
constraint_index: Index for this constraint (used by AL solver).
|
|
216
|
+
|
|
217
|
+
Returns:
|
|
218
|
+
An _AnalyzedCost with augmented residual and placeholder AL params.
|
|
219
|
+
"""
|
|
220
|
+
assert cost.kind != "l2_squared", (
|
|
221
|
+
"Only constraint-mode costs should be augmented here"
|
|
222
|
+
)
|
|
223
|
+
|
|
224
|
+
variables = cost._get_variables()
|
|
225
|
+
assert len(variables) > 0
|
|
226
|
+
|
|
227
|
+
# Support batch axis.
|
|
228
|
+
if not isinstance(variables[0].id, int):
|
|
229
|
+
batch_axes = variables[0].id.shape
|
|
230
|
+
assert len(batch_axes) in (0, 1)
|
|
231
|
+
for var in variables[1:]:
|
|
232
|
+
assert (() if isinstance(var.id, int) else var.id.shape) == batch_axes, (
|
|
233
|
+
"Batch axes of variables do not match."
|
|
234
|
+
)
|
|
235
|
+
if len(batch_axes) == 1:
|
|
236
|
+
return cast(
|
|
237
|
+
_AnalyzedCost[Any],
|
|
238
|
+
jax.vmap(lambda c: _augment_constraint_cost(c, constraint_index))(cost),
|
|
239
|
+
)
|
|
240
|
+
|
|
241
|
+
# Compute constraint dimension.
|
|
242
|
+
def _constraint_no_cache(*args) -> jax.Array:
|
|
243
|
+
constraint_out = cost.compute_residual(*args) # type: ignore
|
|
244
|
+
if isinstance(constraint_out, tuple):
|
|
245
|
+
assert len(constraint_out) == 2
|
|
246
|
+
return constraint_out[0]
|
|
247
|
+
else:
|
|
248
|
+
return constraint_out
|
|
249
|
+
|
|
250
|
+
dummy_vals = jax.eval_shape(VarValues.make, variables)
|
|
251
|
+
constraint_dim = int(
|
|
252
|
+
onp.prod(jax.eval_shape(_constraint_no_cache, dummy_vals, *cost.args).shape)
|
|
253
|
+
)
|
|
254
|
+
|
|
255
|
+
# Create placeholder AL params.
|
|
256
|
+
# Note: penalty_params is scalar (per-instance), lagrange_multipliers is per-element.
|
|
257
|
+
al_params = AugmentedLagrangianParams(
|
|
258
|
+
lagrange_multipliers=jnp.zeros(constraint_dim),
|
|
259
|
+
penalty_params=jnp.array(1.0), # Scalar per-instance penalty.
|
|
260
|
+
original_args=cost.args,
|
|
261
|
+
constraint_index=constraint_index,
|
|
262
|
+
)
|
|
263
|
+
|
|
264
|
+
# Capture cost for closures.
|
|
265
|
+
orig_compute_residual = cost.compute_residual
|
|
266
|
+
orig_kind = cost.kind
|
|
267
|
+
|
|
268
|
+
# Determine if this is an inequality constraint
|
|
269
|
+
is_leq = orig_kind == "constraint_leq_zero"
|
|
270
|
+
is_geq = orig_kind == "constraint_geq_zero"
|
|
271
|
+
is_inequality = is_leq or is_geq
|
|
272
|
+
|
|
273
|
+
# Determine if we need to return a cache for the active mask.
|
|
274
|
+
# This is needed when we have inequality constraints with custom Jacobians.
|
|
275
|
+
needs_active_mask_cache = is_inequality and (
|
|
276
|
+
cost.jac_custom_fn is not None or cost.jac_custom_with_cache_fn is not None
|
|
277
|
+
)
|
|
278
|
+
|
|
279
|
+
def augmented_residual_fn(
|
|
280
|
+
vals: VarValues,
|
|
281
|
+
al_params_inner: AugmentedLagrangianParams[*Args],
|
|
282
|
+
) -> jax.Array | tuple[jax.Array, Any]:
|
|
283
|
+
"""Compute augmented constraint residual with per-constraint penalty.
|
|
284
|
+
|
|
285
|
+
Applies the augmented Lagrangian transformation to convert constraint
|
|
286
|
+
satisfaction into a squared penalty term suitable for least-squares.
|
|
287
|
+
"""
|
|
288
|
+
constraint_out = orig_compute_residual(vals, *al_params_inner.original_args)
|
|
289
|
+
|
|
290
|
+
# Handle Jacobian cache if present.
|
|
291
|
+
if isinstance(constraint_out, tuple):
|
|
292
|
+
assert len(constraint_out) == 2
|
|
293
|
+
constraint_val = constraint_out[0].flatten()
|
|
294
|
+
orig_jac_cache = constraint_out[1]
|
|
295
|
+
else:
|
|
296
|
+
constraint_val = constraint_out.flatten()
|
|
297
|
+
orig_jac_cache = None
|
|
298
|
+
|
|
299
|
+
# For geq_zero, negate to convert to leq_zero form
|
|
300
|
+
if is_geq:
|
|
301
|
+
constraint_val = -constraint_val
|
|
302
|
+
|
|
303
|
+
# For inequality constraints: only penalize when violated (max formulation)
|
|
304
|
+
# For equality constraints: always penalize deviation
|
|
305
|
+
lambdas = al_params_inner.lagrange_multipliers
|
|
306
|
+
rho = al_params_inner.penalty_params
|
|
307
|
+
if is_inequality:
|
|
308
|
+
# g(x) <= 0: penalize only when g(x) + lambda/rho > 0
|
|
309
|
+
active = (constraint_val + lambdas / rho) > 0
|
|
310
|
+
residual = jnp.sqrt(rho) * relu(constraint_val + lambdas / rho)
|
|
311
|
+
else:
|
|
312
|
+
# h(x) = 0: always penalize
|
|
313
|
+
active = None
|
|
314
|
+
residual = jnp.sqrt(rho) * (constraint_val + lambdas / rho)
|
|
315
|
+
|
|
316
|
+
# Return cache if needed (either original cache or active mask for custom Jacobians).
|
|
317
|
+
if orig_jac_cache is not None or needs_active_mask_cache:
|
|
318
|
+
# Bundle original cache with active mask for inequality constraints.
|
|
319
|
+
return residual, (orig_jac_cache, active)
|
|
320
|
+
return residual
|
|
321
|
+
|
|
322
|
+
# Create wrapper Jacobian functions if original cost has custom Jacobians.
|
|
323
|
+
# When we have inequality constraints with custom Jacobians, we cache the active
|
|
324
|
+
# mask in the residual computation to avoid redundant constraint evaluation.
|
|
325
|
+
wrapped_jac_custom_fn = None
|
|
326
|
+
wrapped_jac_custom_with_cache_fn = None
|
|
327
|
+
|
|
328
|
+
if cost.jac_custom_fn is not None:
|
|
329
|
+
orig_jac_fn = cost.jac_custom_fn
|
|
330
|
+
|
|
331
|
+
if is_inequality:
|
|
332
|
+
# For inequality constraints, use cache-based wrapper to get active mask.
|
|
333
|
+
def _wrapped_jac_with_cache_from_custom_fn(
|
|
334
|
+
vals: VarValues,
|
|
335
|
+
jac_cache: tuple[None, jax.Array],
|
|
336
|
+
al_params_inner: AugmentedLagrangianParams[*Args],
|
|
337
|
+
) -> jax.Array:
|
|
338
|
+
"""Wrapper using cached active mask for inequality constraint Jacobian."""
|
|
339
|
+
original_jac = orig_jac_fn(vals, *al_params_inner.original_args)
|
|
340
|
+
if is_geq:
|
|
341
|
+
original_jac = -original_jac
|
|
342
|
+
rho = al_params_inner.penalty_params # Scalar per-instance penalty.
|
|
343
|
+
_, active = jac_cache # Extract cached active mask.
|
|
344
|
+
return jnp.sqrt(rho) * original_jac * active[:, None]
|
|
345
|
+
|
|
346
|
+
wrapped_jac_custom_with_cache_fn = _wrapped_jac_with_cache_from_custom_fn
|
|
347
|
+
else:
|
|
348
|
+
# Equality constraints don't need active mask.
|
|
349
|
+
def _wrapped_jac_custom_fn(
|
|
350
|
+
vals: VarValues,
|
|
351
|
+
al_params_inner: AugmentedLagrangianParams[*Args],
|
|
352
|
+
) -> jax.Array:
|
|
353
|
+
"""Wrapper Jacobian for equality constraints."""
|
|
354
|
+
original_jac = orig_jac_fn(vals, *al_params_inner.original_args)
|
|
355
|
+
rho = al_params_inner.penalty_params # Scalar per-instance penalty.
|
|
356
|
+
return jnp.sqrt(rho) * original_jac
|
|
357
|
+
|
|
358
|
+
wrapped_jac_custom_fn = _wrapped_jac_custom_fn
|
|
359
|
+
|
|
360
|
+
if cost.jac_custom_with_cache_fn is not None:
|
|
361
|
+
orig_jac_with_cache_fn = cost.jac_custom_with_cache_fn
|
|
362
|
+
|
|
363
|
+
def _wrapped_jac_custom_with_cache_fn(
|
|
364
|
+
vals: VarValues,
|
|
365
|
+
jac_cache: tuple[CustomJacobianCache, jax.Array | None],
|
|
366
|
+
al_params_inner: AugmentedLagrangianParams[*Args],
|
|
367
|
+
) -> jax.Array:
|
|
368
|
+
"""Wrapper Jacobian with cache that applies chain rule for augmentation."""
|
|
369
|
+
orig_cache, active = jac_cache # Unpack bundled cache.
|
|
370
|
+
original_jac = orig_jac_with_cache_fn(
|
|
371
|
+
vals, orig_cache, *al_params_inner.original_args
|
|
372
|
+
)
|
|
373
|
+
|
|
374
|
+
# For geq_zero, negate the Jacobian.
|
|
375
|
+
if is_geq:
|
|
376
|
+
original_jac = -original_jac
|
|
377
|
+
|
|
378
|
+
rho = al_params_inner.penalty_params # Scalar per-instance penalty.
|
|
379
|
+
if is_inequality:
|
|
380
|
+
assert active is not None
|
|
381
|
+
return jnp.sqrt(rho) * original_jac * active[:, None]
|
|
382
|
+
else:
|
|
383
|
+
return jnp.sqrt(rho) * original_jac
|
|
384
|
+
|
|
385
|
+
wrapped_jac_custom_with_cache_fn = _wrapped_jac_custom_with_cache_fn
|
|
386
|
+
|
|
387
|
+
# Create function to compute original constraint value (without augmentation).
|
|
388
|
+
def compute_residual_original_fn(
|
|
389
|
+
vals: VarValues,
|
|
390
|
+
al_params_inner: AugmentedLagrangianParams[*Args],
|
|
391
|
+
) -> jax.Array:
|
|
392
|
+
"""Compute original constraint value (not augmented).
|
|
393
|
+
|
|
394
|
+
Used by the AL solver to track constraint violations and update
|
|
395
|
+
Lagrange multipliers.
|
|
396
|
+
"""
|
|
397
|
+
constraint_out = orig_compute_residual(vals, *al_params_inner.original_args)
|
|
398
|
+
if isinstance(constraint_out, tuple):
|
|
399
|
+
constraint_val = constraint_out[0].flatten()
|
|
400
|
+
else:
|
|
401
|
+
constraint_val = constraint_out.flatten()
|
|
402
|
+
# For geq_zero, negate to get the leq_zero equivalent
|
|
403
|
+
if is_geq:
|
|
404
|
+
constraint_val = -constraint_val
|
|
405
|
+
return constraint_val
|
|
406
|
+
|
|
407
|
+
return _AnalyzedCost(
|
|
408
|
+
compute_residual=augmented_residual_fn, # type: ignore[arg-type]
|
|
409
|
+
args=(al_params,),
|
|
410
|
+
kind=cost.kind,
|
|
411
|
+
jac_mode=cost.jac_mode,
|
|
412
|
+
jac_batch_size=cost.jac_batch_size,
|
|
413
|
+
jac_custom_fn=wrapped_jac_custom_fn,
|
|
414
|
+
jac_custom_with_cache_fn=wrapped_jac_custom_with_cache_fn,
|
|
415
|
+
name=f"augmented_{cost._get_name()}",
|
|
416
|
+
num_variables=len(variables),
|
|
417
|
+
sorted_ids_from_var_type=sort_and_stack_vars(variables),
|
|
418
|
+
residual_flat_dim=constraint_dim,
|
|
419
|
+
compute_residual_original=compute_residual_original_fn,
|
|
420
|
+
)
|