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.
Files changed (81) hide show
  1. archeone/__init__.py +44 -0
  2. archeone/_vendor/__init__.py +12 -0
  3. archeone/_vendor/jaxls/LICENSE +21 -0
  4. archeone/_vendor/jaxls/__init__.py +103 -0
  5. archeone/_vendor/jaxls/_analyzed_cost.py +420 -0
  6. archeone/_vendor/jaxls/_augmented_lagrangian.py +368 -0
  7. archeone/_vendor/jaxls/_cost.py +374 -0
  8. archeone/_vendor/jaxls/_covariance.py +179 -0
  9. archeone/_vendor/jaxls/_lie_group_variables.py +35 -0
  10. archeone/_vendor/jaxls/_preconditioning.py +131 -0
  11. archeone/_vendor/jaxls/_problem.py +1053 -0
  12. archeone/_vendor/jaxls/_py310/__init__.py +0 -0
  13. archeone/_vendor/jaxls/_py310/_analyzed_cost.py +297 -0
  14. archeone/_vendor/jaxls/_py310/_augmented_lagrangian.py +268 -0
  15. archeone/_vendor/jaxls/_py310/_cost.py +185 -0
  16. archeone/_vendor/jaxls/_py310/_covariance.py +96 -0
  17. archeone/_vendor/jaxls/_py310/_lie_group_variables.py +56 -0
  18. archeone/_vendor/jaxls/_py310/_preconditioning.py +114 -0
  19. archeone/_vendor/jaxls/_py310/_problem.py +722 -0
  20. archeone/_vendor/jaxls/_py310/_schur.py +914 -0
  21. archeone/_vendor/jaxls/_py310/_solvers.py +849 -0
  22. archeone/_vendor/jaxls/_py310/_sparse_matrices.py +198 -0
  23. archeone/_vendor/jaxls/_py310/_variables.py +292 -0
  24. archeone/_vendor/jaxls/_py310/_visualization.py +533 -0
  25. archeone/_vendor/jaxls/_py310/utils.py +113 -0
  26. archeone/_vendor/jaxls/_schur.py +1210 -0
  27. archeone/_vendor/jaxls/_solvers.py +1210 -0
  28. archeone/_vendor/jaxls/_sparse_matrices.py +257 -0
  29. archeone/_vendor/jaxls/_variables.py +408 -0
  30. archeone/_vendor/jaxls/_visualization.py +596 -0
  31. archeone/_vendor/jaxls/utils.py +155 -0
  32. archeone/_vendor/pyroki/LICENSE +21 -0
  33. archeone/_vendor/pyroki/__init__.py +4 -0
  34. archeone/_vendor/pyroki/_residuals/__init__.py +32 -0
  35. archeone/_vendor/pyroki/_residuals/_pose_residual_analytic_jac.py +258 -0
  36. archeone/_vendor/pyroki/_residuals/_pose_residual_numerical_jac.py +94 -0
  37. archeone/_vendor/pyroki/_residuals/_residuals.py +350 -0
  38. archeone/_vendor/pyroki/_robot.py +178 -0
  39. archeone/_vendor/pyroki/_robot_urdf_parser.py +462 -0
  40. archeone/_vendor/pyroki/collision/__init__.py +11 -0
  41. archeone/_vendor/pyroki/collision/_collision.py +160 -0
  42. archeone/_vendor/pyroki/collision/_geometry.py +740 -0
  43. archeone/_vendor/pyroki/collision/_geometry_pairs.py +337 -0
  44. archeone/_vendor/pyroki/collision/_robot_collision.py +554 -0
  45. archeone/_vendor/pyroki/collision/_utils.py +113 -0
  46. archeone/_vendor/pyroki/costs.py +66 -0
  47. archeone/_vendor/pyroki/utils.py +28 -0
  48. archeone/_vendor/pyroki/viewer/__init__.py +3 -0
  49. archeone/_vendor/pyroki/viewer/_batched_urdf.py +175 -0
  50. archeone/_vendor/pyroki/viewer/_manipulability_ellipse.py +197 -0
  51. archeone/_vendor/pyroki/viewer/_weight_tuner.py +112 -0
  52. archeone/client.py +354 -0
  53. archeone/credentials.py +74 -0
  54. archeone/errors.py +150 -0
  55. archeone/ik/__init__.py +6 -0
  56. archeone/ik/dream_ik/__init__.py +19 -0
  57. archeone/ik/dream_ik/assets/piper_dual_left.urdf +179 -0
  58. archeone/ik/dream_ik/assets/piper_dual_right.urdf +179 -0
  59. archeone/ik/dream_ik/assets/piper_right.urdf +179 -0
  60. archeone/ik/dream_ik/assets/yam.urdf +221 -0
  61. archeone/ik/dream_ik/decode.py +194 -0
  62. archeone/ik/dream_ik/solver.py +923 -0
  63. archeone/ik/dream_ik/specs.py +655 -0
  64. archeone/ik/solver.py +528 -0
  65. archeone/observation.py +124 -0
  66. archeone/pacing.py +42 -0
  67. archeone/py.typed +1 -0
  68. archeone/seam_join.py +182 -0
  69. archeone/smoothing.py +226 -0
  70. archeone/transport.py +227 -0
  71. archeone/window.py +140 -0
  72. archeone-0.1.0.data/data/share/archeone/docs/quickstart.md +412 -0
  73. archeone-0.1.0.dist-info/METADATA +181 -0
  74. archeone-0.1.0.dist-info/RECORD +81 -0
  75. archeone-0.1.0.dist-info/WHEEL +5 -0
  76. archeone-0.1.0.dist-info/licenses/LICENSE +202 -0
  77. archeone-0.1.0.dist-info/licenses/licenses/dreamlabs-LICENSE +202 -0
  78. archeone-0.1.0.dist-info/licenses/licenses/dreamlabs-NOTICE +60 -0
  79. archeone-0.1.0.dist-info/licenses/licenses/jaxls-LICENSE +21 -0
  80. archeone-0.1.0.dist-info/licenses/licenses/pyroki-LICENSE +21 -0
  81. 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
+ )