From 49267de1814d487b37ef5020340f4eb25546f8df Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 14:52:50 -0800 Subject: [PATCH 01/40] extremely generic API for trajectory optimization --- ambersim/trajopt/base.py | 54 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 ambersim/trajopt/base.py diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py new file mode 100644 index 00000000..0a6188e2 --- /dev/null +++ b/ambersim/trajopt/base.py @@ -0,0 +1,54 @@ +from typing import Tuple + +import jax +from flax import struct + + +@struct.dataclass +class TrajectoryOptimizerParams: + """The parameters for generic trajectory optimization algorithms. + + Parameters we may want to optimize should be included here. + + This is left completely empty to allow for maximum flexibility in the API. Some examples: + - A direct collocation method might have parameters for the number of collocation points, the collocation + scheme, and the number of optimization iterations. + - A shooting method might have parameters for the number of shooting points, the shooting scheme, and the number + of optimization iterations. + The parameters also include initial iterates for each type of algorithm. Some examples: + - A direct collocation method might have initial iterates for the controls and the state trajectory. + - A shooting method might have initial iterates for the controls only. + + Parameters which we want to remain untouched by JAX transformations can be marked by pytree_node=False, e.g., + ``` + @struct.dataclass + class ChildParams: + ... + # example field + example: int = struct.field(pytree_node=False) + ... + ``` + """ + + +@struct.dataclass +class TrajectoryOptimizer: + """The API for generic trajectory optimization algorithms.""" + + def __init__(self) -> None: + """Initialize the trajopt object.""" + + @staticmethod + def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array]: + """Optimizes a trajectory. + + Args: + params: The parameters of the trajectory optimizer. + + Returns: + xs (shape=(N + 1, nq + nv)): The optimized trajectory. + us (shape=(N, nu)): The optimized controls. + """ + # abstract dataclasses are weird, so we just make all children implement this - to be useful, they need it + # anyway, so it isn't really a problem if an "abstract" TrajectoryOptimizer is instantiated. + raise NotImplementedError From 1035dd4899df3745b5b117e9cce233b4fdf3ecd0 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 14:53:22 -0800 Subject: [PATCH 02/40] fix spacing --- ambersim/trajopt/base.py | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 0a6188e2..79c678ce 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -11,23 +11,23 @@ class TrajectoryOptimizerParams: Parameters we may want to optimize should be included here. This is left completely empty to allow for maximum flexibility in the API. Some examples: - - A direct collocation method might have parameters for the number of collocation points, the collocation - scheme, and the number of optimization iterations. - - A shooting method might have parameters for the number of shooting points, the shooting scheme, and the number - of optimization iterations. + - A direct collocation method might have parameters for the number of collocation points, the collocation + scheme, and the number of optimization iterations. + - A shooting method might have parameters for the number of shooting points, the shooting scheme, and the number + of optimization iterations. The parameters also include initial iterates for each type of algorithm. Some examples: - - A direct collocation method might have initial iterates for the controls and the state trajectory. - - A shooting method might have initial iterates for the controls only. + - A direct collocation method might have initial iterates for the controls and the state trajectory. + - A shooting method might have initial iterates for the controls only. Parameters which we want to remain untouched by JAX transformations can be marked by pytree_node=False, e.g., - ``` - @struct.dataclass - class ChildParams: - ... - # example field - example: int = struct.field(pytree_node=False) - ... - ``` + ``` + @struct.dataclass + class ChildParams: + ... + # example field + example: int = struct.field(pytree_node=False) + ... + ``` """ @@ -43,11 +43,11 @@ def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array]: """Optimizes a trajectory. Args: - params: The parameters of the trajectory optimizer. + params: The parameters of the trajectory optimizer. Returns: - xs (shape=(N + 1, nq + nv)): The optimized trajectory. - us (shape=(N, nu)): The optimized controls. + xs (shape=(N + 1, nq + nv)): The optimized trajectory. + us (shape=(N, nu)): The optimized controls. """ # abstract dataclasses are weird, so we just make all children implement this - to be useful, they need it # anyway, so it isn't really a problem if an "abstract" TrajectoryOptimizer is instantiated. From 22f9867caa9131c2f83f821b5eb1680249246c62 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 14:56:36 -0800 Subject: [PATCH 03/40] remove unnecessary __init__ method (brainfart) --- ambersim/trajopt/base.py | 3 --- ambersim/trajopt/shooting.py | 14 ++++++++++++++ 2 files changed, 14 insertions(+), 3 deletions(-) create mode 100644 ambersim/trajopt/shooting.py diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 79c678ce..0b1716b5 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -35,9 +35,6 @@ class ChildParams: class TrajectoryOptimizer: """The API for generic trajectory optimization algorithms.""" - def __init__(self) -> None: - """Initialize the trajopt object.""" - @staticmethod def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array]: """Optimizes a trajectory. diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py new file mode 100644 index 00000000..d2242768 --- /dev/null +++ b/ambersim/trajopt/shooting.py @@ -0,0 +1,14 @@ +import jax +from flax import struct + +from ambersim.trajopt.base import TrajectoryOptimizer, TrajectoryOptimizerParams + + +@struct.dataclass +class ShootingParams(TrajectoryOptimizerParams): + """Parameters for shooting methods.""" + + +@struct.dataclass +class ShootingAlgorithm(TrajectoryOptimizer): + """A trajectory optimization algorithm based on shooting methods.""" From b8d7e8e2f042bdf3d5c2cace6443d8d5d65a3983 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 15:09:56 -0800 Subject: [PATCH 04/40] some additional massaging for best abstraction + useless scaffolding for shooting methods --- ambersim/trajopt/base.py | 20 ++++++++++++++++---- ambersim/trajopt/shooting.py | 17 +++++++++++++++++ 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 0b1716b5..60ca2607 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -33,18 +33,30 @@ class ChildParams: @struct.dataclass class TrajectoryOptimizer: - """The API for generic trajectory optimization algorithms.""" + """The API for generic trajectory optimization algorithms on mechanical systems. + + We choose to implement this as a flax dataclass (as opposed to a regular class whose functions operate on pytree + nodes) because: + (1) the OOP formalism allows us to define coherent abstractions through inheritance; + (2) struct.dataclass registers dataclasses a pytree nodes, so we can deal with awkward issues like the `self` + variable when using JAX transformations on methods of the dataclass. + """ @staticmethod - def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array]: + def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory. + The shapes of the outputs include (?) because we may choose to return non-zero-order-hold parameterizations of + the optimized trajectories (for example, we could choose to return a cubic spline parameterization of the + control inputs over the trajectory as is done in the gradient-based methods of MJPC). + Args: params: The parameters of the trajectory optimizer. Returns: - xs (shape=(N + 1, nq + nv)): The optimized trajectory. - us (shape=(N, nu)): The optimized controls. + qs (shape=(N + 1, nq) or (?)): The optimized trajectory. + vs (shape=(N + 1, nv) or (?)): The optimized generalized velocities. + us (shape=(N, nu) or (?)): The optimized controls. """ # abstract dataclasses are weird, so we just make all children implement this - to be useful, they need it # anyway, so it isn't really a problem if an "abstract" TrajectoryOptimizer is instantiated. diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index d2242768..24b9e881 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -1,5 +1,8 @@ +from typing import Tuple + import jax from flax import struct +from mujoco import mjx from ambersim.trajopt.base import TrajectoryOptimizer, TrajectoryOptimizerParams @@ -8,7 +11,21 @@ class ShootingParams(TrajectoryOptimizerParams): """Parameters for shooting methods.""" + model = mjx.Model + @struct.dataclass class ShootingAlgorithm(TrajectoryOptimizer): """A trajectory optimization algorithm based on shooting methods.""" + + @staticmethod + def optimize(params: ShootingParams) -> Tuple[jax.Array, jax.Array]: + """Optimizes a trajectory. + + Args: + params: The parameters of the trajectory optimizer. + + Returns: + xs (shape=(N + 1, nq + nv)): The optimized trajectory. + us (shape=(N, nu)): The optimized controls. + """ From f5610fc323d18f500dfb70ace21abe2cf4e87c76 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 16:12:26 -0800 Subject: [PATCH 05/40] add some informative comments to base.py --- ambersim/trajopt/base.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 60ca2607..d5043ffd 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -40,10 +40,18 @@ class TrajectoryOptimizer: (1) the OOP formalism allows us to define coherent abstractions through inheritance; (2) struct.dataclass registers dataclasses a pytree nodes, so we can deal with awkward issues like the `self` variable when using JAX transformations on methods of the dataclass. + + Further, we choose not to specify the mjx.Model as either a field of this dataclass or as a parameter. The reason is + because we want to allow for maximum flexibility in the API. Two motivating scenarios: + (1) we want to domain randomize over the model parameters and potentially optimize for them. In this case, it makes + sense to specify the mjx.Model as a parameter that gets passed as an input into the optimize function. + (2) we want to fix the model and only randomize/optimize over non-model-parameters. For instance, this is the + situation in vanilla predictive sampling. If we don't need to pass the model, we instead initialize it as a + field of this dataclass, which makes the optimize function more performant, since it can just reference the + fixed model attribute of the optimizer instead of applying JAX transformations to the entire large model pytree. """ - @staticmethod - def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: + def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory. The shapes of the outputs include (?) because we may choose to return non-zero-order-hold parameterizations of @@ -54,9 +62,9 @@ def optimize(params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, j params: The parameters of the trajectory optimizer. Returns: - qs (shape=(N + 1, nq) or (?)): The optimized trajectory. - vs (shape=(N + 1, nv) or (?)): The optimized generalized velocities. - us (shape=(N, nu) or (?)): The optimized controls. + qs_star (shape=(N + 1, nq) or (?)): The optimized trajectory. + vs_star (shape=(N + 1, nv) or (?)): The optimized generalized velocities. + us_star (shape=(N, nu) or (?)): The optimized controls. """ # abstract dataclasses are weird, so we just make all children implement this - to be useful, they need it # anyway, so it isn't really a problem if an "abstract" TrajectoryOptimizer is instantiated. From 17642f6883685179ffccf179a7ee5dbdfa98f5a7 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 17:09:23 -0800 Subject: [PATCH 06/40] [maybe] added cost function to base API --- ambersim/trajopt/base.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index d5043ffd..c4c69ea7 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -49,8 +49,24 @@ class TrajectoryOptimizer: situation in vanilla predictive sampling. If we don't need to pass the model, we instead initialize it as a field of this dataclass, which makes the optimize function more performant, since it can just reference the fixed model attribute of the optimizer instead of applying JAX transformations to the entire large model pytree. + + Finally, abstract dataclasses are weird, so we just make all children implement the below functions by instead + raising a NotImplementedError. """ + def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: + """Computes the cost of a trajectory. + + Args: + qs: The generalized positions over the trajectory. + vs: The generalized velocities over the trajectory. + us: The controls over the trajectory. + + Returns: + The cost of the trajectory. + """ + raise NotImplementedError + def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory. @@ -66,6 +82,4 @@ def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Ar vs_star (shape=(N + 1, nv) or (?)): The optimized generalized velocities. us_star (shape=(N, nu) or (?)): The optimized controls. """ - # abstract dataclasses are weird, so we just make all children implement this - to be useful, they need it - # anyway, so it isn't really a problem if an "abstract" TrajectoryOptimizer is instantiated. raise NotImplementedError From 4e1ae77a24195f18db01432190ff252e2613f7f4 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sat, 2 Dec 2023 17:10:21 -0800 Subject: [PATCH 07/40] shooting method APIs, no cost function implemented yet --- ambersim/trajopt/shooting.py | 159 +++++++++++++++++++++++++++++++++-- 1 file changed, 154 insertions(+), 5 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 24b9e881..14a7825d 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -1,31 +1,180 @@ from typing import Tuple import jax +import jax.numpy as jnp from flax import struct +from jax import lax, vmap from mujoco import mjx +from mujoco.mjx import step from ambersim.trajopt.base import TrajectoryOptimizer, TrajectoryOptimizerParams +"""Shooting methods and their derived subclasses.""" + + +# ##### # +# UTILS # +# ##### # + + +def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: + """Utility function that shoots a model forward given a sequence of control inputs. + + Args: + m: The model. + q0: The initial generalized coordinates. + v0: The initial generalized velocities. + us: The control inputs. + + Returns: + qs (shape=(N + 1, nq)): The generalized coordinates. + vs (shape=(N + 1, nv)): The generalized velocities. + """ + d = mjx.make_data(m) + d = d.replace(qpos=q0, qvel=v0) # setting the initial state. + + def scan_fn(d, u): + """Integrates the model forward one step given the control input u.""" + d = d.replace(ctrl=u) + d = step(m, d) + x = jnp.concatenate((d.qpos, d.qvel)) # (nq + nv,) + return d, x + + # scan over the control inputs to get the trajectory. + _, xs = lax.scan(scan_fn, d, us, length=us.shape[0]) + _qs = xs[:, : m.nq] + _vs = xs[:, m.nq : m.nq + m.nv] + qs = jnp.concatenate((q0[None, :], _qs), axis=0) # (N + 1, nq) + vs = jnp.concatenate((v0[None, :], _vs), axis=0) # (N + 1, nv) + return qs, vs + + +# ################ # +# SHOOTING METHODS # +# ################ # + +# vanilla API + @struct.dataclass class ShootingParams(TrajectoryOptimizerParams): """Parameters for shooting methods.""" - model = mjx.Model + # inputs into the algorithm + us_guess: jax.Array # shape=(N, nu) or (?) + + @property + def N(self) -> int: + """The number of time steps. + + By default, we assume us_guess represents the ZOH control inputs. However, in the case that it is actually an + alternate parameterization, we may need to compute N some other way, which requires overwriting this method. + """ + return self.us_guess.shape[0] @struct.dataclass class ShootingAlgorithm(TrajectoryOptimizer): """A trajectory optimization algorithm based on shooting methods.""" - @staticmethod - def optimize(params: ShootingParams) -> Tuple[jax.Array, jax.Array]: - """Optimizes a trajectory. + def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: + """Computes the cost of a trajectory. + + Args: + qs: The generalized positions over the trajectory. + vs: The generalized velocities over the trajectory. + us: The controls over the trajectory. + + Returns: + The cost of the trajectory. + """ + raise NotImplementedError + + def optimize(self, params: ShootingParams) -> Tuple[jax.Array, jax.Array, jax.Array]: + """Optimizes a trajectory using a shooting method. + + Args: + params: The parameters of the trajectory optimizer. + + Returns: + qs (shape=(N + 1, nq) or (?)): The optimized trajectory. + vs (shape=(N + 1, nv) or (?)): The optimized generalized velocities. + us (shape=(N, nu) or (?)): The optimized controls. + """ + raise NotImplementedError + + +# predictive sampling API + + +@struct.dataclass +class VanillaPredictiveSamplerParams(ShootingParams): + """Parameters for generic predictive sampling methods.""" + + key: jax.Array # random key for sampling + + +@struct.dataclass +class VanillaPredictiveSampler(ShootingAlgorithm): + """A vanilla predictive sampler object. + + The following choices are made: + (1) the model parameters are fixed, and are therefore a field of this dataclass; + (2) the control sequence is parameterized as a ZOH sequence instead of a spline; + (3) the control inputs are sampled from a normal distribution with a uniformly chosen noise scale over all params. + (4) the cost function is quadratic in the states and controls. + """ + + model: mjx.Model = struct.field(pytree_node=False) + nsamples: int = struct.field(pytree_node=False) + stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) + + def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: + """Computes the cost of a trajectory using quadratic weights.""" + raise NotImplementedError # TODO(ahl): implement this, maybe make a library of parametric cost functions + + def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: + """Optimizes a trajectory using a vanilla predictive sampler. Args: params: The parameters of the trajectory optimizer. Returns: - xs (shape=(N + 1, nq + nv)): The optimized trajectory. + qs (shape=(N + 1, nq)): The optimized trajectory. + vs (shape=(N + 1, nv)): The optimized generalized velocities. us (shape=(N, nu)): The optimized controls. """ + # unpack the params + m = self.model + nsamples = self.nsamples + + q0 = params.q0 + v0 = params.v0 + us_guess = params.us_guess + N = params.N + key = params.key + + # sample over the control inputs + # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- + # reasonable default value + keys = jax.random.split(key, nsamples) # splitting the random key so we can vmap over random sampling + _us_samples = vmap(jax.random.normal, in_axes=(0, None))(keys, shape=(N, m.nu)) + us_guess # (nsamples, N, nu) + + # clamping the samples to their control limits + # TODO(ahl): check whether joints with no limits have reasonable defaults for m.actuator_ctrlrange + limits = m.actuator_ctrlrange + us_samples = vmap(vmap(jnp.clip, in_axes=(0, None, None)), in_axes=(0, None, None))( + _us_samples, a_min=limits[:, 0], a_max=limits[:, 1] + ) # apply limits only to the last dim, need a nested vmap + # limited = m.actuator_ctrllimited[:, None] # (nu, 1) whether each actuator has limited control authority + # default_limits = jnp.array([[-1000.0, 1000.0]] * m.nu) # (nu, 2) default limits for each actuator + # limits = jnp.where(limited, m.actuator_ctrlrange, default_limits) # (nu, 2) + + # predict many samples, evaluate them, and return the best trajectory tuple + qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) + costs = vmap(self.cost)(qs_samples, vs_samples, us_samples) # (nsamples,) + best_idx = jnp.argmin(costs) + qs_star = lax.dynamic_slice(qs_samples, (best_idx, 0, 0), (1, N + 1, m.nq))[0] # (N + 1, nq) + vs_star = lax.dynamic_slice(vs_samples, (best_idx, 0, 0), (1, N + 1, m.nv))[0] # (N + 1, nv) + us_star = lax.dynamic_slice(us_samples, (best_idx, 0, 0), (1, N, m.nu))[0] # (N, nu) + return qs_star, vs_star, us_star From 8c9cd6b676bb3f1117c64c3c1e0b9cdc1198e0c0 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 15:21:28 -0800 Subject: [PATCH 08/40] define generic CostFunction API --- ambersim/trajopt/base.py | 108 ++++++++++++++++++++++++++++++++++----- 1 file changed, 95 insertions(+), 13 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index c4c69ea7..385405ca 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -2,6 +2,11 @@ import jax from flax import struct +from jax import grad, hessian + +# ####################### # +# TRAJECTORY OPTIMIZATION # +# ####################### # @struct.dataclass @@ -49,24 +54,13 @@ class TrajectoryOptimizer: situation in vanilla predictive sampling. If we don't need to pass the model, we instead initialize it as a field of this dataclass, which makes the optimize function more performant, since it can just reference the fixed model attribute of the optimizer instead of applying JAX transformations to the entire large model pytree. + Similar logic applies for not specifying the role of the CostFunction - we trust that the user will either use the + provided API or will ignore it and still end up implementing something custom and reasonable. Finally, abstract dataclasses are weird, so we just make all children implement the below functions by instead raising a NotImplementedError. """ - def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: - """Computes the cost of a trajectory. - - Args: - qs: The generalized positions over the trajectory. - vs: The generalized velocities over the trajectory. - us: The controls over the trajectory. - - Returns: - The cost of the trajectory. - """ - raise NotImplementedError - def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory. @@ -83,3 +77,91 @@ def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Ar us_star (shape=(N, nu) or (?)): The optimized controls. """ raise NotImplementedError + + +# ############# # +# COST FUNCTION # +# ############# # + + +@struct.dataclass +class CostFunctionParams: + """Generic parameters for cost functions.""" + + +@struct.dataclass +class CostFunction: + """The API for generic cost functions for trajectory optimization problems for mechanical systems. + + Rationale behind CostFunctionParams in this generic API: + (1) computation of higher-order derivatives could depend on results or intermediates from lower-order derivatives. + So, we can flexibly cache the requisite values to avoid repeated computation; + (2) we may want to randomize or optimize the cost function parameters themselves, so specifying a generic pytree + as input generically accounts for all possibilities; + (3) there could simply be parameters that cannot be easily specified in advance that are key for cost evaluation, + like a time-varying reference trajectory that gets updated in real time. + (4) histories of higher-order derivatives can be useful for updating their current estimates, e.g., BFGS. + """ + + def cost( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, CostFunctionParams]: + """Computes the cost of a trajectory. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + val (shape=(,)): The cost of the trajectory. + new_params: The updated parameters of the cost function. + """ + raise NotImplementedError + + def grad( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + """Computes the gradient of the cost of a trajectory. + + The default implementation of this function uses JAX's autodiff. Simply override this function if you would like + to supply an analytical gradient. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. + gcost_vs (shape=(N + 1, nv): The gradient of the cost wrt vs. + gcost_us (shape=(N, nu)): The gradient of the cost wrt us. + gcost_params: The gradient of the cost wrt params. + new_params: The updated parameters of the cost function. + """ + return grad(self.cost, argnums=(0, 1, 2, 3))(qs, vs, us, params) + (params,) + + def hess( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + """Computes the Hessian of the cost of a trajectory. + + The default implementation of this function uses JAX's autodiff. Simply override this function if you would like + to supply an analytical Hessian. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. + Let t, s be times from 0 to N + 1. Then, d^2/dq_{t,i}dq_{s,j} = Hcost_qs[t, i, s, j]. + Hcost_vs (shape=(N + 1, nv, N + 1, nv)): The Hessian of the cost wrt vs. + Let t, s be times from 0 to N + 1. Then, d^2/dv_{t,i}dv_{s,j} = Hcost_vs[t, i, s, j]. + Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. + Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. + Hcost_params: The Hessian of the cost wrt params. + new_params: The updated parameters of the cost function. + """ + return hessian(self.cost, argnums=(0, 1, 2, 3))(qs, vs, us, params) + (params,) From 0d175f022ee0bdc7aee31ed6e616313b6a6fd4c3 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 15:22:25 -0800 Subject: [PATCH 09/40] expose a CostFunction field of shooting methods --- ambersim/trajopt/shooting.py | 23 ++++------------------- 1 file changed, 4 insertions(+), 19 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 14a7825d..21d8db2d 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -8,6 +8,7 @@ from mujoco.mjx import step from ambersim.trajopt.base import TrajectoryOptimizer, TrajectoryOptimizerParams +from ambersim.trajopt.cost import CostFunction """Shooting methods and their derived subclasses.""" @@ -77,19 +78,6 @@ def N(self) -> int: class ShootingAlgorithm(TrajectoryOptimizer): """A trajectory optimization algorithm based on shooting methods.""" - def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: - """Computes the cost of a trajectory. - - Args: - qs: The generalized positions over the trajectory. - vs: The generalized velocities over the trajectory. - us: The controls over the trajectory. - - Returns: - The cost of the trajectory. - """ - raise NotImplementedError - def optimize(self, params: ShootingParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory using a shooting method. @@ -125,14 +113,11 @@ class VanillaPredictiveSampler(ShootingAlgorithm): (4) the cost function is quadratic in the states and controls. """ - model: mjx.Model = struct.field(pytree_node=False) + model: mjx.Model + cost: CostFunction nsamples: int = struct.field(pytree_node=False) stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) - def cost(self, qs: jax.Array, vs: jax.Array, us: jax.Array) -> jax.Array: - """Computes the cost of a trajectory using quadratic weights.""" - raise NotImplementedError # TODO(ahl): implement this, maybe make a library of parametric cost functions - def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory using a vanilla predictive sampler. @@ -172,7 +157,7 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # predict many samples, evaluate them, and return the best trajectory tuple qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) - costs = vmap(self.cost)(qs_samples, vs_samples, us_samples) # (nsamples,) + costs, _ = vmap(self.cost, in_axes=(0, 0, 0, None))(qs_samples, vs_samples, us_samples, None) # (nsamples,) best_idx = jnp.argmin(costs) qs_star = lax.dynamic_slice(qs_samples, (best_idx, 0, 0), (1, N + 1, m.nq))[0] # (N + 1, nq) vs_star = lax.dynamic_slice(vs_samples, (best_idx, 0, 0), (1, N + 1, m.nv))[0] # (N + 1, nv) From 987e3348372f49dbd639f0c2e8a38e9ac5e61f64 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 15:26:52 -0800 Subject: [PATCH 10/40] add specific implementation of (non-sparse) quadratic CostFunction with static goal and cost matrices --- ambersim/trajopt/cost.py | 170 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 170 insertions(+) create mode 100644 ambersim/trajopt/cost.py diff --git a/ambersim/trajopt/cost.py b/ambersim/trajopt/cost.py new file mode 100644 index 00000000..b09d1cb7 --- /dev/null +++ b/ambersim/trajopt/cost.py @@ -0,0 +1,170 @@ +from typing import Tuple + +import jax +import jax.numpy as jnp +from flax import struct + +from ambersim.trajopt.base import CostFunction, CostFunctionParams + +"""A collection of common cost functions.""" + + +class StaticGoalQuadraticCost(CostFunction): + """A quadratic cost function that penalizes the distance to a static goal. + + This is the most vanilla possible quadratic cost. The cost matrices are static (defined at init time) and so is the + single, fixed goal. The gradient is as compressed as it can be in general (one matrix multiplication), but the + Hessian can be far more compressed by simplying referencing Q, Qf, and R - this implementation is inefficient and + dense. + """ + + def __init__(self, Q: jax.Array, Qf: jax.Array, R: jax.Array, qg: jax.Array, vg: jax.Array) -> None: + """Initializes a quadratic cost function. + + Args: + Q (shape=(nx, nx)): The state cost matrix. + Qf (shape=(nx, nx)): The final state cost matrix. + R (shape=(nu, nu)): The control cost matrix. + qg (shape=(nq,)): The goal generalized coordinates. + vg (shape=(nv,)): The goal generalized velocities. + """ + self.Q = Q + self.Qf = Qf + self.R = R + self.qg = qg + self.vg = vg + + @staticmethod + def _setup_util( + qs: jax.Array, vs: jax.Array, qg: jax.Array, vg: jax.Array + ) -> Tuple[jax.Array, jax.Array, jax.Array, jax.Array, int, int, int]: + """Utility function that sets up the cost function. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + qg (shape=(nx,)): The goal generalized position. + vg (shape=(nv,)): The goal generalized velocity. + + Returns: + xs (shape=(N + 1, nx)): The states over the trajectory. + xg (shape=(nx,)): The goal state. + xs_err (shape=(N, nx)): The state errors up to the final state. + xf_err (shape=(nx,)): The state error at the final state. + nq: The number of generalized coordinates. + nv: The number of generalized velocities. + """ + xs = jnp.concatenate((qs, vs), axis=-1) + xg = jnp.concatenate((qg, vg), axis=-1) + xs_err = xs[:-1, :] - xg + xf_err = xs[-1, :] - xg + return xs, xg, xs_err, xf_err, qs.shape[-1], vs.shape[-1] + + @staticmethod + def batch_quadform(bs: jax.Array, A: jax.array) -> jax.Array: + """Computes a batched quadratic form for a single instance of A. + + Args: + bs (shape=(..., n)): The batch of vectors. + A (shape=(n, n)): The matrix. + + Returns: + val (shape=(...,)): The batch of quadratic forms. + """ + return jnp.einsum("...i,ij,...j->...", bs, A, bs) + + @staticmethod + def batch_matmul(bs: jax.Array, A: jax.Array) -> jax.Array: + """Computes a batched matrix multiplication for a single instance of A. + + Args: + bs (shape=(..., n)): The batch of vectors. + A (shape=(n, n)): The matrix. + + Returns: + val (shape=(..., n)): The batch of matrix multiplications. + """ + return jnp.einsum("...i,ij->...j", bs, A) + + def cost( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, CostFunctionParams]: + """Computes the cost of a trajectory. + + cost = 0.5 * ([q; v] - [qg; vg])' @ Q @ ([q; v] - [qg; vg]) + 0.5 * u' @ R @ u + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + cost_val: The cost of the trajectory. + new_params: Unused. Included for API compliance. + """ + xs, xg, xs_err, xf_err, _, _ = self._setup_util(qs, vs, self.qg, self.vg) + val = 0.5 * ( + self.batch_quadform(xs_err, self.Q) + self.batch_quadform(xf_err, self.Qf) + self.batch_quadform(us, self.R) + ) + return val, params + + def grad( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + """Computes the gradient of the cost of a trajectory. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. + gcost_vs (shape=(N + 1, nv): The gradient of the cost wrt vs. + gcost_us (shape=(N, nu)): The gradient of the cost wrt us. + gcost_params: Unused. Included for API compliance. + new_params: Unused. Included for API compliance. + """ + xs, xg, xs_err, xf_err, nq, _, _ = self._setup_util(qs, vs, self.qg, self.vg) + gcost_xs = jnp.concatenate( + ( + self.batch_matmul(xs_err, self.Q), + (self.Qf @ xf_err)[None, :], + ), + axis=-1, + ) + gcost_qs = gcost_xs[:, :nq] + gcost_vs = gcost_xs[:, nq:] + gcost_us = self.batch_matmul(us, self.R) + return gcost_qs, gcost_vs, gcost_us, params, params + + def hess( + self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + """Computes the Hessian of the cost of a trajectory. + + Args: + qs (shape=(N + 1, nq)): The generalized positions over the trajectory. + vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + us (shape=(N, nu)): The controls over the trajectory. + + Returns: + Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. + Let t, s be times from 0 to N + 1. Then, d^2/dq_{t,i}dq_{s,j} = Hcost_qs[t, i, s, j]. + Hcost_vs (shape=(N + 1, nv, N + 1, nv)): The Hessian of the cost wrt vs. + Let t, s be times from 0 to N + 1. Then, d^2/dv_{t,i}dv_{s,j} = Hcost_vs[t, i, s, j]. + Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. + Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. + Hcost_params: Unused. Included for API compliance. + new_params: Unused. Included for API compliance. + """ + N = us.shape[0] + xs, xg, xs_err, xf_err, nq, _, _ = self._setup_util(qs, vs, self.qg, self.vg) + Q_tiled = jnp.tile(self.Q[None, :, None, :], (N + 1, 1, N + 1, 1)) + Hcost_xs = Q_tiled.at[-1, :, -1, :].set(self.Qf) + + Hcost_qs = Hcost_xs[:, :nq, :, :nq] + Hcost_vs = Hcost_xs[:, nq:, :, nq:] + Hcost_us = jnp.tile(self.R[None, :, None, :], (N, 1, N, 1)) + + return Hcost_qs, Hcost_vs, Hcost_us, params, params From 2b268f8a0286b106cdb80199d8058aa6d8911b98 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 22:42:17 -0800 Subject: [PATCH 11/40] fixed missing field in docstrings --- ambersim/trajopt/base.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 385405ca..8d705144 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -112,6 +112,7 @@ def cost( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: The parameters of the cost function. Returns: val (shape=(,)): The cost of the trajectory. @@ -131,6 +132,7 @@ def grad( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: The parameters of the cost function. Returns: gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. @@ -153,6 +155,7 @@ def hess( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: The parameters of the cost function. Returns: Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. From 13efcfd4c45cd7fac2935a0df4f94d04c6bf8658 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 22:42:32 -0800 Subject: [PATCH 12/40] fixed missing field in docstrings --- ambersim/trajopt/cost.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ambersim/trajopt/cost.py b/ambersim/trajopt/cost.py index b09d1cb7..454d74e6 100644 --- a/ambersim/trajopt/cost.py +++ b/ambersim/trajopt/cost.py @@ -61,7 +61,7 @@ def _setup_util( return xs, xg, xs_err, xf_err, qs.shape[-1], vs.shape[-1] @staticmethod - def batch_quadform(bs: jax.Array, A: jax.array) -> jax.Array: + def batch_quadform(bs: jax.Array, A: jax.Array) -> jax.Array: """Computes a batched quadratic form for a single instance of A. Args: @@ -97,6 +97,7 @@ def cost( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: Unused. Included for API compliance. Returns: cost_val: The cost of the trajectory. @@ -117,6 +118,7 @@ def grad( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: Unused. Included for API compliance. Returns: gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. @@ -147,6 +149,7 @@ def hess( qs (shape=(N + 1, nq)): The generalized positions over the trajectory. vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. us (shape=(N, nu)): The controls over the trajectory. + params: Unused. Included for API compliance. Returns: Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. From b6ea473af53844750bbb23d15162afb75f0f334e Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 22:43:15 -0800 Subject: [PATCH 13/40] pass non-kwarg functions to vmap since vmap breaks in that case --- ambersim/trajopt/shooting.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 21d8db2d..b333ca57 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -1,3 +1,4 @@ +from functools import partial from typing import Tuple import jax @@ -62,6 +63,8 @@ class ShootingParams(TrajectoryOptimizerParams): """Parameters for shooting methods.""" # inputs into the algorithm + q0: jax.Array # shape=(nq,) or (?) + v0: jax.Array # shape=(nv,) or (?) us_guess: jax.Array # shape=(N, nu) or (?) @property @@ -114,7 +117,7 @@ class VanillaPredictiveSampler(ShootingAlgorithm): """ model: mjx.Model - cost: CostFunction + cost_function: CostFunction nsamples: int = struct.field(pytree_node=False) stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) @@ -143,21 +146,23 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- # reasonable default value keys = jax.random.split(key, nsamples) # splitting the random key so we can vmap over random sampling - _us_samples = vmap(jax.random.normal, in_axes=(0, None))(keys, shape=(N, m.nu)) + us_guess # (nsamples, N, nu) + jrn = partial(jax.random.normal, shape=(N, m.nu)) # jax random normal function with shape already set + _us_samples = vmap(jrn)(keys) + us_guess # (nsamples, N, nu) # clamping the samples to their control limits # TODO(ahl): check whether joints with no limits have reasonable defaults for m.actuator_ctrlrange limits = m.actuator_ctrlrange - us_samples = vmap(vmap(jnp.clip, in_axes=(0, None, None)), in_axes=(0, None, None))( - _us_samples, a_min=limits[:, 0], a_max=limits[:, 1] - ) # apply limits only to the last dim, need a nested vmap + clip_fn = partial(jnp.clip, a_min=limits[:, 0], a_max=limits[:, 1]) # clipping function with limits already set + us_samples = vmap(vmap(clip_fn))(_us_samples) # apply limits only to the last dim, need a nested vmap # limited = m.actuator_ctrllimited[:, None] # (nu, 1) whether each actuator has limited control authority # default_limits = jnp.array([[-1000.0, 1000.0]] * m.nu) # (nu, 2) default limits for each actuator # limits = jnp.where(limited, m.actuator_ctrlrange, default_limits) # (nu, 2) # predict many samples, evaluate them, and return the best trajectory tuple qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) - costs, _ = vmap(self.cost, in_axes=(0, 0, 0, None))(qs_samples, vs_samples, us_samples, None) # (nsamples,) + costs, _ = vmap(self.cost_function.cost, in_axes=(0, 0, 0, None))( + qs_samples, vs_samples, us_samples, None + ) # (nsamples,) best_idx = jnp.argmin(costs) qs_star = lax.dynamic_slice(qs_samples, (best_idx, 0, 0), (1, N + 1, m.nq))[0] # (N + 1, nq) vs_star = lax.dynamic_slice(vs_samples, (best_idx, 0, 0), (1, N + 1, m.nv))[0] # (N + 1, nv) From 6bf291d2e97de56965b30a67161b8fcdfd11f15a Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 22:43:30 -0800 Subject: [PATCH 14/40] added preliminary dead simple predictive sampling example --- examples/trajopt/ex_predictive_sampling.py | 72 ++++++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 examples/trajopt/ex_predictive_sampling.py diff --git a/examples/trajopt/ex_predictive_sampling.py b/examples/trajopt/ex_predictive_sampling.py new file mode 100644 index 00000000..895a81a0 --- /dev/null +++ b/examples/trajopt/ex_predictive_sampling.py @@ -0,0 +1,72 @@ +import timeit + +import jax +import jax.numpy as jnp +from jax import jit +from mujoco.mjx._src.types import DisableBit + +from ambersim.trajopt.cost import StaticGoalQuadraticCost +from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams +from ambersim.utils.io_utils import load_mjx_model_and_data_from_file + +if __name__ == "__main__": + # initializing the predictive sampler + model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) + model = model.replace( + opt=model.opt.replace( + timestep=0.002, # dt + iterations=1, # number of Newton steps to take during solve + ls_iterations=4, # number of line search iterations along step direction + integrator=1, # RK4 instead of Euler semi-implicit + solver=2, # Newton solver + disableflags=DisableBit.CONTACT, # [IMPORTANT] disable contact for this example + ) + ) + cost_function = StaticGoalQuadraticCost( + Q=jnp.eye(model.nq + model.nv), + Qf=10.0 * jnp.eye(model.nq + model.nv), + R=0.01 * jnp.eye(model.nu), + # qg=jnp.zeros(model.nq).at[6].set(1.0), # if force_float=True + qg=jnp.zeros(model.nq), + vg=jnp.zeros(model.nv), + ) + nsamples = 100 + stdev = 0.01 + ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) + + # sampler parameters + key = jax.random.PRNGKey(0) # random seed for the predictive sampler + q0 = jnp.zeros(model.nq).at[6].set(1.0) + v0 = jnp.zeros(model.nv) + num_steps = 100 + us_guess = jnp.zeros((num_steps, model.nu)) + params = VanillaPredictiveSamplerParams(key=key, q0=q0, v0=v0, us_guess=us_guess) + + # sampling the best sequence of qs, vs, and us + optimize_fn = jit(ps.optimize) + + def _time_fn(): + qs_star, vs_star, us_star = optimize_fn(params) + qs_star.block_until_ready() + vs_star.block_until_ready() + us_star.block_until_ready() + + compile_time = timeit.timeit(_time_fn, number=1) + print(f"Compile time: {compile_time}") + + # informal timing test + # TODO(ahl): identify bottlenecks and zap them + # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number + # of samples. Here are a few preliminary results: + # * nsamples=100, numsteps=10. avg: 0.01s + # * nsamples=1000, numsteps=10. avg: 0.015s + # * nsamples=10000, numsteps=10. avg: 0.07s + # * nsamples=100, numsteps=100. avg: 0.1s + # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps + # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind + # that we've completely disabled contact for this example and set the number of solver iterations and line search + # iterations to very runtime-friendly values + num_timing_iters = 100 + time = timeit.timeit(_time_fn, number=num_timing_iters) + print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves + breakpoint() From 2cfdbfd579db871f4cdd49c1f5897a8205c94765 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 22:57:47 -0800 Subject: [PATCH 15/40] minor, get from property --- ambersim/trajopt/shooting.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index b333ca57..6029a784 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -135,11 +135,11 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # unpack the params m = self.model nsamples = self.nsamples + N = self.N q0 = params.q0 v0 = params.v0 us_guess = params.us_guess - N = params.N key = params.key # sample over the control inputs From dc5d247b6421fc356057d8fd5c1e39bca4b2baa2 Mon Sep 17 00:00:00 2001 From: alberthli Date: Sun, 3 Dec 2023 23:03:20 -0800 Subject: [PATCH 16/40] fixed some params + simplify sampling of us to one line --- ambersim/trajopt/shooting.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 6029a784..98a95b3c 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -135,21 +135,20 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # unpack the params m = self.model nsamples = self.nsamples - N = self.N + stdev = self.stdev q0 = params.q0 v0 = params.v0 us_guess = params.us_guess + N = params.N key = params.key # sample over the control inputs - # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- - # reasonable default value - keys = jax.random.split(key, nsamples) # splitting the random key so we can vmap over random sampling - jrn = partial(jax.random.normal, shape=(N, m.nu)) # jax random normal function with shape already set - _us_samples = vmap(jrn)(keys) + us_guess # (nsamples, N, nu) + _us_samples = us_guess + jax.random.normal(key, shape=(nsamples, N, m.nu)) * stdev # clamping the samples to their control limits + # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- + # reasonable default value # TODO(ahl): check whether joints with no limits have reasonable defaults for m.actuator_ctrlrange limits = m.actuator_ctrlrange clip_fn = partial(jnp.clip, a_min=limits[:, 0], a_max=limits[:, 1]) # clipping function with limits already set From ff6dbccad4d475b98ff3c6e7101254b6012c2889 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 10:41:16 -0800 Subject: [PATCH 17/40] [DIRTY] commit that contains commented out code for pre-allocating data in case we need it later --- ambersim/trajopt/shooting.py | 38 +++++++++++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 98a95b3c..2a12b840 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -19,11 +19,13 @@ # ##### # +# def shoot(m: mjx.Model, d: mjx.Data, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: """Utility function that shoots a model forward given a sequence of control inputs. Args: m: The model. + # d: The data. q0: The initial generalized coordinates. v0: The initial generalized velocities. us: The control inputs. @@ -33,7 +35,8 @@ def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[ja vs (shape=(N + 1, nv)): The generalized velocities. """ d = mjx.make_data(m) - d = d.replace(qpos=q0, qvel=v0) # setting the initial state. + # d = d.replace(qpos=q0, qvel=v0) # setting the initial state. + # d = mjx.forward(m, d) # setting other internal states like acceleration without integrating def scan_fn(d, u): """Integrates the model forward one step given the control input u.""" @@ -117,10 +120,40 @@ class VanillaPredictiveSampler(ShootingAlgorithm): """ model: mjx.Model + # data: mjx.Data cost_function: CostFunction nsamples: int = struct.field(pytree_node=False) stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) + # @classmethod + # def create( + # cls, + # model: mjx.Model, + # cost_function: CostFunction, + # nsamples: int, + # stdev: float, + # ) -> "VanillaPredictiveSampler": + # """Creates a vanilla predictive sampler from the given parameters. + + # You should always initialize the sampler using this class method. + # """ + # # initializing a batched mjx.Data object + # def _init_data(model: mjx.Model) -> mjx.Data: + # data = mjx.make_data(model) + # data = mjx.forward(model, data) + # return data + + # _dummy_fn = lambda _dummy: _init_data(model) # dummy fn to allow vmapping over func with no inputs + # data = vmap(_dummy_fn)(jnp.empty(nsamples)) # (nsamples,) + + # return cls( + # model=model, + # data=data, + # cost_function=cost_function, + # nsamples=nsamples, + # stdev=stdev, + # ) + def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory using a vanilla predictive sampler. @@ -134,6 +167,7 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j """ # unpack the params m = self.model + # d = self.data nsamples = self.nsamples stdev = self.stdev @@ -158,6 +192,8 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # limits = jnp.where(limited, m.actuator_ctrlrange, default_limits) # (nu, 2) # predict many samples, evaluate them, and return the best trajectory tuple + # vmap over the input data and the control trajectories + # qs_samples, vs_samples = vmap(shoot, in_axes=(None, 0, None, None, 0))(m, d, q0, v0, us_samples) qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) costs, _ = vmap(self.cost_function.cost, in_axes=(0, 0, 0, None))( qs_samples, vs_samples, us_samples, None From 9f12d740c3e6ed18e0a82acba18a12ee162cd2d7 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 10:46:08 -0800 Subject: [PATCH 18/40] revert to allocating data at shooting time --- ambersim/trajopt/shooting.py | 39 +++--------------------------------- 1 file changed, 3 insertions(+), 36 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 2a12b840..60a80606 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -19,13 +19,11 @@ # ##### # -# def shoot(m: mjx.Model, d: mjx.Data, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: """Utility function that shoots a model forward given a sequence of control inputs. Args: m: The model. - # d: The data. q0: The initial generalized coordinates. v0: The initial generalized velocities. us: The control inputs. @@ -34,9 +32,10 @@ def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[ja qs (shape=(N + 1, nq)): The generalized coordinates. vs (shape=(N + 1, nv)): The generalized velocities. """ + # initializing the data d = mjx.make_data(m) - # d = d.replace(qpos=q0, qvel=v0) # setting the initial state. - # d = mjx.forward(m, d) # setting other internal states like acceleration without integrating + d = d.replace(qpos=q0, qvel=v0) # setting the initial state. + d = mjx.forward(m, d) # setting other internal states like acceleration without integrating def scan_fn(d, u): """Integrates the model forward one step given the control input u.""" @@ -120,40 +119,10 @@ class VanillaPredictiveSampler(ShootingAlgorithm): """ model: mjx.Model - # data: mjx.Data cost_function: CostFunction nsamples: int = struct.field(pytree_node=False) stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) - # @classmethod - # def create( - # cls, - # model: mjx.Model, - # cost_function: CostFunction, - # nsamples: int, - # stdev: float, - # ) -> "VanillaPredictiveSampler": - # """Creates a vanilla predictive sampler from the given parameters. - - # You should always initialize the sampler using this class method. - # """ - # # initializing a batched mjx.Data object - # def _init_data(model: mjx.Model) -> mjx.Data: - # data = mjx.make_data(model) - # data = mjx.forward(model, data) - # return data - - # _dummy_fn = lambda _dummy: _init_data(model) # dummy fn to allow vmapping over func with no inputs - # data = vmap(_dummy_fn)(jnp.empty(nsamples)) # (nsamples,) - - # return cls( - # model=model, - # data=data, - # cost_function=cost_function, - # nsamples=nsamples, - # stdev=stdev, - # ) - def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: """Optimizes a trajectory using a vanilla predictive sampler. @@ -167,7 +136,6 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j """ # unpack the params m = self.model - # d = self.data nsamples = self.nsamples stdev = self.stdev @@ -193,7 +161,6 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # predict many samples, evaluate them, and return the best trajectory tuple # vmap over the input data and the control trajectories - # qs_samples, vs_samples = vmap(shoot, in_axes=(None, 0, None, None, 0))(m, d, q0, v0, us_samples) qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) costs, _ = vmap(self.cost_function.cost, in_axes=(0, 0, 0, None))( qs_samples, vs_samples, us_samples, None From 3fda8bad7d3de74789ff0e8b9055ef1082295c57 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 13:18:52 -0800 Subject: [PATCH 19/40] [DIRTY] some additions to the example script for profiling help --- examples/trajopt/ex_predictive_sampling.py | 59 ++++++++++++---------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/examples/trajopt/ex_predictive_sampling.py b/examples/trajopt/ex_predictive_sampling.py index 895a81a0..73cca5c8 100644 --- a/examples/trajopt/ex_predictive_sampling.py +++ b/examples/trajopt/ex_predictive_sampling.py @@ -1,5 +1,3 @@ -import timeit - import jax import jax.numpy as jnp from jax import jit @@ -17,7 +15,7 @@ timestep=0.002, # dt iterations=1, # number of Newton steps to take during solve ls_iterations=4, # number of line search iterations along step direction - integrator=1, # RK4 instead of Euler semi-implicit + integrator=0, # Euler semi-implicit integration solver=2, # Newton solver disableflags=DisableBit.CONTACT, # [IMPORTANT] disable contact for this example ) @@ -30,7 +28,7 @@ qg=jnp.zeros(model.nq), vg=jnp.zeros(model.nv), ) - nsamples = 100 + nsamples = 100000 stdev = 0.01 ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) @@ -38,35 +36,40 @@ key = jax.random.PRNGKey(0) # random seed for the predictive sampler q0 = jnp.zeros(model.nq).at[6].set(1.0) v0 = jnp.zeros(model.nv) - num_steps = 100 + num_steps = 25 us_guess = jnp.zeros((num_steps, model.nu)) params = VanillaPredictiveSamplerParams(key=key, q0=q0, v0=v0, us_guess=us_guess) # sampling the best sequence of qs, vs, and us optimize_fn = jit(ps.optimize) - def _time_fn(): - qs_star, vs_star, us_star = optimize_fn(params) - qs_star.block_until_ready() - vs_star.block_until_ready() - us_star.block_until_ready() + # [DEBUG] profiling with nsight systems + # qs_star, vs_star, us_star = optimize_fn(params) # JIT compiling + # with jax.profiler.trace("/tmp/jax-trace", create_perfetto_link=True): + # qs_star, vs_star, us_star = optimize_fn(params) # after JIT + + # def _time_fn(): + # qs_star, vs_star, us_star = optimize_fn(params) + # qs_star.block_until_ready() + # vs_star.block_until_ready() + # us_star.block_until_ready() - compile_time = timeit.timeit(_time_fn, number=1) - print(f"Compile time: {compile_time}") + # compile_time = timeit.timeit(_time_fn, number=1) + # print(f"Compile time: {compile_time}") - # informal timing test - # TODO(ahl): identify bottlenecks and zap them - # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number - # of samples. Here are a few preliminary results: - # * nsamples=100, numsteps=10. avg: 0.01s - # * nsamples=1000, numsteps=10. avg: 0.015s - # * nsamples=10000, numsteps=10. avg: 0.07s - # * nsamples=100, numsteps=100. avg: 0.1s - # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps - # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind - # that we've completely disabled contact for this example and set the number of solver iterations and line search - # iterations to very runtime-friendly values - num_timing_iters = 100 - time = timeit.timeit(_time_fn, number=num_timing_iters) - print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves - breakpoint() + # # informal timing test + # # TODO(ahl): identify bottlenecks and zap them + # # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number + # # of samples. Here are a few preliminary results: + # # * nsamples=100, numsteps=10. avg: 0.01s + # # * nsamples=1000, numsteps=10. avg: 0.015s + # # * nsamples=10000, numsteps=10. avg: 0.07s + # # * nsamples=100, numsteps=100. avg: 0.1s + # # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps + # # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind + # # that we've completely disabled contact for this example and set the number of solver iterations and line search + # # iterations to very runtime-friendly values + # num_timing_iters = 100 + # time = timeit.timeit(_time_fn, number=num_timing_iters) + # print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves + # breakpoint() From 20687cb160676d3ef8607417ce515c024181017f Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 18:44:13 -0800 Subject: [PATCH 20/40] added dependencies for profiling --- pyproject.toml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 64bd86e4..f1fe9a12 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,7 @@ dependencies = [ "numpy>=1.23.1", "scipy>=1.10.0", "torch>=1.13.1", - "tensorboard>=2.15.1", + "tensorboard>=2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 ] [project.optional-dependencies] @@ -47,6 +47,8 @@ test = [ "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", + "tensorflow==2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorboard-plugin-profile==2.13.0", ] # All packages From 441c6889c9d137624cdaa3ae64b0498f8dba83f7 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 18:53:57 -0800 Subject: [PATCH 21/40] README instructions updated to include profiling guidelines --- README.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/README.md b/README.md index 621354e7..f2240173 100644 --- a/README.md +++ b/README.md @@ -103,6 +103,43 @@ Major versioning decisions: * `python=3.11.5`. `torch`, `jax`, and `mujoco` all support it and there are major reported speed improvements over `python` 3.10. * `cuda==11.8`. Both `torch` and `jax` support `cuda12`; however, they annoyingly support different minor versions which makes them [incompatible in the same environment](https://github.com/google/jax/issues/18032). Once this is resolved, we will upgrade to `cuda-12.2` or later. It seems most likely that `torch` will support `cuda-12.3` once they do upgrade, since that is the most recent release. +### Code Profiling +The majority of the profiling we do will be on JAX code. Here's a generic template for profiling code using `tensorboard` (you need the test dependencies): +``` +# create the function you want to profile - we recommend jitting it, since this typically changes the profiling results +def fn_to_profile(): + ... + +jit_fn = jit(fn_to_profile) + +# choose a path to store the profiling results +with jax.profiler.trace("/dir/to/profiling/results"): + jit_fn(inputs) +``` +To view the profiling results, run +``` +tensorboard --logdir=/dir/to/profiling/results --port +``` +where `--port` should be some open port like `8008`. In the top right dropdown menu which should say "Inactive," scroll down and select "Profile." Select the run you'd like to analyze and under tools, the most useful tab will usually be "trace_viewer." + +Sometimes, we want to expose certain subroutines to the profiler. We can do so with the following: +``` +# in one file +def fn(): + # stuff that we don't want to profile + fn1() + + # stuff we do want to specifically profile + with jax.named_scope("name_of_your_choice"): + fn2() + +# in another file containing the jitted function to profile +jit_fn = jit(fn) +with jax.profiler.trace("/dir/to/profiling/results"): + jit_fn() +``` +Now, the traced results will specifically show the time spent in `fn2` under the name you chose. + ### Tooling We use various tools to ensure code quality. From 2e6c649966a2c1900d2790a9c5a5e402d7173b6e Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 20:15:28 -0800 Subject: [PATCH 22/40] moved timing script to benchmarks --- benchmarks/trajopt/bm_predictive_sampling.py | 99 +++++++++++++++++++ benchmarks/trajopt/samples_vs_runtime.png | Bin 0 -> 22628 bytes benchmarks/trajopt/samples_vs_throughput.png | Bin 0 -> 27312 bytes examples/trajopt/ex_predictive_sampling.py | 75 -------------- 4 files changed, 99 insertions(+), 75 deletions(-) create mode 100644 benchmarks/trajopt/bm_predictive_sampling.py create mode 100644 benchmarks/trajopt/samples_vs_runtime.png create mode 100644 benchmarks/trajopt/samples_vs_throughput.png delete mode 100644 examples/trajopt/ex_predictive_sampling.py diff --git a/benchmarks/trajopt/bm_predictive_sampling.py b/benchmarks/trajopt/bm_predictive_sampling.py new file mode 100644 index 00000000..5d6833d4 --- /dev/null +++ b/benchmarks/trajopt/bm_predictive_sampling.py @@ -0,0 +1,99 @@ +import timeit + +import jax +import jax.numpy as jnp +import matplotlib.pyplot as plt +import numpy as np +from jax import jit +from mujoco import mjx +from mujoco.mjx._src.types import DisableBit + +from ambersim.trajopt.cost import CostFunction, StaticGoalQuadraticCost +from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams +from ambersim.utils.io_utils import load_mjx_model_and_data_from_file + + +def make_ps(model: mjx.Model, cost_function: CostFunction, nsamples: int) -> VanillaPredictiveSampler: + """Makes a predictive sampler for this quick and dirty timing script.""" + stdev = 0.01 + ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) + return ps + + +if __name__ == "__main__": + # initializing the model + model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) + model = model.replace( + opt=model.opt.replace( + timestep=0.002, # dt + iterations=1, # number of Newton steps to take during solve + ls_iterations=4, # number of line search iterations along step direction + integrator=0, # Euler semi-implicit integration + solver=2, # Newton solver + disableflags=DisableBit.CONTACT, # [IMPORTANT] disable contact for this example + ) + ) + + # initializing the cost function + cost_function = StaticGoalQuadraticCost( + Q=jnp.eye(model.nq + model.nv), + Qf=10.0 * jnp.eye(model.nq + model.nv), + R=0.01 * jnp.eye(model.nu), + # qg=jnp.zeros(model.nq).at[6].set(1.0), # if force_float=True + qg=jnp.zeros(model.nq), + vg=jnp.zeros(model.nv), + ) + + # sampler parameters we pass in independent of the number of samples + key = jax.random.PRNGKey(0) # random seed for the predictive sampler + q0 = jnp.zeros(model.nq).at[6].set(1.0) + v0 = jnp.zeros(model.nv) + num_steps = 10 + us_guess = jnp.zeros((num_steps, model.nu)) + params = VanillaPredictiveSamplerParams(key=key, q0=q0, v0=v0, us_guess=us_guess) + + nsamples_list = [1, 10, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 2000, 3000, 4000, 5000, 10000] + runtimes = [] + throughputs = [] + for nsamples in nsamples_list: + print(f"Running with nsamples={nsamples}...") + ps = make_ps(model, cost_function, nsamples) + optimize_fn = jit(ps.optimize) + + # # [DEBUG] profiling with tensorboard + # qs_star, vs_star, us_star = optimize_fn(params) # JIT compiling + # with jax.profiler.trace("/home/albert/tensorboard"): + # qs_star, vs_star, us_star = optimize_fn(params) # after JIT + + def _time_fn(fn=optimize_fn) -> None: + """Function to time runtime.""" + qs_star, vs_star, us_star = fn(params) + qs_star.block_until_ready() + vs_star.block_until_ready() + us_star.block_until_ready() + + compile_time = timeit.timeit(_time_fn, number=1) + print(f" Compile time: {compile_time}") + + num_timing_iters = 100 + time = timeit.timeit(_time_fn, number=num_timing_iters) + print(f" Avg. runtime: {time / num_timing_iters}") # returns TOTAL time, so compute the average ourselves + + runtimes.append(time / num_timing_iters) + throughputs.append(nsamples / (time / num_timing_iters)) + + plt.scatter(np.array(nsamples_list), np.array(runtimes)) + plt.xlabel("number of samples") + plt.ylabel("runtime (s)") + plt.title("Predictive Sampling: Number of Samples vs. Runtime") + plt.xlim([-100, max(nsamples_list) + 100]) + plt.ylim([0, max(runtimes) + 0.01]) + plt.show() + + plt.scatter(np.array(nsamples_list), np.array(throughputs)) + plt.xlabel("number of samples") + plt.ylabel("samples per second (s)") + plt.title("Predictive Sampling: Sampling Throughput vs. Number of Samples") + plt.xlim([-100, max(nsamples_list) + 100]) + plt.ylim([0, max(throughputs) + 10000]) + plt.show() diff --git a/benchmarks/trajopt/samples_vs_runtime.png b/benchmarks/trajopt/samples_vs_runtime.png new file mode 100644 index 0000000000000000000000000000000000000000..d457e27b4bce89691419a023981f46c7f285405f GIT binary patch literal 22628 zcmeIacUV=~mOXmFfTDty7?5N@1w;@Kk!(f~0VQV@$&x`Nqn1)Cpke?-f`H^8IR~Xg z0g;@OO3oR{ys@l%Z{O~|FLb~D{qe&0m6iw2IeYIF<{V?pG1mhHIjK#wyJ#sC$|mXa zXD(AH)b11t)wgwP@e{$S)-L={*!HZNt)hj2t^HMNeagkFww5<6Y;PD}+kI2t+Q!(z zT!7~|4w>oaC7`|k^<#}}*3WfeE`9l>Y8EH(Rs8&gz zIjMB(^>Ca0EhXh2E8`XHPh>fy&!3_dXXxMc@b}%cJnwgNJbI){b@Jivs3|oLYAVgm zJ9eBc4UO%S*?N}Y&np~vWWv{+5xK^(S2}9_??J)^lC%0R_q<^buUMECn=E|oG?LZZ zF53M}^Q)RC-#OfdSg{^)Gyr^gj(Gp|pMw&y9wYY2Mqr*{sQ-C}VY|F~mK+lv;T=g-4G zvAJpG=jY>NulV(4PTF(IU!TrCw_Nh5!k&S3b>ZIYGeb@I<%~z2Nu9QR+uM_O*K%@l zMu+719xi)%_>k@QuA!!shYufKzPomlQnsz}-26NjJNuc_9&~lOZcECQZHFfKbqZ8s zm8HCyOOEE*4c41fhMc6DitA&PaL#|B4r%2Hx;nyGzJc5cFk0z8bUb9zor z&R{~$NOh)Fr|tI-_r!FUr}}FIoo0r_bm?e)f5W_9?}$yzkNNiPigu>uNy(+DX9i#V z-sqRTG+vk)=2DJP%Ae`JO~YC_H`&|W)AKTZ?&V?42;-UveMahvV9CVa_D}qL>o7N= zFFsbdsyWXo!*P1x?fbhlgLt6kBt4IwuYs|jSe<0IaVdRnkZ6m()99Vi`QziGSI4bF zBjf@#Dz!P2RpK??;@(FcF`^b4iO2I39v!l`w|ANS?%@;u{5gZm(sYC4a9TsE+lp(s zzraCxUvZat+qtN(5zX>r#4Yn5<@r;Xy0Ddpz9WI z-j=N|EU%&QO2(HbaL~od%IayINqOBdo1DCsq+8?H%$kzV+Vp&>dL}Q?e`NT>y$#v6 z{g?B9_RDSi^JXr~o;`c!zul!jYEsJ)xq&TfT0&M%?qx>C&%=Y;J~h}`DTkg}KiyYV zw7fXi8?5?h`{AP&?P7V8Uqxxxt^52_qWfd7C@sAl)s9te3oRY&((Bi+_eltn<4}z7 zoEhy%oEdJ3z0s6B(4LpWUSg19T>6|dMzI_n)>LKLQ0^BUFn;PdY(FNOLmwI z>9;a3s_)WEM6rn8ysk0hm9RyeP9&05l#v9j;n6Jyjz=_~(PIbdz3ek~-Kd zPCrMKAG3;gb$7p==q@uIYE0rU?i>4FG{4ZX!h`Qvvv#e?L{|yjq~VR0H5;}YP4!jL zZrrHUU;VZ&Nl%JJ!g==PpzV^NJLA&a($9{%9D54~C#SNqvI{QrQ&m|uz05u0Zp*p% zH!|*(l$3mNSW^Y#Q2TVBRB~>D<DZ7t5yFvvjPSwL3-(8zU|T zFl^bf2Y2Xj>lVlH<7)EQR~K7cyPirZW{cH}A%_t4q|?WpkOT3N$-%or&t*&d;+?eojxP`D$lb zpWU)$%f!#04U#K!@^&Mw>ON$F=1o;!$R5pKn8Koqkq?%5C1B{ickf?Uf1`$JBF+%oIOS+kI(zUaB)AgD$>M0RA&f{m}wR4WuEL!A_-_Ku~ z(e&EMck1;0jhAlRi2G1fWQPC{Ff(~B=+=+*fx^yN)M@6%MZY8O-^d|< z2-*!OrCD_7SmaH=6cZDxeRJA_yCqgLBf)WI$f2c-o{mnXB1nAtJq=seyr9usQoJfc zBMs}V6VcJpNDtKk7WvUNk@ABUZVLg&ZZtlLE?-$FSgFCjnCc0z81Od?%DUR|%*UrT z^z4Siy7^idu-x`rotsE##{x+$aAmvojcUQ^EjF@{%jo%!lXfnm!B$q!6=~?0pRm6;q{{QjY$L0fTEX&sY-nicRI5#eaf#3VT}O?c zxXeH2R8)$-yuU75@zb)SWA_VH^X4AR%+%+7k2#JWReE;dq7RZ+_RZlKGU1pnlaBnn z19GpUQd5tLFVA+0*z|b1w1WH?k{Z9 zbNuQ@11`np4Q?Ijq+ctf6|K+8Hma+t`tZRr+*SWVp@<)!%Tda*%EG{bAS7xS( z#r*8mD-+(8#mNdU_DPZ?%8;)0V^rb?DkBFo^v<~jxy~DVI6fAi@f*jUm+Z)ONO^kw z{wAg#g+#=C|K=}habZ?%v+6+!z^T;b6iD4z82h$Sfg0r>asBT`|qpw zl{hbsfAVi8L3cVmz3D~-^8A6ldp~@ib{=UJ-pb5;qdDCyAX>gH+b*X&ov+yMYSDd0 z=^kLIR9|eM;?*=X0T>7WsVQ5JX}05*-wwBMn3zjhBjnyr1eL4DlP_n}- zYhL2TbpRVv-WC9!r`O3m@)j&!w|ggEva4h+N~-<9qR z%Xe95F3ZcyQ_i_*=AkIgh?o|gtY6j~EQ#GpOG{gg7v`A~LX$%&6(V8M?71eXh{6h89O;Hxjgi zoF}9Mk6V3M7P)ZYf?~iiv(%fz8cjEjc^pgZXctPp(Zr90_vQOhD}R3d+5$H>mgANu zMD0g%^Sms*e#~U&w^h9JtO&wLio9;h+y-DGzBpDikB!0N?Z~6{?nHTjkSfw9bMwml zV4}eA=xZ5VhRhqLc;RUcV39nKxt+J9=>BN8m1Vlg4!4yh0y%(vZCVX*)gh_V`=f6T zH&4vWs9=dBvzYK!%(@jX3l;Yvvet$@mf+I!ERyj2;|~QH8JU-ff?b=auWUIA)b_Es z*aVrfIm;#zLq~u1%$ei0oR>m>$G+}TNVn*acpD!6xuc_F`Q)?l<4!YiKF^-*FIj%C z5UX<7W3J4X*R4!(3u9!Mjm`1Lo~H5Jxz(=lYQDR2`_!pZwhh|$(mrJQGwK~h`jj0+ z=-4kkGo7=AmzVeAtE18g-Icdj(M$!eEJ_iGurOMX*r4N7t~1(I@@$ugZ4_{6*Z1#l zfbI?f*;Lo%{7mp2e`_^t4Me!OxXAxP*Nx@ba|cOmt-3h%8dpb-oP<&NVu^CK8H`Q$ zj~~}`BR7)CshTjq{>6_k8^HB&;+iksx$X5>jL+Ed&uZhHC#K|rs0`>TqFA00K7HJor zRY)-?XXZKg^&zsCmKO75Ub(@uf`XMz9t<3a4++zJLRmREEs3205-!<3K0Z^E6_SJH zLLI3MC9~gLz<>nqec#x${s#q&#N12s8~JE{iA6S!DVb?Ncc)D%;?e?yCi z-U>iW3+?*#CCPGFugpOm{X1@%a}E3ov1L1J7kqVHt9SoT{GJE9|E3@{PQ zI6cbPO=_c__KWi&4g|0=7Q6uc=$J7a55!JX?)@4#-R-L@va#dEce!@S&l27gk*B~w z#)a-w552wXF*t+^MPD!RAsKBuk5-g@hl?ukXJ)A{Z{iMJ=Sze`fHcfM2TTZDxRt}Sf{F6 z2L-MUTme#{GI-T1M0t|7Ew@$#<_kHyxYR|;vt109KqMNzj6e>yEh)Y*ltPji;BYM2 zA;{nb-G-Cp1o#OUe5o8vEJ%1IXj+PlFOh%4JOHpB%p<|`TIu%{w8u|i@&w%9LV?TP*Y-EOiy zKr}#kAIR1W*2P9%3^-=|;rI2g#GTURULW7%=H@17Qv3OPs)`DkXhE1ofm-pJw%HBTU*Z zzH7fDD8_8q!P5s8RgWZTKIr26;>T=ueqhBrvd4kUiN1gYPoyy1hVj?e!Wnjo-gPF* z01twOUzx!%3?1|Ah8iys%q7L<5)P%gtfE3bUL&pg_w`$cge1CNe*S#E{I#$m5ey6} zg5nK>9L_;OoCkLa^YhzD^1(wdFJ&YH>s(K(&f zP;GdH$?_2QDkMnUX&|v++1e-!nk$4^ zy2lT;9_%TZ$DA{4zqgk{=-1PyPf0n%**O=Km=y$wLs(e2Y#!uK5qq>Y!$OzjLulid zf!b1C7H&#cLj(gYO#}+$l914W4quH6A!mJ(HOqq>DB?I3wZwO8?3+`IS3ef)3#{^Q zFAJ&HKGLH4)1J(Wc>V&zOyYYcrh#%#_cj~V^Ms&>!vKAx(m~=fuq@&B!4fX)(&A1t zN*^C?+s49jSekHMQWPM%CJJ#RnjbT34p&bxkj~z<%<8si_3_guQ-E}^{tR8VzRu2l zkO@D2{AdhR;yCu6Mmhb4I(a!e_MPM)tSr?dOn9E%Yzz-6jc+gPfFdi4iBJTH#mZvX zx>c{Bs6imr?x z8;uO}!x*vhU`btMkp>XwZOqJZ--|p%wK{1i6b0sO53MMVxQe$>Q$qT_V>N26Lf~{? zJp2ELv;X0c{y!8PJAuGRRS;>adw4h=>v{la1Pq+4>|Vqg{P@w4c9x839$Txr|iEo;=wM@FGyAf1@>%!2TKtK+11rwqbu?0MQ_TtS;49 zQTj3z0#d@1Tv=2^p*T~&Kv7Y#8@LI@U*PPX>vhQE5}p687&>z-H!pAaySxvAEySl# z@CU=DV(gC>uU-v|VK6yNXvHY3lCE0){rxD}T5`~@Tcc)nJEWpyYn#e$ zjN%Uh=Ekv_P-`hN|O@h+D*H{5zjw={CF1i$9gPk-=Bxb zRo!Vn9(O;xt#cg}MPX`eNqb!a6%gNv<-PxmMmZUTZet3&(rp8(?*INwM#ARgvVOeD z_Mv^MJ1(5Gk5$->ce`IZXs~eUqFS<|lp;@K*`3_DhmNU18cLkn?&61-kFUYi9=`A! zmYQxg9H+wy{Y{alFRS9mwj6;ZcSn_5n}LE)Bt113c=hgjWb&>~p0FD~>~khlF5LT# zYBSOz6Vq>U1|k#ohxc)EPG4KLi+VzC(f!)Kxi4NtY|9=LEXIeMR+8)QjGTIUg2efc z=Tfe)t#MGd3wy7_?L#fr_D^4yz_b0VxQ4tB|Gh%b-hFHd&k|j@JlDBn>#G?z>^?8x zcvL0Cx3mA-3Eo(PDWkabY3dD8hWBVzom_=WMrz;Poff~tubcNa!1Q+8pR~-sZt()X z^HGHZfu!Om{UInsjiFZbAgP@R`olWt&wHTFBm>3WM zCZ?vi`$q7-mcpO*$KefsID^9)$O!&a-1$gS)XRtyR76-+mficJ?ImT zU>9rC`xO+d0Bltv&Ad${V;)y@*LvBx&z;CQ0O0J>s9@wlf;c3dtPwBbw(LAIMTalT z;`t;+K`A^I)t+d$jGA-o6A`ORg|G#2a&yZPaU9yQaY3g& zr}rmxbXBI39v+5n#y@aCLPhLLO`jwWuQJ_L;sZ62y{W z5O;R7+4`B~``g@<(yeL(isx9enkP~`6a^NSvD>Tg7TAq2m?a;vcu>?gw?D%NxZ2mnVF^# zuDOq%m7N#nz{^2lX9dS3&uw9Mxz2OEjE7nMpb*orz%fROi#b~c&!SPB#>5>ZD$IT zkjEz|+5yA)cvOb{!gTKM-@jMC9|O6e4xmEQj7YryWq#%qh_XHLx?umK={hEv|gWfk$(8cADLH|0{oF$uoCv=0?T2nqL2`TEeQ<& zJn?jaysufk`t!AUrLs#y+QGlcCQN>P5TS&;cz3mENFV%;cEg6U zOKkktTkDt^7;L46u>}|musl&-4s)IFKiFndA#UIL0E!u~r2;R3d@4)gKhN@XLFO-lNp{H`TT~>1}_ch@Wbo z`77Xdyx&sz`|rQE4k1HPCZsW*7!{ioHoW?Lu!+{a!+Fw+$o4O|)i2}U?&rth=C;C}NR=v7D(H?C_~gfeXn+*O?3m`u6@2lDs;8Y+6u9yf zwR3OrDb4|$P9Yz(Uy8(Kp4eN9ZS;g@0h10FBy5ifYz5TYgB1($O$t927U;Yw2A)6d z^9W&qRl~lP6Cdjeyg|RJ2Ac{Y6#rpCo!c+7nD_wxmHq7OQBeG|nx)D6f^N_hqS7So zlWyJ?1I_&fry|PZ<>eRgpo*AQoA|qU+h^@2I`>PLF8PE5TSudqjkKKA{?~<=SAIL6 zsFMfJ#Gnv_v6^>BAaG`-1QCcJ-*uca(vl$$&tWQ}dapu|mCYXltpsf`AKq3?+%3eRf6t>{E^~4W-S`C7lXnv_@V49 zHWdf8H3J33XaD|PC-6lKn}qW>Wa8RH(tj4Jx$dL&AR|@CAD^u#t@$oFyM)ZSr3qFm zN;P%+jgIor2Sh{gYKa`n#jGp58zz#u2C5uQ8g@rfoYqpZK3z*-`?MzGFmeEvvy8O# z)5u7}llC-)EDxJpl`34eCk{$72rP#OR)z8H+03zacKPdFU1}=#P>wc(qgPA5%7VS* z-T2@WD3dWRB;X$U7?8;482fA6>fKma?mMuHCMk$CRo5PTuwA2Fk>0kP7y! zY&C{yBXQ|c25NyIDRGU*;}j}}4bbCfSDFRaReQ2}ZR_j4^-8F+taS4c5G-c;)te|E zFXgN~X`;#d1b3|vw*}1_BLAQ!loLVU8IE9C{;R$Q|5IBjsA@%E7E4UhxL{0VI(8$| zcnPXN4E723qzR0Bh=e^yTu@#iHp;HS!9$NehGO-CVFP*XI&K+4N-hzyjKnl6EiIju zrGcrxC->h}aoThaf`%W~Os|FOz^h@yIyi_(r{IS!j{l}| zQ}bp7Rv-oliMSXCJNutHK|VgA#McdBl2nQIOXt{+{&F|%*bxaQZbxzl^k3*9mt|xg zLbC5E9tVr|rs(_wKxO8A0qusn;Nhc3W!6C`Hhawbz%gxAG_T$ShJ~WRP$fp! zP6;@TDcxrL$646KL`X}_!&lVw?ywN3Iyz>C*C|W7kJp>c1qVmJT!CZ;B)gs zW3jRnN(1Lu@SVwDP{%YP$Ks!*>tEM_FE*T*J($uwqN!2u-I)cgpL^zzEPx)eu>zxyw|v^6qBxPq-+ zA-$Q(Qu)hFnBz#wEswE-(hoRyOhGun22FK$TuaH+A=-BMzeGRL#Gn+8a!Aazj}Kuz zc*(wrMFiEGv}%Zqq*zU4W1zz7IQ5hvvmm$x!ON;qiiUoZ`7D8TZo|fn&r#BtpBawD z1?FaFKiAYK!mFpErq)Qrns08^X6LrP98yv_=;X!zD17~en=xj2C$Lo~vg|4Dga0>$ zoV>3>9nO06Pkd_AOjV^XD=9rMe$2)zJ>O8UydSP3va3i<1Z+P-J~)WpbRz@rKeUm* z1)u15yYlB6_$Vh{{zKkTb;+Cl9_m|f;sjnX5>{9>7g zO&TjQ2jWF*lEHfk+KJ?WbE-epZKut@;pJS@VM^g9$TY2dsyiNqvc?~j_CIx{crr?A z&} zjGFRU5Fe&}@Gpd>lqDmo>5UYH*3FdLr*{7e<#PVq5&F-GX4Rh_X96rFzWPMS=Ef(M z{54MtOc|x2WEq|QP_j74KnZDt-gNK8gg=u+)wQJrd#hgDAN>0Ln$mL zUmNoGSbl}~pJTa#>=yj8uI(<3r z4$Y$IMCFM+)d?q`G}mwF@;!lE{E&ii__uIQdB`V6g-m;#(VG&gCY`}37femDM228s z;%j+qf-FT>5VfUCU%57HI`cLZGgC|2ogYvAwY6qtB_m(@S)U|14pLOUz z7KUYAdY$iZ*k74WpD3lhrzvNiX6*TXJz}3*1HX@VgjOZ=z_|wCG+I9D-4AoS1cd0^ zgrjKyxTWyc-hT|wU#}au8>LR_)XNsU33wkGYrjfp!%RtqIiyFUEtE@L|0>rZ&#iOk z*!~}KT@0i6E&;mT2wPWn8!<2q?^+#=4d}XlHN{!Jg1H7tl=p}c5GK-H$8M0`7hzeUV!}VcSbbn9 zB*qRnUp+(slAnFNW_3&U^>Mz2hWM!S8H^u?CEp}wuwH6i-YS#fe`n-30LWGQX|q@P z5K9_VY?GcZ{^U}yEFjIi0jUF=msDJWs2`0YA#r2vdn^_Tj)QxULt#V}42&JH>tZ$3 zs7NBpK?K<)dNU3ZQL0U^9F!C%&BXlqm>`E9#)V0gx_g#rcl?GmvT6On7tTF*=$IuV z9hm4i`KIr)O%US|tYu2U1TC3JMZ72@Co2hGYyD@qkh3J*N7k#T7Th zDI?i+tDz;Iom&viV3_6n2FPj*)ebiG4BxEGOb#(IEmWt~zi!>JqgO%K2hTq=4bcfX zw@={{B+WN(-uO4ah9!x3n?Qc@;3*}R-c1XRe^1-AVR3TZI+`;h4_%PFW&`d^N|(-- zUBO0tO}Ca3vXgU6vG6~n^i=ebara8g%6>_9MS0Z4@m!PF!1?MYdrlKY4+-d(4F?5h zP_!7J;)7+~=cJ@eVLfWneDD0S?y30b<^B7mBP+DO-gG)wUG;oJ^ApR&M{(q(h?gQT!c5Vc5l0j}vreCE9_0?{mR=T>5Ng4*dL|D2M zu#c6YkKLzdOJIeW7sh}!$t>pkJ8dnll{)0j3@6A&I@)L!YZqdFJd1tQ9bt!>N7W?` z>MQ?RIyvC|H|d1t5ZYwkV8fa~u-hf>mPbOM(;*E&^gzE z1H}?+Ltx1!9=7q$PlM<|rKO{*hf$U#Vf>Kn#H6jo{S(C~^@ON#F?3S4D?12#R}Qzf zK5d~EZ>x(}-=;iy={IqRE&2ewKUwF#W-s#cLhtWdqQirV+2O?!mvwIkF*ji*GtN=I=s7st zl{)vuX)@`)RvA8E&O~*DzI%ETYwKq)N$WclKPl^lweI&rjWHd!WF<98)B1|xiii( z*kL>Wr`yr4Y}}oK(PoOx$ri`kubRS=Frh4^tHj#`pMBTH74{Te+?^HvR|P8y;hV7? zW58g7C+!-z_FYaFpYF4w-HA$22`R~uSw%JD|8Xy5->i54zgQbE_pCy{I0FL%J6hym zQ#6IGhR%+dOkoiy1N4VC2`zSsUxUO01_vR$q2~RNlKEGz<$_o_K0cn^8{|ZCZC6(p z_h6!Ch8)aCiuj&1{j%*ON`T!}qxvyy2eWqg&vqwyX!Ny+V#;WLhu^CYWOhb`NaU#E z5E&l|oe8vk;v)=7=hpW%d9yzr{c19YByEH)eUzVL(X{gx9b?82Q%UKYo%6tf zbHgne4QLgKrrpNX`X5QIo+1{aSnA;1bOmEhT?Zv zCs)mfn2rDZ^9;tfCDI9L8V`a@OKt!yE1p(luT#3S*5l_f{>u`IbaatD~||xHAls+FIYO^HKRW%?b49WMyY_adMWt5rY*R z7BHZSojY&U%rmYW*uS5XgX8lXIoR3rzD^7IBU7BkrET`_cxc{G%>$C>aW_#sI$mu}fBaCdj4yiOm$_Lj&!5FrKZFWD5 zfR&Y%TAGL)?jIrwA5&y5!)im0Hh?u5&A+`s-be{{h>i~nPHmy&*!G9Q!)y#-EF&wcgyDi!Wk|@1S;Y1{56_CIS`%7- z%L7GK^@=^KP+&tF(P6UMHtjn4uE7D_J1=0+L8Bkzs09fDIB%~Kn}yH6w$a@ z(p>@-db-_dw5=*TGbiT|wscMOwJx@qlkfG|80dyP?tGHqxHWV)j+n|25udoxF8)$8 z|91`u=?VQ$h@Uq5!dVrVM~H+=+W423m&qt#2X-p-{wL8)xQ6l_k_3?@lu*?|xoi;i zgld#Qm74gJ?Kt5hT4k|Sc@QHk`Q!%J}5+K4GuF+ zh8MAc%K;h7u}WnD;xEDlJ5-a1j$o9}uYf_70}6csS@8@G4rT;;&ijf+j;mDK!kDlzlXsovO3({>_gwy{R999O^;nD_J)+m|rMW8O$13zuq zq}eanLQI_o-wJQf^uhxcfem>;L?oNyr0mR&-fwAmCx` zl0QFwWYkM=b)*%5}D`JXoatFgNX%+2sZTh>SfrEFs-DP{O4d`!B#@vWHR6vkC2>Pp=;;UEK3?-?7DnCA09`H-2$(Z2K1o9;r1Z{kZ4*XW^9U&coo$x0r)%xQ0K4>`fBs25pgevxrRgY4+{C#tqw56j z`5g6Dmb)M&dVgcqwi*+;)u>8mnT0-vo0|WfDcte zC7&ke9ejSWhjbRPFIh0P52YOBgeSH6R+ZIZZ$A_Ms8dtO|EZ@j{T!9o{F}F&U+E!f1zV)bPZ)0K_9NYTvvm{b9 zVt5@+ir|zco)+w-1eg+tk8e$@Xzv)hx=hg>LYfT+8WIyxr9c7WD&P?H0_E&3DT>W) z#0D*GZGpBWKQw|B78a7RIbu|~8|QK;#i}N%K$9Qp#u~+<_Q9qYR#<5^V5DGsJV|a+yy{qfGpn0nrDvk<39I2>Jac))2@RjWiD292}%xZ^J>3!GL>^ zz2B{6%ph1C7l4`drk`tf={5u$e$9mvb#m0mOqe@qE>M&ge+wsN%j1VB^b@+S=95fy zoOBDnQ~WkS|Nj^A`nD?-4|()FiR%xtjSM{Jmn|*d6-|*=K~g1easEqYf8rA0aqnI~ zf`iTM=0Ci=UPMMlE?p4h=Kk`AtYjgQAm_0;Q|$&-7Q&F$h(~v61zE-pN`n5bWg{H( zLF#?%oam0mL(sMGV!;eHB-%I9{AY}z;1mcCH$2Ngt*>19}iZ?&o|n#>#!O8 za65*=t4m;`xehF>kF8p~nO(?gSi7+J9sNWzXumAx@(Yti?o~i|LtqCzl0zgGh6J1f z;U(Jz-`XU$UjVFG{v?2_(|J^3S*PI0ok?s-f5boGjUHC>Q^q9ZlP>=h^l%8kbf$>( z9?5d23$BxCe$+lw=~tz|snprqYpCNor|-lvb8UBd#Ag)@Ytpv#roT*>VE(33hJ z58HhWmb3_RTXs-P&=My1F^QHxe{`OSq}t$Qlu6(WCh(zOw*uy1gT4q45p0NKAQiruh@^i#BCbXxrdWam9 zdwm4_NB~|uKI|Vn(4 zoe6fgxsF5kHjg)_8)>pEu%FajX{HA1%62|0Xu+Wdl0x=vcD@8#!2T=*)0A0IyuEAk zPMdPe9uRSk88mhM*C;Im7a~VJWJXYgo5ed}6xT%$CAxXgHD-F!zCV2l!-bdru8E8k z83tP#O30B%4<1y(!#<79{wQGSiOI=BeLPf!Ec;MwnL+=Gev(?(51*xHtZ>?>LN+7T zO&al4?swjEUZMD>qBjq%D^LYnFfYba{3t1d>zbZW;a)d*o z38-VN+Kq-V{ zw$iQa?S-HS1BOlFVFS=_`3mhZeq^V^pVSim_N~4Kjv`pBAzveg^KA@pBou*w{7`2P zaB-FH)cXRPa5*_`2V3SkiHTSeDXlsLCLu}*!p$XsWhaC>Iv?hfuXv{oh`fM#>+uxy zdQHMx+}L3F{?3}2IVYSYgVsaSwRFrzH%HsemJw!lN|S?aa?!+Zfbidw)}SMR{eP4& z<3<{I@c4C|$L|sR*58mA%!$7HUuVNe9Jd>I4zn zu^up)zGv1h%6@-r^mbcuSX9hFPlw;dA?f{^K2RTk+!a(|WwDejn&m*O%1IZeMz023 zmPcW$PnR5pSsJyT7#IKIoj9e!%m$0GC0IAh`XhPbF9AXoSUE11X1DU4=aQq9qJ10& z<1;+@{8wGUez=Gu=vd|_zLmRbvjB_Mn^Z-^D>GU%hgr)5D6Pw~G4#-v z4>)#_&^7>=aBRdun5*FSvoZ>UYnrZsW9{nIQDE0Iy~yP-+?J=(fB5iWXoVLjh_s=> z8m8Y=L0Z3wtrI7G4Cxo|#9=nXHipGX-~=Lk0&+1q!NPGU=@VFRbW1aAJ#^?!c=8q- zLPRV>jho-1bbuLdXJp>Qs{$-TYoVoM$i7~%`eF2Q4%&1N=aMeEG;uSG>gdm6jn z;vV~P>OdvIRLBj#@FRHOerY^O1Dd6yNb?ct+lHNtw6CKrvgD0dTzXlcdF&u*-vxoN zY&0ii4{#t7eSlHuu_f(Axek+15yMGia*)%=)t!tXJ5e+KoQ~*x!G~-@-m>!XaTE(i z#}*;lUqX!R!g)eidz>)BqJ5qmyjHXi?n})w6zUQ{MVRjlVJ_l8Vq-49m1#1GMXkvZ z+!sFiy?J9FHjtwlhlYu39&gJfjgx3yF~LG=GFdPnXO}IkfDnfgpBs{adc7H>131b3 zjdO5XjiR}^xlaP?*^Tt{4WyCTtmPOvI0$&DG;NuhgBhDGxg4PspC)Gu0=n@a(4K?V zQj#`9kGY4yM-C|f_pZSR3W709SCa!jp_|5H5=chF@gc<647G?Hj#6H4O&|vjK2#&Q z0`Hp!aDw6@gX&b%E`9@mbB%oGEPRTzd7(j}<+K`x3GKHB$O&%<7j-z~Dh)$?V|vm6 zP%VPsTi9~yiH{obnH=4sUeAJ-bxdC*R-y6Vj!=Rl-4wFWd7o6Ac1b`vY50a;J&I5Z z&dlg=1ZC|!7)uir-|9g`t2|X*Z>T)CiQXt;0@%Nfd)DxmauK> z$X z6A1?qY2;r*T=ZezwDZUb(#-~jigZf)+r$y*=h4xSoaV_yR0nc69F{lf>LlHf=Ev1Y z&W}f80^Va@Jx_3=H&BJo`+(VzHABv=+`*%zfH5TpcarlMROe|!#@_jvR-h%} z;qM4#vvOw= JPF=b4zX4tIPBQ=i literal 0 HcmV?d00001 diff --git a/benchmarks/trajopt/samples_vs_throughput.png b/benchmarks/trajopt/samples_vs_throughput.png new file mode 100644 index 0000000000000000000000000000000000000000..8adca982314048ea8d5cfb2aafd3a27e5042bd93 GIT binary patch literal 27312 zcmdqJc{tZ=yEguzP)Q;oWk{(EWk{3^DMM*8M}rKhXfS3hl!S;B(Vz^O37Kb#29!vt z%*t41BIA4BT5IiRKYOix?DswPKfhnc^Bk*uzdpk~T-SM>=Xu@U>IYPr7I7`2D2i#% zZY52MqIIJvnwACh_{o;)hHv=KHd|#qTP=%|w)RHWrqq5TTg$T+wr9r{!bID5MxGB=G=Rf z6tx{5^fo#=>^n8dFwnm*bI;nluVhlME>$k&+8GjY=rxf7v1`H@{y*=H%M2FqTS`T9%sMfR+q#nhJf8Slf?uLIVoxDcJhkpj# zHTwVm$9FF0UifbO>gMY5VeajAJEAm=UixvX9J!_z^{n#l<{MH9Hex4UEv-#DQWtjP zdv$i7!K1ywn>_I8-OZ==-FffM*6z?!OrsSuwEMct@sCfAo$1l;Vp+UGV*bycKcf#s z*7C1eQyrnqcv_MxR$J)A$&>eFq9lx#OkNW@M6pkYaL7q=F+Fr>Tj#rlasrvV1 zELt=3k%ADr~eJib<6U$lP7IJ4FI+X9#IR)&$j z#+6&lYd6$A;0YG|_;vcn=QM_6xwe;sgE6=zr+V0U z%Y~EkuLK)7EtHayYR}v!EiFx+oBo_l=i9Ph!<~cOF^|}NqFxR^XI9|Db1qwdK=OhR zbGp%U`qCFKo;5c&*T!k9O^)@e9yqY1$+=%{J{?`7bY}eY9UbN6r>ah!sFEFCd^qbo z6U8ZRJpbs?qXVzkKWcHEo#dSu>p$Lh=E?CQm*&}%JVA$(uBk+d_sHImE2JT(&q@V7o_l(oKDL1Z+3At@l-uLSYpL0z_EtT$f+IgZFI%!? z38$DLMeRtgGil0qJYDm6A(o)5tgINu{%C>I2g8T_SH_xMc{9vETnP>3&b95nvPRX9 z?a0r9Co~ON7Q9YBe|YzNNQlH!p8lA)qND0j^0zFk7)orbsm`~z-NG_ zJi5L9Aldqs*49^a6UyF&@4}j7a<9X-!maWc-5*^13%KL+ManZ3K(p+{_#ot z!FKz;%fBATTv)XKo}}O)!^g(P#+F~V1ojS$yg&H7v5^~VabR?`EJE4mdcl+Pjk)7> zMKd0VEnyeFe@w~9P<|i1Kd9;}qRzW?GcCl0@yW@rEhX;s$)Xca9>`zfDk&+!&f_w# zjlFE&e2IhKx_L~iW{aEhuODAB7-?u|-kp|=J(SE-IN6tHoJSEOI=irOth#Fi+(RN7vd(^vNUTUMoLEFzPK=o4 zkGJ_)+Sy&hWMeye-dHIuZ+VFto4KKvZMhmV&46V$fByWIDur34&bOh}IX0cfzrK$; zT^uN3h}B78$MVUw>%ES#Q@w#zCdGcqk(U~)h78Vv4f3*ku2cE+3rdoYf zc_8Pk`|C#Tq)h$xUx;mprUD%XX(#vR4?JU3BTK%ezn__bfnlqROoZ9{=<@!rk(QR0 z#gF&gPH~xbDlIF+4(+mUIX^ZjtI#u+u-mNgcvPcwW511)=$Vg+my3!N5W-q}dahrd z$3Uj7eB^EDc9VgY63R{L+!qA+6-U3jWX1nHLQipQI$E4!^t`3Dm1gD2m6B#P^L;o( zH?e0Kme7)aAgsk26v!X|+~7rox;!juaZ-z7UoN_7(+}&II`ut|6Iu*L! z$1qcdwXqr@G8cqoY`feLeoVUFh3lmn)8T7Yh<)v&n@?62Z|~1{M@0Pk)n7rSz-jbd zfwTS5BA4mQzm$}fTjOFL?ld;$U$-yB4Wk@^vH6r@c*7-(?YIdD17}~IB_wEHh zdV2@)`|S5Kokq>clVZpzH|)}|V|jL4oklh^T^!zD8M>W=OnkCI!N72pLbG%y5(Pz( zWy9d29}y8j=6*3Y9wWO*@m|q_t2zm~;isw|3KV7{tiAg_8Y1T`^(FJ14+#dt9dF1( zsAHZkj#R4-3=WnA3Th3!iKQk|BPwlCz!Hb9<<)BP+tJ#Liv2 z#Ls*bK?GZl)JBGV-Tr%5u?BEA+L!~OJMkRhb>Eo`j_c`p@yIz9KYu>YrlXRqmuJtP zv+(fD6wB_l9{RS2tV5DWJKl!+VD+0IKG4tZIF@ax_$$VIVx&7n(u`F%QEyOBxp>-l zrSuZdCEPn59JZ?X^V}HyUT5;-Q)*e8AFqPn%=BdBk*xD1TOCe6eK|c{GTow?=Sb$+ zk`QqdY#m-cX=x4wq#yH&4vjrMUCmD*0D|tb2it7krJAUY^n76Kta>CW?(*};=kZ^i zkHk&O+4en>=XM_J)0v4Yc>f{cP<7g=g9slcZy#(UI}y)ttj+I-Vw20{x%AW3v`?Nq zDYvMde!}phE}3^`{44Y0=xDC=euNPYQG-&<`TUlj{i?Gw(;5X%7s$MlDVMSD^KHzt zSFrW7zb9ry8-FPI{K%Pvitm#;UoI_EAL<-2xU=cl^}o2;c2ilRsS`7k{o;sY&TrF@m7=1e!m&Qbd$XFw?0)D~K2qS7mzU?1 zznEQeYUaL0VF}hw>-X8?RF=rbOrx)WcJlQSSTYW8>@S>rQu8t6%po&iwr?FBTq0?gCV$q1 zDY~-;>wE)9sYx-~MZ-rfxq8>NS6R(2!T@;!FPhE)Zqe=_QxdgNR`w|M>Q%s+pv)^* zXybGe?jXb6m2=LlaT)E=NzKUEXw~>JQD5t6T3XOgUe_rN>ebY*;h;>s@7_?E3yZCD zo#oiuH=q3^=APYD;Ov7xL$Xp*y40&u?`2GF*k_t&A1YULBhjW_u+B<4l64)grKhL2 z+t4s<-+ih5nP)%qBNCSx?>+nyxkoeEK+ZiAv0?%BIqj5xO{QrDI}KlEW~TMeANo<# zm~Dyi#OqVT$5u$6e^3)$_g!G^TBXFKr1qMqz4ZOVZ^sNqF6~oO6S`9s`pA7`#5OfG zHK6Xe>oK#H_o5Mg@ja_^Baw|K#O;S}13l}aF%^CDoxr-y)jf%!v~`_u3xX3Ur-Q*q^hZ>=TJ4eT60R8UsUuUpxI+!QENn5 zf%DxpQDyRbm6OjkWT$>>Zx`$x?yTaVtj7n8q7YIJ(C)y_PDpQWZ{GtzUE0`q?AL=J zse=KN$@O{M3NDxn#HGbwUiz+B0|;M!%IVqFg>7A3rw5#WZpEmqMb<}Y;aIoL$AzKKuBggJ-?d0B&mwnoOrAoF!0W0i4tsn1!QtWCK*vJ8E2YjZ zr6f#V_!{@SV(T$Ku+W*@xDva-!O5w-y&{;&+NnD#SSjIyP=9Mq=R+;|Gs}+!$XAVG^j49o{!$KwC*N=ZR@#35$P_GkT$g8NW+o*o>*deh zdK#)|`sd1_;bHOf4O`u{=%R9#P5^N1R#SV1CE~-)Bifz5%WUL(9dmbgH+{@a0^kY^ z{9UFeXebE@iGj)fB4O36>rM5*U`to6@-3R3sc*SC+ek_Ui)Q(Kp)$*hduO>&k1RQO z@F0naj*gD>smj(iHdeoeOw-TQEuqw8rzMO_7qnY2GBT#0Z{*^?()!#}xEBEU^rw{7 z2Oh~E`!VWH&xBOzJ~ZXQVc-@MvmWW%wrb2qA-Uk^y^>)+?(J=;QC_`$OK+JcJfGs+ ze9~93pBTnwpvx zELkFTx<1`(87(dCz_;56h0bA3ac;9-i-_%BT3YH^-d=opUbc1nB2weH-w&i;%;|=# z6FhzoZ^6}YS~uOLzv&MC(5s%uCPOsc@Hi%Bjg-{S@S%%(2W1E6ljMR_AbgJO>O8xR zNQ6OE>9=n$Lv`X88@nO0Q{9$rpl{8Df28ajR9Y_BZFBe*@H3yB95>dXc=kG(wQJ|! z;MyW&w_=x>PE@l5OUXfn>8tPFse5^qOfWB5LdNUqaT{yvXMi=SShku;>$T#a(zO6z zk#yf%pS4C@Tw`0O>-xQW_twViRD|{R7tP|=*by;XJ33AdWZU)LvmP|BJ`k?xPEVg~ zZ)20!$!r@C@xEF)#`H$Gsz1*=^9*{0M1)=y?8|}DWe2J3OI=5m2HtM(XLGht_AKJH zwR!@Wj{IoZQMm&Dp>KAX+{h($)(g;dRe! zk?P^5&@bs`?j*TkPQ1LmOGdii6EMSJ;7}5F^HYJd^pz`D)RPWx*}83834pdm!6>_1 z+xP6pql{F>nYvA>(|s2bbrUq^LTW_ShseaQdBZg4lfP~cc1Bibt&0)=Xk~^7exD=9 z`To2tn{&W>sXvi8gx~Y3XiPQsf2gS$dRX@s)PY^)6Ster+9>icQCIuU1y>yNGX>eVj%?L^GAvfAQ-LX+sMO ze>XQu^7K2($jHe5Quj{%xTp3s4D;u2u%~JExp9N3*8Dk_UUD6&fP{4uHw9L5 zu)1BQP_%HI7I}}1m;BVgE`jK-!(M_~(G9~2&pkiqe2mj#K!ryt6f#hxZpPY?cdM^^ z*9#5+AzK*>0uR89pakT`mbY%+4sZMGP2c-81k1{D{VM`1HW{BgcLVVv3|YVdqabG6 zrJkstM&H`ny7fX28>vrJZg92UJ}9pU>}Z59X(LmPZt>`=qdW(Bfl+YJ{lq!jSs%$4gX+X9jVGJ@B(i;I5_y} zsVSqBMU(HF`_d+_ZwYWpo?d!qee3W>?3HMLc%R+5vt0 zl7AGLDsNl7YO8ty3+)s;pOXFO7VzJ>Osm(1#h2`XbS9HC#$BT%% z?^dWU+7(QxtyXRGsfMv9RWCJw!&&Q2=_h?9f8}1K;F7O5nQHPfu)A16wY`r=uC*^x zM0pVRlgvJo85TyJdi%gJv1+u;hr{QB<@)V-5WL`2wZU_37fKZOIY5T{TNGmLa#FI&~`1pn!{nza2Z=O3Wx=22xEQzcd;& z)Smag@t8qFFE(WM#b4H`KhsWC@soh-p+55HQ{${-Q2S|`=1XJT*4Ea6ooDKvD66S0 zdg$13S3=*Qnxz(t=vmmbfm}my=E@Mn>fmwSGu0WsV@F=<>+9=1J+VjdsjK88*O{E5 zEUy-z3eTXR<=8TZ*Hthg_S@P@wtf8?l&!zV9`#VpnSb3n`s(UxwR%)MgQIoF zBZ0`kdC(11DY)77)J$max@ks4QBz9Y3SqSib^bA;64k z8aotFsVVK0J42jWktsVnJ1N^;Cc0x-`^^h&njObZ)qVNGRajWKG{D&2{63dI)f9#J z!->!fB;heW?qJ*B#EmH(2PR4d;37qgX#N0gLqmfY%CpLQTNr02>Wbc>E?Xt*uoQrA zlU?JN##-|&gf2xwC1u$z&m|s!H$W5DP_r9ie^Oynoc_EDJb>Sz0o8N&Z1BbuJanCL zOgx;is4n@qsMYC64@QnQ)L2U3j|O`Zk|PWXoObWsy9?9v_K|`=F4|}_qoJX(fSLJa zT9v$yUGEpcX~B&cL7*|UxFMg2gS?gZrI`;sIjWdv*PCL~d7xe4b)Nrq$@2}IC_$WB zZt#75apO5SfOmQJ7Lw+*3jmxxrJZ8dO*XgzqD!dmKF00Xww)4$W6ZN-YQ8wk1Yp4H z=;@#UiZ37_fK%2%jF1f^g5p{Z-;=V+e?0X90c7-RsGZc~NQ$23AL3h!W~W8-^Yd>8 z1Ps2C-WTK{-F$JG@S!KAo2z8q2fx02{`vC?jOZ>+O;$|o`gYCR-~k9P5+Z*|251)S zqc3*h3O2{$-q07I_z5T`OerD12`WQ<8G%jiKiYH#44l9G!~#U2l4S>F3Cda{FVAz_ zb$Ux;Vj_-Mf+*l5~B5VFDcvKS8NKFf{aRp!K=< zg&u9xY=(nvWk5B;9~7*a6C|6HYx?f<+`&?jpXgdcrVq2R6GcV9ojccn)X263T~V{9 zAO?Jwl=IKCXPXPT0e8z##VtC1{P>9zC-^pPT0+VL3{Buz`z$xNtb@9Y-H#0*oi(wv?ru4A=i<-Lr)YO*YH9UnaQO)C z$3xGa3>+yaIMm0VRdfi^>IxYFmB;|#xzFGyysZ=!6+yJE;o;#K`gp`bu(>|Zbq$EA z&2d^WexE*l+Kq(tqp9EsLO~R8dfCv?w39bV&iG`ugY^!~jOecd&9+udOkI-e1|C^e zes*eD_3bbwWoW2E)aUl?+cnDCW&kXT)GIHRHP`na$+Jr$0RbVP?8}$K*v%)voZh;1 zOY_Hcapl<4=JE7l+gzbv_+;@eVxSY+F>- zU4W!{_;BV~K@33Kj~^oQ=ohQ!?iU^sYb@5zKp_(V-fbZx<83^Q4^|9xfbK6D2?XD& zRYIE(I4Cz7n4O&s!uke5LeU38g^&}OkDhAZFJSyE;fXiTd&KE?QF}Q6Cp!XWCbwiF z3#6a@%vupFDw@`(R$hN|^{x~oOT@J|wQ;Cfdit=to&o-sUkcf#_5S-PHtcg`ReuYl z!SLhFIX0(`7GBKJ4jpFI*|NMhtFqRCiLZK%kXc{=s4vAN-LIg5B@|V3sv5tKqv9P0 zi#CYbQTJDysI+v}r!Cqf(__D?ty<*g)nzomwIf1R5@8*h7NLw&($lqb6HsKHNR^N4 zsz#X_0nYSvL$-KLP0awxS^n+YR{|9I#c2uKAi|V^S|4ch5q(xxMrUPZHU6{SqP4Tr z6Z-=)Ba4Q>OWK=VEE(V~kz5>5Wm)!q+2Y{nXw*@8mymt?Lk|ZL9y0M*9y@48>S*de0_YR}9Gt<;}T~Ck0 z9zTAZ1*?+RcBATupB^QDE#iG4a^>8!2;F>jl}#z}ppu&IlbVjW4}7w<9K8kS`_tb1 z8rnE4JGdVg)zLk%bCHUu|K<%d6yFwMj>zDbtA)frDBaarHFu5J@(s0&{2$jxJl5EB z{mV%UdEJMCI=lb)$ZJpe2D@+l2Wa^=Zcm;x&Q-9@_M``OlQl-oyaKFScj6 z5vLW)_Q!`TExn5mCee@O79FB1eqJ3ds}+CqkB^MPbywpVsq4!xD_1pNjMm8M&}K=c zFuV~$QJ#<1vooEg3;j4?n{ptbB;sf zOZ#_x#r66t7M3>rafR1AlXeL$A-<-ct7jJ%7pH%}_ckLjJLsGkS>p!wPEk$m`VbWCV2N`zkN4;mxyXTD zb%$Kp)>h`@F!QT3s2fxY!m%RYDXMgL(~3JrAQeH|Ut_%;7rvyT+!b z0uZb;zWDLT2~lfhWDYASp@Lm&o^cqlJ17K2uXki*B8XRmBf!jRcB3&~+jVwYnh1(c zqdi8&ZZuo%`&LnWQc~>Dbatw%-(;b zX*K0xE{|FZ^!lozLhB(4Bn878#I~X%tFn=L;#uKP-kK8veff=A%dcpgG(_kR6Z`D7OPK!glM=R}V9k zL3Vuh@gqB=6Cm*o;vH`tWj6%hBDxFmLiw6)+j4A$uV7Qg(h9z(9Y9(k{DQ2UTq{&d zVPWC3^_lzcx}cPVOuXds<;#>?aq*5-@)tj~oNojfN3LF1cgQ=(hDTXkc{n9A^IB0+ zk)ki697^=SkeY9rtl&)WH7;4)#7l_Vq)B6F;(Kxh*7{mO!9AdTHX>cO_4EjVtmWq) z`f?`OJ2*Ia3Q58mMwEq%7YE{w?W07^=cWK}A_)MVnl6Q6VrNzz;Xwo%2%-?L1YLyTHF zk{(Wq`!wlmY^+=R!W7-IHjC5>X~ifjoscAHq;Kinn^(L>y?4JZ*ZCrw!FAPhBEs`g z%)-rU_$7EA(yrs5a1B@}$XOu!Hf(&JTW#V|UjrUb z(zzKc8&(ews2~5yuvwNqXLoYCYNk z%`KDP8fa%?3gpuT+pnzSkL+&k^gdrG<2QfjpRfN^Jz=R{b6Ks~QkLpVe~zkP z8m%Q#qI#S*QeQ;Poxe@j-~Y$yI&%u7Mg+nDJT0f|GdCI=8!yH_=OAG_K6c-t;_1n; zuRzl~p))W3vL1pyk=Q|%e#)^~kK)Y>La)JHLN_5NBQcp&J(SbSJiCG@)GIch+#rt7 zLlSxLrejNadHubYtv4?9VgXz0!F*RT}%j9PTzj@u=6oQ|jBdQVw&;R7$0Q(E&ygjKXtx z7JT;a-M`->BV)^h2M-p1k&~6ZdGB6)X@maM^8KRs?21Bwmf`GJb|~3E8A5=!INXxL z7d8o?@MOoV^=%6!0a`-r3rq30Zzl--4a(0=MQh8;NqC!N5T*lFt*xKNX^~vwbXrB2 zGBGg;c=*r{l0`{P4J(u`GPk7qMEwp7G7nVJtIA4Ahhv8ibJ@ypQ#)ebFV9<+)%DBW zcojD{u^1gXbO_W)Ag4{IEKlIxJ!j9Jg^%p?015*B__Myc6!?JU5GPeGUd$gl-I%)t z8e7=ZIa5;)AP2$ZZ7x$=$$~}Jq;?n@@-5+&KWmwFUR#?DmxmuA;ZNX$1oa%2K|U2Y z=jt=hsreQa&CaudRe?V~-q(&P3{>&d)YRfH$f5Kc92`-X#s^vnHS{1fw6&|t3)ZsL zVq!}nKg^>DiwKs{p*`pj;p1S-XltIGk%ptf(8OdO<?j0e+dS0tadE6IUds)g$h1ZzrKntNpA8tl8P%<|(D{3?YC>F1+|lf~;dh z>{^GxGLAtgoraM8wq6`h@qWLC+|5MG*};KNT$}}U2!tS|Is2FN`3CqyJ^uVk+A0~_ z4HG|m8Nsoke!||Sp`i79qAtF>RgPRmLaG9o>seJ5GiqRRCnK`9fNG0jwlQlPAk8-c6MlUv3zHd_ z^!c;zLV|)kfH6%#(t2ILt~s%AZN$^%|Al?1CFtAvhWrbq6!O~RuRJ}n4igpJbpYbLB++RJ0;krRKb_nes^m21^ zBM`x&-@3m_EL&^J`QT^4eG2$V87PbPdtTsiV|-0ua4D|_-Az)7_iMCau==okHRxLU z!oKX28YJtEGN&mP!^p-`+QKdZvgmQ?)Go_xSffQyZ``@ zkk^0C$JdS(Be`hk14I8Rqyf#fXIs=xCAlft>NkfxLmn@F-1+TUW9v_|<2=7G0VYpn z9R~Q?+S=6B)!o1~(ozQ=Y^A}fg&FKVW`EcuTsJiVX;et-4%~3WKl#+OLLG7+RSXte z9O|g;k5AR5-GIp~0jY5)4OhdfrVOrp@j8@iVW5S;szi8eCXB;(1hq3DD2Pr~RaGly z7gkDC@w9XpsO=Zv%CGOww(Vwx?zHbFX(3<<61V^%m)s%oNY)ZqvtELT9)!|rgm6fz z>;0j;uve|Z6==DhUb!^Zg1I3nHt2Rw6My04}*pCU#E)ciCQbZ>uz zq`Q)-q)}LI= zcVBx~*9JoOBeR%@t%i(;&8vFyWVD$?!h8oNn%|Ej9rp42Xj!fc4J9m`VEmPqR8t!5 zF+?a6|J{I~VU};%N=qD;V9xG|ZJB#1#vbCs`26`Zd3`ch$QaMjFo2&B2{dkaF1W2p z15+r4ff+zFB@vP$@&~AdSEX@epfvOiRp0qv<_N%Ynw#Gqs5)PvXbDTTf<=7^%Tk5H z+?9m1dXV83aw8B*g~N9T_e;`3P4XxaPahY;Q#rm9nM1{Y$Xt8c;@;roq;cZvJ5rv691T3um2A`%`%A;d0`DUIyHvyld4Lt9q;Jj4UYLo1vjM-p3p?c^9V0EJvCNjDXldaF>ClCm!{$aFsm|Sfff& z*zV6TM?}aZn}L`@F~Z6Dg()dZ(bw^+s_G@g-juVSB?zelX+_-_B>hrW)+;a@@IeYg zLj~~kniNgnsh#t;UY{a*oWLP7i!z*bUH3;J7#t?>wA9HgrWbk_fl&Z-Nb@epl51G(Rj9 zPZ(p@mcf5?8SF~QRp_(CCeJJfNVt+8lS(`R@R5-g z9gw12!pCo8E(o~a7fO#kP;H_C^FK&B_WPDl)V6NjDm$_m58wLr>pWLiS0aIf$a@Ko z>c0?e0x;6Y1VKUf zBl`(kj#|pe>Gj-`d1XV=%H_*R<{F=v5E8e4bk7ecr2_^aCOoSWNFIgP}kcS>DEL}aamgr>KChItS;?pFu{0x*53_^lEojYIYv3LAoJT8EWLg;_t_fH` z_&|_sdqItZZGW(B4LYBE0j{qnBqZd|OnfKKT6ba{*^59|3e66FoAvNy3*M=KTeh^K zVlS4Z03Y9upM8yI#0n&HWEUvx`184hSXkckxF_cSg(S+}myplqk2SkFeST_6%9 z287FpaSJA|Wopf?sG}9})xWOc1g3Ps;>D#vJp%wVCD6;LrHBZqnhEH_9|3dDO5GF@mio%RuaVp0zROQ)7rizT#+v@WI7k_ zfE;;7Odt7kszMBoC>-^#iG7nv7p-`C&`9m5!Q485%L+*tMW)K=TB zrASbKm(|b76X-S5Rb|9_gFO_P!Fa2MxuAhy5M25iA)!ThpYp3Qk6gI0UAGvSbs|=dtu5fuFcVL_@PR|CCKr~*-4uDl&GjEaf?%7nyKFxlb{x_#zYavEFw@? z^=bUC1u`!w@1x(|z;Ak$yS0P4Cq0ga4cS&$>GLUAs!dQX6TPVE(%7+OF{E8s>BVDf zF!<>n5#Q-xml7Xotu?U8c2 zI>LBa;mpLD#hhZre~#4C`RAMGKfUl>cg{1O0&_W$QOGibuol#EQ2tk`q&_J#0;=^; zr-Z@CL%GeB21kz_BRyY0yaG3mJ)a}CnC{ePSx8@4GB%1tG*1VMTK-IZkF1kqFgE*Sw}o&Nk(00rJ< zfH7JMTHF=n?0Zt%$!kc+jN%bkknKRtv|_Pdf!%IS+9OkAeyt&T%Lqqz1Wz>xX^8UFg zDu-~(s|Lxccx0q(Cemk|lA21I)WqQL((0TeUsR$;=7m7|L#tvNxElZ}kAi}NIGcol zPySdkq!N=h)F1H#`-$AY|1Ob@S%T(OfMuR3q`_Jij?pF?C014sDy>%Mj^#=q8g2(( z9(iyyd;967(B&C3%4^N=TLA(3oAjTS)^1a$`)7HEFg+a8g#9Fl%ys71dh`PGnwQSPC<{hf1U(3S*XK%ZA_Ogt-56a*234SLKr?Ye2vV zY_u*gU*;bZy-8o4u<-ZrlbJ9BUiI)WLPP}r^9*JDA03Uyyzhef`j~v&pV+36qsYwl zG#o0@I~2BZdjzGeG#iN|5wNT zsPcBj_-E(7RmVIDo0WFv<;)uTV7DN#UvtR{vi6eVQ)XR`_CQZ3&Ogww*FcJbmnK>T z5Geikx@6bHyByq#wsv;(6o~#d#F7Pz7HJwoS(uLufB-%(``ASo3$z8{Pr(L8H4civ zixq77b>r(D0oV_pKIyr0W*oi|7RDzKEt%7>QB;%}U!(Ug-z2wwtVEHSg>0YYkG=r$ z2sAEC{9q2}_^9f`RLItVN+hu1m;xFKsy;#1>({S$*MngVv^F3&yQ}hhpXf%>bxJl?p+72 zZ40$;a{RW!)58PhX>00%)rTYjo>kGEGBk&7)ciLV*ROzB*Z1MG&!Vh;`XvK)o2m%Z zie0R~>2Uf2Sj(>hF+e<_8VVWfUc8`34gU;Au&DVj%gTbxqrvBv0WOoyR?RE0<)ed9G3LFXxP5cd5!+LJq2w?3No$TOaUPIn|k_*-u7#Iw}@m%qT&kvPwQ1}0$!QfVHZLMbK5FaW}+8vOHcrFC#Y@qXS`fM}w zbjo@l|Hn2*{Y8sLqYr?uf7KQLWoA-?lg$lW7mO~Z0}iow8|B^gexUE2RiFdBg9KWG z;|vONVHetixFjGTP-Z9(6K}w&hmI;}S}XhTLC3&#`a;fy?{^d{L7GQF%s>OlMlcwg z^;81Tmh1&f1u-NL=VN1DouQMNbfyolVa9xJ~d%OSQ zol%uscxQoNpcC8DsQ zsmWOSvh3uKv~_AX{&STCfq$3u@leGhFgy59ArjuD2~vv^>eft&@XlZOOAY~IDC`tgwvCg<0fbuKojiYJhEDWN2M?LE!Q>e**(-3 zuwyHEj=Krv>9d=Jg;3rzCgg93zzRjDLK?D!kZolcFbKf?R!|AX7QX;;#wGqo0RSbLv{s?9R{flFb_TxB% z(e`c3o?HuG}8PazT(tx5s zNS<^yI$_HGTcn~lp`6XPz=>iSEQ(Q^PpMa7q_m>P{`!XtnFC%4>4C7vm0tU!u8vS*t;4}sEcC2VakwSS(+e9pBHoA2aQRNluDkNQBdNTka-toWwWrbgPz{q#0Brk3EwALE4bf zo5Tke1pVs8ICkJN6a%66X=!QI=tuw+!+_>@qST4Qzx&%xPl85~tLtK3`BexxNsnZ zmhnTU9zmpkVR6v+Tj38uG=~Q!=K=h$?I7>9glici4jwC+Z4>32s9o+iN>-)D`a({cy zB{}DP(n?Uop3J+5JsI)xg$u>|4D%cI!90kR5H|{a|F(3i4&H;u>4<7rv?KHY1BD*3 z&8H9U-K!PdZ7T!BxN@K9h7CJZRIVxR+cTVPvtsOej?IL-wcu;g+Z+@WC5v3ZJMe_} zz?_E8A`tpxqq&dXqgQjB?TB7^5;gMk=d}Ydq^C466Lz2<^;yh+I$d$V89&aR@csq* z0H>{x04mVouP{@LOb{3X1FYoPPfMXdA2RMcdO%}ewYA#q|1j)`MTb20HCCPJTT6iq zWw{+_RND@+C%_SQQwOa0(w8qu|5b7C{f%fywCZcv2IIrN2U}T4yD(|2fzOr`>1!u8 z09ks}4j@?4rkg*}GuOS{*O(`clSlGVo5gBG(o;lvzJ_A|8Z)mS>BJ)n7F;ju4n9o$ zMubi!6&2&Yh8$7{o_zbj8!hvOD61*;_@nz)%f;9pdlyJJQm@#xDg_I_fj#M}iSilB zkKM2m&1}SihTYc+eU0?{qktei9Q+aztXyvtOd6h`%NbBy8GJ>!ZUX7BBW*k7w&?xB zP!XjN=Qj|h8;soIiACt!k|T$tO##@wIAT%YLC?h10{#5*qeqWeweB4)zn{0O`_8d1 zTM<7aZpA8H8lYwA)&S8w`9?En2fhIIHwKr9;^sX0j_7lzd68gAGgR zMU(3xm3+o*+TW}!V~bs?eN03rsOULbv?~?S*3fC((HK_DcV^eitD@`>NlYq;%2)oyPspONyc8x%{P`~@4LUFx z(y7OCG!2k2*?x^K%v*Vm9*-#DEdFi6{M=4xnV^LaX{EydcKy%s;t}z__+7RhI+pLt z_O2RA`hyN%6-Njvuf(_! zLsRAAKPgPD;&m7-gJCkr>z>o5Jot04#4LXgmSY{skU@+OX*a3UeV=ec+MNg0g9>QQ%Y|Gf_w6l^J;`0oeH)Q`?^TMs;` zfE|i4TV&rPyf-HXAY#+Sy12rv7@YPWK44Is5+c<8|2$aN|9!Cjp6~r@EYBO3>xCP9 z>Ll(Ioeg*qbwEA1q4@=b519?unnSIq_>BurI4sKcg=%9sGPKpbjhWI+vnHkef7Bb> z+1*V4o|fVlI)W?zt*6?{YlfGxSmIy&Mqa*vRV&bjGw1)EgDH0||I=X77SpEe`}@IM zhwbM(GB8|<)=nn;d=5Ih4jn?PFDrTaKTqX74wuGBB!jdEEP|Q6?2hfee#M&PX45Wkr_nTI*a4p*X zfl$ytoLdt;r{G`h>gr-Wej5ku<>$Nl{WLW-rJ+U~rFpXR8O?$NLVE{7^kJE){t71~ zJ>?bB->!f12xahFlOGru5Ia3KFi?zh57aKcK|UvSW8(C)h}FzTo3b5?vsL#5;wj6O za3&>)vAFkQ3)41Iyk$tw;$XlX#B1|3KRA}Dyl{Xy~iU!}z4 z&bwc~TwgMh&9pE1@i{*JSFc|yA}Q{B5ZWDbj;5=p2WJ{ELdaf`9y3R21&wq4W~(^U z!Kwste(&mfC7S1LwK9Y!OR=oD*U+hfKH7dvN~~^;t8(SfbxT2al5@w14fg);-W3S$ zr{^?Fukt&ed1&Ul{i98F%Y`{9&6G?78m(I)YooP_9^@f8C>1%EJx~0b5vVghS=szg zamGRlxcSR->f)B$@i>>etyTu^6X>|ejANKs0#-=W|ItOb{Li5HU-tQp(-3ZnCkci_ zg8V{4IJIK5&0rW~JohF2GJLxr&Z_|!a3Dnk-#Q%224^!4T)kXSAP4WPR-#G&|J0^a zG104|hQk@i@v$higQU*8KecatMu$^AmJ;jF@8ch>(5PdY>tzTB*}%xi)ug12I7Vgw z=j^SlTmv^NdRW~+^8%QPT^zS0eWiRIuo&!w)ik{Nw0fT1Ikc_E9Bn$T4BxXC7*PhC zG(;{vJux~6XW9zi)fMLqFS*h^9G``iK;gc8~pi>Hplk=ING#&vU zT*t6v73sg;0~q}mMTmYSYe=G>)6H%}Co9K74hjKn>k}Fp3J1PEs$|oWD|CPKAhl1p z2O;Ufr z#d2}ZqIW+n@;g7Y9@b&Z}5NdHeaX2h=O@{kN@w6>!{<*1+J=Bk_{3R6T~A&$@iM z0FDIET(kr0aO}lOi&r=S$FZv0sldQiCiLRe;7he5xEnE-pwU=aX)Wv})GPQIf44bCWug~@rHcwm;k zzCI$HlCwulop2lk0u0l~M{uSiYB6PAhFXtrCY8cDjh<^0$ct)~P=V+W+62}?CxPVl z67O6waZdL%j4z}=&$6v_Ayo{5HU(!p$qrX;rVwDmW$PSuW-b90&c{(DDhM5^EJ`Q= zA@-X`B*rg++7Ih^1pN5}!^86^lzliMnTr^BQth7%gZXC7+EblASq6jFS8!&ggLXne z*ov!hE(6wvkkpO(upgl~78b^vohWXI*UpzmjtffaOL%&zsk9~iWA)HS7C|a>?}j%X zCz83u_MUh`4gn>l4)GMAASO-$QY<05P{rpA7q#cS?O(37JkwjBnNBrm7lcI!SLDUC zVvKcB1I`sd%F2q7)gdz6X^&CNp-R38j{@a}jmMAUl(F3e;1WCA_M`GSYCNyy!n!RG z&vt+U{iE9^0&esMdhNKu1z{}M`cOzBJ~jHozWuJ-c3pYAIVb3Isk5KlFusxhrna>8 zAMKP0eNuW(Q?l1Z9MlH_+?Jp9aoRYk;J@n^ajy^ab#!t#*gEav z4A(vMRiGtY8uKw zPu~X4hF*adq6~V`we8|jF4GThxZY|vkDN>epBm~~14rTnBn7a z)Nxc21w}|2Tef7P0=8?gHH$M9Po7+du?Px*$&4HHH&h-SV{)P-cF$gK@9WohB6Ja* zPQuwWs05U0nC{9xp^YLUVRheWELVo?bbK-_G(CBuCr>?oF81RH1lT;aNjU!YDxeJ> zL(8!>K~1odDJOg=C;g`a0?Lkr-}QDxg9rqQV9jU}} zJqjKWFhIjCrIG+)kQUModV(p0K9`n1Kz`=D^KQ?*_qpHi{k&|lXEJF+9*4d$KR;Fv zA23R0oCzfh9bLOqI%3N;jjDtR?(U(U6DG80+fLVG9GH))?}7pDEBHoki*T%R0J4rwfWR@*xg<-4(n=_SYtizBR)3B7dy0`@L61j1ZMYpVK_wZ_ZqCH2R zSsQ&Sd5>fs1P6mtvSr)F+S-Eb!M1fEG_A5Sx@yNtdYXdkEYHyVLUY-wQ;Z(fW%k`y zixcm|+}vZMoIH|AQLXLmZkU1`?Ci#fC1`p0*5O$WYHV;X>yn#F$HUSqz!xWZjh6F# ztyXJuhfFX64%1szH*dazN2MKD(spr>RNVu@dk*S3Hp6!ttCvK0;UBlawr8^ z;8+`*(=dZ#HMRBjZ5M2c*C38Ewsbo15?QM#v%gQbbg36h#0S?svmV}gQ{Mkc z?{7A35vm8bz(9W7ZeI174Gz2n!C7)0BR7PKhYw7{$-oAeM%9H=1Z{_I3+~JCg09$( zfWDqWe`Zre7h`C?fEQ@n_kT^C<54j6jPF=!Gk9!jc;v4JJQa>3z&oWic;2CXcJ^Y} zBlctO$lJ7O^78NlC-FZUBa3xy&knVRJ#WECsVAKi^!jSv!?{r6c$~XlaFebWgyH%zYxEVIv-eC5EB0lJ| z8!~|cP~*wFqfhuCor`)vaVdjUFbH0Wo~Vd@nuy=S(`Ta0nn#PT=}1-g8Qc>T1m)<^ z^n6qCo?-u16epjB18Okm9EhYSrs?`Nkdo3Zrbi_Q7V0xK$RMVX=z2X56%|X8h=#hp zUbCu1%@LUOUuPT8Ye$hIZR4}|&V2-xOlY0lYrzF?^NiLejS|8X@i#ZLkYX52g-8_z2k&G$*XH=J@LH(K`g?OIlmff%~nHVuEu@;ddBc0K&6i3 z+XDuy;Wgx^-~-xPY9vYvhS!xER@~MG7|l^i4Mkjt>641aTPK!`#q_foW&(v}0rtKy zswmjL5faP*l6X61m2qHs#~H!Y(>1F_daJIlj}ZHq+ae57*RU;_L9m*ziFMIdhexG{7t!m+k^p&SmgFQBN|2^#{f?G0SKUzC1A+3-w> z+CTLu=4dJQ!z~qrlAP4VW@ZCsQ^E3=1zyx0EZS81RBZ2 zMXGMKut+r;;Zg%#+LeL0x{M-3E(HdS7Pv1fkq;O+vY#{KTrsgTaBy@S3-Z?M##=dg z9*&G5Mb3h|XTsL!w|L3*gB!%BqUn0qs)eHnJE2j${igC22$#)_sk*?A3Y&Qd;J;3v>^9c!t;yDg2BXTdvc?LIQA%J8} z*X8F?2Ge0*o{8wII|D?$Li$BWG_fDP7i--1N=pZ4B9LY(izzN3O4nuP9 zq}18i+}H#dI*8KMO_e{A#@1u8$WB=P(-jL^x0M)CZ@0Hc6?dZ&q)J&PQ&i|SY#ZM^ z+KKwE!?q(O2LyxopqE}qL$=y4ubf_&K*c@ze0B#8=Pg?s&4rGv4?M3+DADLTsm#vgx_Nh zXah?cEVH!6nx`AuQ8a&Dd;jIbk==WiCJKfPw~CPZWiA$Kys_fMa@>8Yb+OSS`P>Ds z@`hLA(yYYqiuLVFh9{MBW?O}GCmN4jX-0-W4V3jc%1Xf*dGxlmcYw1RHRSE zYb*K{TqQMyyUZ7}$+-RV&-sjIe+ufTGc?EexO{=e_8Zu#ihlFv{l`Zf%Pb`kjqq0K^+tqtM`|EcUA|G!=w5pB8DU-d2+ZIT@oWz`W-u7ODZ$>^psPj zm;Y9n$L>``c}(zLC}t{3m9tm7-1e_u%Pi|OKXQaf&usZGpfr3*JeL;zb3gQ6W~7+C zX&a$ad=z3r!^Nm5^05~Y^F(IndFbKEu-lcX307T(Q5?7`a=6pv7?JFuX|g~U zAJ51)>RO)73VtwSxL4fAcsLk{R Date: Mon, 4 Dec 2023 20:16:31 -0800 Subject: [PATCH 23/40] remove accidentally committed pngs --- .gitignore | 5 ++++- benchmarks/trajopt/samples_vs_runtime.png | Bin 22628 -> 0 bytes benchmarks/trajopt/samples_vs_throughput.png | Bin 27312 -> 0 bytes 3 files changed, 4 insertions(+), 1 deletion(-) delete mode 100644 benchmarks/trajopt/samples_vs_runtime.png delete mode 100644 benchmarks/trajopt/samples_vs_throughput.png diff --git a/.gitignore b/.gitignore index d5b956c4..ffe707c6 100644 --- a/.gitignore +++ b/.gitignore @@ -164,4 +164,7 @@ cython_debug/ # mujoco MUJOCO_LOG.TXT -mujoco \ No newline at end of file +mujoco + +# media +*.png \ No newline at end of file diff --git a/benchmarks/trajopt/samples_vs_runtime.png b/benchmarks/trajopt/samples_vs_runtime.png deleted file mode 100644 index d457e27b4bce89691419a023981f46c7f285405f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 22628 zcmeIacUV=~mOXmFfTDty7?5N@1w;@Kk!(f~0VQV@$&x`Nqn1)Cpke?-f`H^8IR~Xg z0g;@OO3oR{ys@l%Z{O~|FLb~D{qe&0m6iw2IeYIF<{V?pG1mhHIjK#wyJ#sC$|mXa zXD(AH)b11t)wgwP@e{$S)-L={*!HZNt)hj2t^HMNeagkFww5<6Y;PD}+kI2t+Q!(z zT!7~|4w>oaC7`|k^<#}}*3WfeE`9l>Y8EH(Rs8&gz zIjMB(^>Ca0EhXh2E8`XHPh>fy&!3_dXXxMc@b}%cJnwgNJbI){b@Jivs3|oLYAVgm zJ9eBc4UO%S*?N}Y&np~vWWv{+5xK^(S2}9_??J)^lC%0R_q<^buUMECn=E|oG?LZZ zF53M}^Q)RC-#OfdSg{^)Gyr^gj(Gp|pMw&y9wYY2Mqr*{sQ-C}VY|F~mK+lv;T=g-4G zvAJpG=jY>NulV(4PTF(IU!TrCw_Nh5!k&S3b>ZIYGeb@I<%~z2Nu9QR+uM_O*K%@l zMu+719xi)%_>k@QuA!!shYufKzPomlQnsz}-26NjJNuc_9&~lOZcECQZHFfKbqZ8s zm8HCyOOEE*4c41fhMc6DitA&PaL#|B4r%2Hx;nyGzJc5cFk0z8bUb9zor z&R{~$NOh)Fr|tI-_r!FUr}}FIoo0r_bm?e)f5W_9?}$yzkNNiPigu>uNy(+DX9i#V z-sqRTG+vk)=2DJP%Ae`JO~YC_H`&|W)AKTZ?&V?42;-UveMahvV9CVa_D}qL>o7N= zFFsbdsyWXo!*P1x?fbhlgLt6kBt4IwuYs|jSe<0IaVdRnkZ6m()99Vi`QziGSI4bF zBjf@#Dz!P2RpK??;@(FcF`^b4iO2I39v!l`w|ANS?%@;u{5gZm(sYC4a9TsE+lp(s zzraCxUvZat+qtN(5zX>r#4Yn5<@r;Xy0Ddpz9WI z-j=N|EU%&QO2(HbaL~od%IayINqOBdo1DCsq+8?H%$kzV+Vp&>dL}Q?e`NT>y$#v6 z{g?B9_RDSi^JXr~o;`c!zul!jYEsJ)xq&TfT0&M%?qx>C&%=Y;J~h}`DTkg}KiyYV zw7fXi8?5?h`{AP&?P7V8Uqxxxt^52_qWfd7C@sAl)s9te3oRY&((Bi+_eltn<4}z7 zoEhy%oEdJ3z0s6B(4LpWUSg19T>6|dMzI_n)>LKLQ0^BUFn;PdY(FNOLmwI z>9;a3s_)WEM6rn8ysk0hm9RyeP9&05l#v9j;n6Jyjz=_~(PIbdz3ek~-Kd zPCrMKAG3;gb$7p==q@uIYE0rU?i>4FG{4ZX!h`Qvvv#e?L{|yjq~VR0H5;}YP4!jL zZrrHUU;VZ&Nl%JJ!g==PpzV^NJLA&a($9{%9D54~C#SNqvI{QrQ&m|uz05u0Zp*p% zH!|*(l$3mNSW^Y#Q2TVBRB~>D<DZ7t5yFvvjPSwL3-(8zU|T zFl^bf2Y2Xj>lVlH<7)EQR~K7cyPirZW{cH}A%_t4q|?WpkOT3N$-%or&t*&d;+?eojxP`D$lb zpWU)$%f!#04U#K!@^&Mw>ON$F=1o;!$R5pKn8Koqkq?%5C1B{ickf?Uf1`$JBF+%oIOS+kI(zUaB)AgD$>M0RA&f{m}wR4WuEL!A_-_Ku~ z(e&EMck1;0jhAlRi2G1fWQPC{Ff(~B=+=+*fx^yN)M@6%MZY8O-^d|< z2-*!OrCD_7SmaH=6cZDxeRJA_yCqgLBf)WI$f2c-o{mnXB1nAtJq=seyr9usQoJfc zBMs}V6VcJpNDtKk7WvUNk@ABUZVLg&ZZtlLE?-$FSgFCjnCc0z81Od?%DUR|%*UrT z^z4Siy7^idu-x`rotsE##{x+$aAmvojcUQ^EjF@{%jo%!lXfnm!B$q!6=~?0pRm6;q{{QjY$L0fTEX&sY-nicRI5#eaf#3VT}O?c zxXeH2R8)$-yuU75@zb)SWA_VH^X4AR%+%+7k2#JWReE;dq7RZ+_RZlKGU1pnlaBnn z19GpUQd5tLFVA+0*z|b1w1WH?k{Z9 zbNuQ@11`np4Q?Ijq+ctf6|K+8Hma+t`tZRr+*SWVp@<)!%Tda*%EG{bAS7xS( z#r*8mD-+(8#mNdU_DPZ?%8;)0V^rb?DkBFo^v<~jxy~DVI6fAi@f*jUm+Z)ONO^kw z{wAg#g+#=C|K=}habZ?%v+6+!z^T;b6iD4z82h$Sfg0r>asBT`|qpw zl{hbsfAVi8L3cVmz3D~-^8A6ldp~@ib{=UJ-pb5;qdDCyAX>gH+b*X&ov+yMYSDd0 z=^kLIR9|eM;?*=X0T>7WsVQ5JX}05*-wwBMn3zjhBjnyr1eL4DlP_n}- zYhL2TbpRVv-WC9!r`O3m@)j&!w|ggEva4h+N~-<9qR z%Xe95F3ZcyQ_i_*=AkIgh?o|gtY6j~EQ#GpOG{gg7v`A~LX$%&6(V8M?71eXh{6h89O;Hxjgi zoF}9Mk6V3M7P)ZYf?~iiv(%fz8cjEjc^pgZXctPp(Zr90_vQOhD}R3d+5$H>mgANu zMD0g%^Sms*e#~U&w^h9JtO&wLio9;h+y-DGzBpDikB!0N?Z~6{?nHTjkSfw9bMwml zV4}eA=xZ5VhRhqLc;RUcV39nKxt+J9=>BN8m1Vlg4!4yh0y%(vZCVX*)gh_V`=f6T zH&4vWs9=dBvzYK!%(@jX3l;Yvvet$@mf+I!ERyj2;|~QH8JU-ff?b=auWUIA)b_Es z*aVrfIm;#zLq~u1%$ei0oR>m>$G+}TNVn*acpD!6xuc_F`Q)?l<4!YiKF^-*FIj%C z5UX<7W3J4X*R4!(3u9!Mjm`1Lo~H5Jxz(=lYQDR2`_!pZwhh|$(mrJQGwK~h`jj0+ z=-4kkGo7=AmzVeAtE18g-Icdj(M$!eEJ_iGurOMX*r4N7t~1(I@@$ugZ4_{6*Z1#l zfbI?f*;Lo%{7mp2e`_^t4Me!OxXAxP*Nx@ba|cOmt-3h%8dpb-oP<&NVu^CK8H`Q$ zj~~}`BR7)CshTjq{>6_k8^HB&;+iksx$X5>jL+Ed&uZhHC#K|rs0`>TqFA00K7HJor zRY)-?XXZKg^&zsCmKO75Ub(@uf`XMz9t<3a4++zJLRmREEs3205-!<3K0Z^E6_SJH zLLI3MC9~gLz<>nqec#x${s#q&#N12s8~JE{iA6S!DVb?Ncc)D%;?e?yCi z-U>iW3+?*#CCPGFugpOm{X1@%a}E3ov1L1J7kqVHt9SoT{GJE9|E3@{PQ zI6cbPO=_c__KWi&4g|0=7Q6uc=$J7a55!JX?)@4#-R-L@va#dEce!@S&l27gk*B~w z#)a-w552wXF*t+^MPD!RAsKBuk5-g@hl?ukXJ)A{Z{iMJ=Sze`fHcfM2TTZDxRt}Sf{F6 z2L-MUTme#{GI-T1M0t|7Ew@$#<_kHyxYR|;vt109KqMNzj6e>yEh)Y*ltPji;BYM2 zA;{nb-G-Cp1o#OUe5o8vEJ%1IXj+PlFOh%4JOHpB%p<|`TIu%{w8u|i@&w%9LV?TP*Y-EOiy zKr}#kAIR1W*2P9%3^-=|;rI2g#GTURULW7%=H@17Qv3OPs)`DkXhE1ofm-pJw%HBTU*Z zzH7fDD8_8q!P5s8RgWZTKIr26;>T=ueqhBrvd4kUiN1gYPoyy1hVj?e!Wnjo-gPF* z01twOUzx!%3?1|Ah8iys%q7L<5)P%gtfE3bUL&pg_w`$cge1CNe*S#E{I#$m5ey6} zg5nK>9L_;OoCkLa^YhzD^1(wdFJ&YH>s(K(&f zP;GdH$?_2QDkMnUX&|v++1e-!nk$4^ zy2lT;9_%TZ$DA{4zqgk{=-1PyPf0n%**O=Km=y$wLs(e2Y#!uK5qq>Y!$OzjLulid zf!b1C7H&#cLj(gYO#}+$l914W4quH6A!mJ(HOqq>DB?I3wZwO8?3+`IS3ef)3#{^Q zFAJ&HKGLH4)1J(Wc>V&zOyYYcrh#%#_cj~V^Ms&>!vKAx(m~=fuq@&B!4fX)(&A1t zN*^C?+s49jSekHMQWPM%CJJ#RnjbT34p&bxkj~z<%<8si_3_guQ-E}^{tR8VzRu2l zkO@D2{AdhR;yCu6Mmhb4I(a!e_MPM)tSr?dOn9E%Yzz-6jc+gPfFdi4iBJTH#mZvX zx>c{Bs6imr?x z8;uO}!x*vhU`btMkp>XwZOqJZ--|p%wK{1i6b0sO53MMVxQe$>Q$qT_V>N26Lf~{? zJp2ELv;X0c{y!8PJAuGRRS;>adw4h=>v{la1Pq+4>|Vqg{P@w4c9x839$Txr|iEo;=wM@FGyAf1@>%!2TKtK+11rwqbu?0MQ_TtS;49 zQTj3z0#d@1Tv=2^p*T~&Kv7Y#8@LI@U*PPX>vhQE5}p687&>z-H!pAaySxvAEySl# z@CU=DV(gC>uU-v|VK6yNXvHY3lCE0){rxD}T5`~@Tcc)nJEWpyYn#e$ zjN%Uh=Ekv_P-`hN|O@h+D*H{5zjw={CF1i$9gPk-=Bxb zRo!Vn9(O;xt#cg}MPX`eNqb!a6%gNv<-PxmMmZUTZet3&(rp8(?*INwM#ARgvVOeD z_Mv^MJ1(5Gk5$->ce`IZXs~eUqFS<|lp;@K*`3_DhmNU18cLkn?&61-kFUYi9=`A! zmYQxg9H+wy{Y{alFRS9mwj6;ZcSn_5n}LE)Bt113c=hgjWb&>~p0FD~>~khlF5LT# zYBSOz6Vq>U1|k#ohxc)EPG4KLi+VzC(f!)Kxi4NtY|9=LEXIeMR+8)QjGTIUg2efc z=Tfe)t#MGd3wy7_?L#fr_D^4yz_b0VxQ4tB|Gh%b-hFHd&k|j@JlDBn>#G?z>^?8x zcvL0Cx3mA-3Eo(PDWkabY3dD8hWBVzom_=WMrz;Poff~tubcNa!1Q+8pR~-sZt()X z^HGHZfu!Om{UInsjiFZbAgP@R`olWt&wHTFBm>3WM zCZ?vi`$q7-mcpO*$KefsID^9)$O!&a-1$gS)XRtyR76-+mficJ?ImT zU>9rC`xO+d0Bltv&Ad${V;)y@*LvBx&z;CQ0O0J>s9@wlf;c3dtPwBbw(LAIMTalT z;`t;+K`A^I)t+d$jGA-o6A`ORg|G#2a&yZPaU9yQaY3g& zr}rmxbXBI39v+5n#y@aCLPhLLO`jwWuQJ_L;sZ62y{W z5O;R7+4`B~``g@<(yeL(isx9enkP~`6a^NSvD>Tg7TAq2m?a;vcu>?gw?D%NxZ2mnVF^# zuDOq%m7N#nz{^2lX9dS3&uw9Mxz2OEjE7nMpb*orz%fROi#b~c&!SPB#>5>ZD$IT zkjEz|+5yA)cvOb{!gTKM-@jMC9|O6e4xmEQj7YryWq#%qh_XHLx?umK={hEv|gWfk$(8cADLH|0{oF$uoCv=0?T2nqL2`TEeQ<& zJn?jaysufk`t!AUrLs#y+QGlcCQN>P5TS&;cz3mENFV%;cEg6U zOKkktTkDt^7;L46u>}|musl&-4s)IFKiFndA#UIL0E!u~r2;R3d@4)gKhN@XLFO-lNp{H`TT~>1}_ch@Wbo z`77Xdyx&sz`|rQE4k1HPCZsW*7!{ioHoW?Lu!+{a!+Fw+$o4O|)i2}U?&rth=C;C}NR=v7D(H?C_~gfeXn+*O?3m`u6@2lDs;8Y+6u9yf zwR3OrDb4|$P9Yz(Uy8(Kp4eN9ZS;g@0h10FBy5ifYz5TYgB1($O$t927U;Yw2A)6d z^9W&qRl~lP6Cdjeyg|RJ2Ac{Y6#rpCo!c+7nD_wxmHq7OQBeG|nx)D6f^N_hqS7So zlWyJ?1I_&fry|PZ<>eRgpo*AQoA|qU+h^@2I`>PLF8PE5TSudqjkKKA{?~<=SAIL6 zsFMfJ#Gnv_v6^>BAaG`-1QCcJ-*uca(vl$$&tWQ}dapu|mCYXltpsf`AKq3?+%3eRf6t>{E^~4W-S`C7lXnv_@V49 zHWdf8H3J33XaD|PC-6lKn}qW>Wa8RH(tj4Jx$dL&AR|@CAD^u#t@$oFyM)ZSr3qFm zN;P%+jgIor2Sh{gYKa`n#jGp58zz#u2C5uQ8g@rfoYqpZK3z*-`?MzGFmeEvvy8O# z)5u7}llC-)EDxJpl`34eCk{$72rP#OR)z8H+03zacKPdFU1}=#P>wc(qgPA5%7VS* z-T2@WD3dWRB;X$U7?8;482fA6>fKma?mMuHCMk$CRo5PTuwA2Fk>0kP7y! zY&C{yBXQ|c25NyIDRGU*;}j}}4bbCfSDFRaReQ2}ZR_j4^-8F+taS4c5G-c;)te|E zFXgN~X`;#d1b3|vw*}1_BLAQ!loLVU8IE9C{;R$Q|5IBjsA@%E7E4UhxL{0VI(8$| zcnPXN4E723qzR0Bh=e^yTu@#iHp;HS!9$NehGO-CVFP*XI&K+4N-hzyjKnl6EiIju zrGcrxC->h}aoThaf`%W~Os|FOz^h@yIyi_(r{IS!j{l}| zQ}bp7Rv-oliMSXCJNutHK|VgA#McdBl2nQIOXt{+{&F|%*bxaQZbxzl^k3*9mt|xg zLbC5E9tVr|rs(_wKxO8A0qusn;Nhc3W!6C`Hhawbz%gxAG_T$ShJ~WRP$fp! zP6;@TDcxrL$646KL`X}_!&lVw?ywN3Iyz>C*C|W7kJp>c1qVmJT!CZ;B)gs zW3jRnN(1Lu@SVwDP{%YP$Ks!*>tEM_FE*T*J($uwqN!2u-I)cgpL^zzEPx)eu>zxyw|v^6qBxPq-+ zA-$Q(Qu)hFnBz#wEswE-(hoRyOhGun22FK$TuaH+A=-BMzeGRL#Gn+8a!Aazj}Kuz zc*(wrMFiEGv}%Zqq*zU4W1zz7IQ5hvvmm$x!ON;qiiUoZ`7D8TZo|fn&r#BtpBawD z1?FaFKiAYK!mFpErq)Qrns08^X6LrP98yv_=;X!zD17~en=xj2C$Lo~vg|4Dga0>$ zoV>3>9nO06Pkd_AOjV^XD=9rMe$2)zJ>O8UydSP3va3i<1Z+P-J~)WpbRz@rKeUm* z1)u15yYlB6_$Vh{{zKkTb;+Cl9_m|f;sjnX5>{9>7g zO&TjQ2jWF*lEHfk+KJ?WbE-epZKut@;pJS@VM^g9$TY2dsyiNqvc?~j_CIx{crr?A z&} zjGFRU5Fe&}@Gpd>lqDmo>5UYH*3FdLr*{7e<#PVq5&F-GX4Rh_X96rFzWPMS=Ef(M z{54MtOc|x2WEq|QP_j74KnZDt-gNK8gg=u+)wQJrd#hgDAN>0Ln$mL zUmNoGSbl}~pJTa#>=yj8uI(<3r z4$Y$IMCFM+)d?q`G}mwF@;!lE{E&ii__uIQdB`V6g-m;#(VG&gCY`}37femDM228s z;%j+qf-FT>5VfUCU%57HI`cLZGgC|2ogYvAwY6qtB_m(@S)U|14pLOUz z7KUYAdY$iZ*k74WpD3lhrzvNiX6*TXJz}3*1HX@VgjOZ=z_|wCG+I9D-4AoS1cd0^ zgrjKyxTWyc-hT|wU#}au8>LR_)XNsU33wkGYrjfp!%RtqIiyFUEtE@L|0>rZ&#iOk z*!~}KT@0i6E&;mT2wPWn8!<2q?^+#=4d}XlHN{!Jg1H7tl=p}c5GK-H$8M0`7hzeUV!}VcSbbn9 zB*qRnUp+(slAnFNW_3&U^>Mz2hWM!S8H^u?CEp}wuwH6i-YS#fe`n-30LWGQX|q@P z5K9_VY?GcZ{^U}yEFjIi0jUF=msDJWs2`0YA#r2vdn^_Tj)QxULt#V}42&JH>tZ$3 zs7NBpK?K<)dNU3ZQL0U^9F!C%&BXlqm>`E9#)V0gx_g#rcl?GmvT6On7tTF*=$IuV z9hm4i`KIr)O%US|tYu2U1TC3JMZ72@Co2hGYyD@qkh3J*N7k#T7Th zDI?i+tDz;Iom&viV3_6n2FPj*)ebiG4BxEGOb#(IEmWt~zi!>JqgO%K2hTq=4bcfX zw@={{B+WN(-uO4ah9!x3n?Qc@;3*}R-c1XRe^1-AVR3TZI+`;h4_%PFW&`d^N|(-- zUBO0tO}Ca3vXgU6vG6~n^i=ebara8g%6>_9MS0Z4@m!PF!1?MYdrlKY4+-d(4F?5h zP_!7J;)7+~=cJ@eVLfWneDD0S?y30b<^B7mBP+DO-gG)wUG;oJ^ApR&M{(q(h?gQT!c5Vc5l0j}vreCE9_0?{mR=T>5Ng4*dL|D2M zu#c6YkKLzdOJIeW7sh}!$t>pkJ8dnll{)0j3@6A&I@)L!YZqdFJd1tQ9bt!>N7W?` z>MQ?RIyvC|H|d1t5ZYwkV8fa~u-hf>mPbOM(;*E&^gzE z1H}?+Ltx1!9=7q$PlM<|rKO{*hf$U#Vf>Kn#H6jo{S(C~^@ON#F?3S4D?12#R}Qzf zK5d~EZ>x(}-=;iy={IqRE&2ewKUwF#W-s#cLhtWdqQirV+2O?!mvwIkF*ji*GtN=I=s7st zl{)vuX)@`)RvA8E&O~*DzI%ETYwKq)N$WclKPl^lweI&rjWHd!WF<98)B1|xiii( z*kL>Wr`yr4Y}}oK(PoOx$ri`kubRS=Frh4^tHj#`pMBTH74{Te+?^HvR|P8y;hV7? zW58g7C+!-z_FYaFpYF4w-HA$22`R~uSw%JD|8Xy5->i54zgQbE_pCy{I0FL%J6hym zQ#6IGhR%+dOkoiy1N4VC2`zSsUxUO01_vR$q2~RNlKEGz<$_o_K0cn^8{|ZCZC6(p z_h6!Ch8)aCiuj&1{j%*ON`T!}qxvyy2eWqg&vqwyX!Ny+V#;WLhu^CYWOhb`NaU#E z5E&l|oe8vk;v)=7=hpW%d9yzr{c19YByEH)eUzVL(X{gx9b?82Q%UKYo%6tf zbHgne4QLgKrrpNX`X5QIo+1{aSnA;1bOmEhT?Zv zCs)mfn2rDZ^9;tfCDI9L8V`a@OKt!yE1p(luT#3S*5l_f{>u`IbaatD~||xHAls+FIYO^HKRW%?b49WMyY_adMWt5rY*R z7BHZSojY&U%rmYW*uS5XgX8lXIoR3rzD^7IBU7BkrET`_cxc{G%>$C>aW_#sI$mu}fBaCdj4yiOm$_Lj&!5FrKZFWD5 zfR&Y%TAGL)?jIrwA5&y5!)im0Hh?u5&A+`s-be{{h>i~nPHmy&*!G9Q!)y#-EF&wcgyDi!Wk|@1S;Y1{56_CIS`%7- z%L7GK^@=^KP+&tF(P6UMHtjn4uE7D_J1=0+L8Bkzs09fDIB%~Kn}yH6w$a@ z(p>@-db-_dw5=*TGbiT|wscMOwJx@qlkfG|80dyP?tGHqxHWV)j+n|25udoxF8)$8 z|91`u=?VQ$h@Uq5!dVrVM~H+=+W423m&qt#2X-p-{wL8)xQ6l_k_3?@lu*?|xoi;i zgld#Qm74gJ?Kt5hT4k|Sc@QHk`Q!%J}5+K4GuF+ zh8MAc%K;h7u}WnD;xEDlJ5-a1j$o9}uYf_70}6csS@8@G4rT;;&ijf+j;mDK!kDlzlXsovO3({>_gwy{R999O^;nD_J)+m|rMW8O$13zuq zq}eanLQI_o-wJQf^uhxcfem>;L?oNyr0mR&-fwAmCx` zl0QFwWYkM=b)*%5}D`JXoatFgNX%+2sZTh>SfrEFs-DP{O4d`!B#@vWHR6vkC2>Pp=;;UEK3?-?7DnCA09`H-2$(Z2K1o9;r1Z{kZ4*XW^9U&coo$x0r)%xQ0K4>`fBs25pgevxrRgY4+{C#tqw56j z`5g6Dmb)M&dVgcqwi*+;)u>8mnT0-vo0|WfDcte zC7&ke9ejSWhjbRPFIh0P52YOBgeSH6R+ZIZZ$A_Ms8dtO|EZ@j{T!9o{F}F&U+E!f1zV)bPZ)0K_9NYTvvm{b9 zVt5@+ir|zco)+w-1eg+tk8e$@Xzv)hx=hg>LYfT+8WIyxr9c7WD&P?H0_E&3DT>W) z#0D*GZGpBWKQw|B78a7RIbu|~8|QK;#i}N%K$9Qp#u~+<_Q9qYR#<5^V5DGsJV|a+yy{qfGpn0nrDvk<39I2>Jac))2@RjWiD292}%xZ^J>3!GL>^ zz2B{6%ph1C7l4`drk`tf={5u$e$9mvb#m0mOqe@qE>M&ge+wsN%j1VB^b@+S=95fy zoOBDnQ~WkS|Nj^A`nD?-4|()FiR%xtjSM{Jmn|*d6-|*=K~g1easEqYf8rA0aqnI~ zf`iTM=0Ci=UPMMlE?p4h=Kk`AtYjgQAm_0;Q|$&-7Q&F$h(~v61zE-pN`n5bWg{H( zLF#?%oam0mL(sMGV!;eHB-%I9{AY}z;1mcCH$2Ngt*>19}iZ?&o|n#>#!O8 za65*=t4m;`xehF>kF8p~nO(?gSi7+J9sNWzXumAx@(Yti?o~i|LtqCzl0zgGh6J1f z;U(Jz-`XU$UjVFG{v?2_(|J^3S*PI0ok?s-f5boGjUHC>Q^q9ZlP>=h^l%8kbf$>( z9?5d23$BxCe$+lw=~tz|snprqYpCNor|-lvb8UBd#Ag)@Ytpv#roT*>VE(33hJ z58HhWmb3_RTXs-P&=My1F^QHxe{`OSq}t$Qlu6(WCh(zOw*uy1gT4q45p0NKAQiruh@^i#BCbXxrdWam9 zdwm4_NB~|uKI|Vn(4 zoe6fgxsF5kHjg)_8)>pEu%FajX{HA1%62|0Xu+Wdl0x=vcD@8#!2T=*)0A0IyuEAk zPMdPe9uRSk88mhM*C;Im7a~VJWJXYgo5ed}6xT%$CAxXgHD-F!zCV2l!-bdru8E8k z83tP#O30B%4<1y(!#<79{wQGSiOI=BeLPf!Ec;MwnL+=Gev(?(51*xHtZ>?>LN+7T zO&al4?swjEUZMD>qBjq%D^LYnFfYba{3t1d>zbZW;a)d*o z38-VN+Kq-V{ zw$iQa?S-HS1BOlFVFS=_`3mhZeq^V^pVSim_N~4Kjv`pBAzveg^KA@pBou*w{7`2P zaB-FH)cXRPa5*_`2V3SkiHTSeDXlsLCLu}*!p$XsWhaC>Iv?hfuXv{oh`fM#>+uxy zdQHMx+}L3F{?3}2IVYSYgVsaSwRFrzH%HsemJw!lN|S?aa?!+Zfbidw)}SMR{eP4& z<3<{I@c4C|$L|sR*58mA%!$7HUuVNe9Jd>I4zn zu^up)zGv1h%6@-r^mbcuSX9hFPlw;dA?f{^K2RTk+!a(|WwDejn&m*O%1IZeMz023 zmPcW$PnR5pSsJyT7#IKIoj9e!%m$0GC0IAh`XhPbF9AXoSUE11X1DU4=aQq9qJ10& z<1;+@{8wGUez=Gu=vd|_zLmRbvjB_Mn^Z-^D>GU%hgr)5D6Pw~G4#-v z4>)#_&^7>=aBRdun5*FSvoZ>UYnrZsW9{nIQDE0Iy~yP-+?J=(fB5iWXoVLjh_s=> z8m8Y=L0Z3wtrI7G4Cxo|#9=nXHipGX-~=Lk0&+1q!NPGU=@VFRbW1aAJ#^?!c=8q- zLPRV>jho-1bbuLdXJp>Qs{$-TYoVoM$i7~%`eF2Q4%&1N=aMeEG;uSG>gdm6jn z;vV~P>OdvIRLBj#@FRHOerY^O1Dd6yNb?ct+lHNtw6CKrvgD0dTzXlcdF&u*-vxoN zY&0ii4{#t7eSlHuu_f(Axek+15yMGia*)%=)t!tXJ5e+KoQ~*x!G~-@-m>!XaTE(i z#}*;lUqX!R!g)eidz>)BqJ5qmyjHXi?n})w6zUQ{MVRjlVJ_l8Vq-49m1#1GMXkvZ z+!sFiy?J9FHjtwlhlYu39&gJfjgx3yF~LG=GFdPnXO}IkfDnfgpBs{adc7H>131b3 zjdO5XjiR}^xlaP?*^Tt{4WyCTtmPOvI0$&DG;NuhgBhDGxg4PspC)Gu0=n@a(4K?V zQj#`9kGY4yM-C|f_pZSR3W709SCa!jp_|5H5=chF@gc<647G?Hj#6H4O&|vjK2#&Q z0`Hp!aDw6@gX&b%E`9@mbB%oGEPRTzd7(j}<+K`x3GKHB$O&%<7j-z~Dh)$?V|vm6 zP%VPsTi9~yiH{obnH=4sUeAJ-bxdC*R-y6Vj!=Rl-4wFWd7o6Ac1b`vY50a;J&I5Z z&dlg=1ZC|!7)uir-|9g`t2|X*Z>T)CiQXt;0@%Nfd)DxmauK> z$X z6A1?qY2;r*T=ZezwDZUb(#-~jigZf)+r$y*=h4xSoaV_yR0nc69F{lf>LlHf=Ev1Y z&W}f80^Va@Jx_3=H&BJo`+(VzHABv=+`*%zfH5TpcarlMROe|!#@_jvR-h%} z;qM4#vvOw= JPF=b4zX4tIPBQ=i diff --git a/benchmarks/trajopt/samples_vs_throughput.png b/benchmarks/trajopt/samples_vs_throughput.png deleted file mode 100644 index 8adca982314048ea8d5cfb2aafd3a27e5042bd93..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 27312 zcmdqJc{tZ=yEguzP)Q;oWk{(EWk{3^DMM*8M}rKhXfS3hl!S;B(Vz^O37Kb#29!vt z%*t41BIA4BT5IiRKYOix?DswPKfhnc^Bk*uzdpk~T-SM>=Xu@U>IYPr7I7`2D2i#% zZY52MqIIJvnwACh_{o;)hHv=KHd|#qTP=%|w)RHWrqq5TTg$T+wr9r{!bID5MxGB=G=Rf z6tx{5^fo#=>^n8dFwnm*bI;nluVhlME>$k&+8GjY=rxf7v1`H@{y*=H%M2FqTS`T9%sMfR+q#nhJf8Slf?uLIVoxDcJhkpj# zHTwVm$9FF0UifbO>gMY5VeajAJEAm=UixvX9J!_z^{n#l<{MH9Hex4UEv-#DQWtjP zdv$i7!K1ywn>_I8-OZ==-FffM*6z?!OrsSuwEMct@sCfAo$1l;Vp+UGV*bycKcf#s z*7C1eQyrnqcv_MxR$J)A$&>eFq9lx#OkNW@M6pkYaL7q=F+Fr>Tj#rlasrvV1 zELt=3k%ADr~eJib<6U$lP7IJ4FI+X9#IR)&$j z#+6&lYd6$A;0YG|_;vcn=QM_6xwe;sgE6=zr+V0U z%Y~EkuLK)7EtHayYR}v!EiFx+oBo_l=i9Ph!<~cOF^|}NqFxR^XI9|Db1qwdK=OhR zbGp%U`qCFKo;5c&*T!k9O^)@e9yqY1$+=%{J{?`7bY}eY9UbN6r>ah!sFEFCd^qbo z6U8ZRJpbs?qXVzkKWcHEo#dSu>p$Lh=E?CQm*&}%JVA$(uBk+d_sHImE2JT(&q@V7o_l(oKDL1Z+3At@l-uLSYpL0z_EtT$f+IgZFI%!? z38$DLMeRtgGil0qJYDm6A(o)5tgINu{%C>I2g8T_SH_xMc{9vETnP>3&b95nvPRX9 z?a0r9Co~ON7Q9YBe|YzNNQlH!p8lA)qND0j^0zFk7)orbsm`~z-NG_ zJi5L9Aldqs*49^a6UyF&@4}j7a<9X-!maWc-5*^13%KL+ManZ3K(p+{_#ot z!FKz;%fBATTv)XKo}}O)!^g(P#+F~V1ojS$yg&H7v5^~VabR?`EJE4mdcl+Pjk)7> zMKd0VEnyeFe@w~9P<|i1Kd9;}qRzW?GcCl0@yW@rEhX;s$)Xca9>`zfDk&+!&f_w# zjlFE&e2IhKx_L~iW{aEhuODAB7-?u|-kp|=J(SE-IN6tHoJSEOI=irOth#Fi+(RN7vd(^vNUTUMoLEFzPK=o4 zkGJ_)+Sy&hWMeye-dHIuZ+VFto4KKvZMhmV&46V$fByWIDur34&bOh}IX0cfzrK$; zT^uN3h}B78$MVUw>%ES#Q@w#zCdGcqk(U~)h78Vv4f3*ku2cE+3rdoYf zc_8Pk`|C#Tq)h$xUx;mprUD%XX(#vR4?JU3BTK%ezn__bfnlqROoZ9{=<@!rk(QR0 z#gF&gPH~xbDlIF+4(+mUIX^ZjtI#u+u-mNgcvPcwW511)=$Vg+my3!N5W-q}dahrd z$3Uj7eB^EDc9VgY63R{L+!qA+6-U3jWX1nHLQipQI$E4!^t`3Dm1gD2m6B#P^L;o( zH?e0Kme7)aAgsk26v!X|+~7rox;!juaZ-z7UoN_7(+}&II`ut|6Iu*L! z$1qcdwXqr@G8cqoY`feLeoVUFh3lmn)8T7Yh<)v&n@?62Z|~1{M@0Pk)n7rSz-jbd zfwTS5BA4mQzm$}fTjOFL?ld;$U$-yB4Wk@^vH6r@c*7-(?YIdD17}~IB_wEHh zdV2@)`|S5Kokq>clVZpzH|)}|V|jL4oklh^T^!zD8M>W=OnkCI!N72pLbG%y5(Pz( zWy9d29}y8j=6*3Y9wWO*@m|q_t2zm~;isw|3KV7{tiAg_8Y1T`^(FJ14+#dt9dF1( zsAHZkj#R4-3=WnA3Th3!iKQk|BPwlCz!Hb9<<)BP+tJ#Liv2 z#Ls*bK?GZl)JBGV-Tr%5u?BEA+L!~OJMkRhb>Eo`j_c`p@yIz9KYu>YrlXRqmuJtP zv+(fD6wB_l9{RS2tV5DWJKl!+VD+0IKG4tZIF@ax_$$VIVx&7n(u`F%QEyOBxp>-l zrSuZdCEPn59JZ?X^V}HyUT5;-Q)*e8AFqPn%=BdBk*xD1TOCe6eK|c{GTow?=Sb$+ zk`QqdY#m-cX=x4wq#yH&4vjrMUCmD*0D|tb2it7krJAUY^n76Kta>CW?(*};=kZ^i zkHk&O+4en>=XM_J)0v4Yc>f{cP<7g=g9slcZy#(UI}y)ttj+I-Vw20{x%AW3v`?Nq zDYvMde!}phE}3^`{44Y0=xDC=euNPYQG-&<`TUlj{i?Gw(;5X%7s$MlDVMSD^KHzt zSFrW7zb9ry8-FPI{K%Pvitm#;UoI_EAL<-2xU=cl^}o2;c2ilRsS`7k{o;sY&TrF@m7=1e!m&Qbd$XFw?0)D~K2qS7mzU?1 zznEQeYUaL0VF}hw>-X8?RF=rbOrx)WcJlQSSTYW8>@S>rQu8t6%po&iwr?FBTq0?gCV$q1 zDY~-;>wE)9sYx-~MZ-rfxq8>NS6R(2!T@;!FPhE)Zqe=_QxdgNR`w|M>Q%s+pv)^* zXybGe?jXb6m2=LlaT)E=NzKUEXw~>JQD5t6T3XOgUe_rN>ebY*;h;>s@7_?E3yZCD zo#oiuH=q3^=APYD;Ov7xL$Xp*y40&u?`2GF*k_t&A1YULBhjW_u+B<4l64)grKhL2 z+t4s<-+ih5nP)%qBNCSx?>+nyxkoeEK+ZiAv0?%BIqj5xO{QrDI}KlEW~TMeANo<# zm~Dyi#OqVT$5u$6e^3)$_g!G^TBXFKr1qMqz4ZOVZ^sNqF6~oO6S`9s`pA7`#5OfG zHK6Xe>oK#H_o5Mg@ja_^Baw|K#O;S}13l}aF%^CDoxr-y)jf%!v~`_u3xX3Ur-Q*q^hZ>=TJ4eT60R8UsUuUpxI+!QENn5 zf%DxpQDyRbm6OjkWT$>>Zx`$x?yTaVtj7n8q7YIJ(C)y_PDpQWZ{GtzUE0`q?AL=J zse=KN$@O{M3NDxn#HGbwUiz+B0|;M!%IVqFg>7A3rw5#WZpEmqMb<}Y;aIoL$AzKKuBggJ-?d0B&mwnoOrAoF!0W0i4tsn1!QtWCK*vJ8E2YjZ zr6f#V_!{@SV(T$Ku+W*@xDva-!O5w-y&{;&+NnD#SSjIyP=9Mq=R+;|Gs}+!$XAVG^j49o{!$KwC*N=ZR@#35$P_GkT$g8NW+o*o>*deh zdK#)|`sd1_;bHOf4O`u{=%R9#P5^N1R#SV1CE~-)Bifz5%WUL(9dmbgH+{@a0^kY^ z{9UFeXebE@iGj)fB4O36>rM5*U`to6@-3R3sc*SC+ek_Ui)Q(Kp)$*hduO>&k1RQO z@F0naj*gD>smj(iHdeoeOw-TQEuqw8rzMO_7qnY2GBT#0Z{*^?()!#}xEBEU^rw{7 z2Oh~E`!VWH&xBOzJ~ZXQVc-@MvmWW%wrb2qA-Uk^y^>)+?(J=;QC_`$OK+JcJfGs+ ze9~93pBTnwpvx zELkFTx<1`(87(dCz_;56h0bA3ac;9-i-_%BT3YH^-d=opUbc1nB2weH-w&i;%;|=# z6FhzoZ^6}YS~uOLzv&MC(5s%uCPOsc@Hi%Bjg-{S@S%%(2W1E6ljMR_AbgJO>O8xR zNQ6OE>9=n$Lv`X88@nO0Q{9$rpl{8Df28ajR9Y_BZFBe*@H3yB95>dXc=kG(wQJ|! z;MyW&w_=x>PE@l5OUXfn>8tPFse5^qOfWB5LdNUqaT{yvXMi=SShku;>$T#a(zO6z zk#yf%pS4C@Tw`0O>-xQW_twViRD|{R7tP|=*by;XJ33AdWZU)LvmP|BJ`k?xPEVg~ zZ)20!$!r@C@xEF)#`H$Gsz1*=^9*{0M1)=y?8|}DWe2J3OI=5m2HtM(XLGht_AKJH zwR!@Wj{IoZQMm&Dp>KAX+{h($)(g;dRe! zk?P^5&@bs`?j*TkPQ1LmOGdii6EMSJ;7}5F^HYJd^pz`D)RPWx*}83834pdm!6>_1 z+xP6pql{F>nYvA>(|s2bbrUq^LTW_ShseaQdBZg4lfP~cc1Bibt&0)=Xk~^7exD=9 z`To2tn{&W>sXvi8gx~Y3XiPQsf2gS$dRX@s)PY^)6Ster+9>icQCIuU1y>yNGX>eVj%?L^GAvfAQ-LX+sMO ze>XQu^7K2($jHe5Quj{%xTp3s4D;u2u%~JExp9N3*8Dk_UUD6&fP{4uHw9L5 zu)1BQP_%HI7I}}1m;BVgE`jK-!(M_~(G9~2&pkiqe2mj#K!ryt6f#hxZpPY?cdM^^ z*9#5+AzK*>0uR89pakT`mbY%+4sZMGP2c-81k1{D{VM`1HW{BgcLVVv3|YVdqabG6 zrJkstM&H`ny7fX28>vrJZg92UJ}9pU>}Z59X(LmPZt>`=qdW(Bfl+YJ{lq!jSs%$4gX+X9jVGJ@B(i;I5_y} zsVSqBMU(HF`_d+_ZwYWpo?d!qee3W>?3HMLc%R+5vt0 zl7AGLDsNl7YO8ty3+)s;pOXFO7VzJ>Osm(1#h2`XbS9HC#$BT%% z?^dWU+7(QxtyXRGsfMv9RWCJw!&&Q2=_h?9f8}1K;F7O5nQHPfu)A16wY`r=uC*^x zM0pVRlgvJo85TyJdi%gJv1+u;hr{QB<@)V-5WL`2wZU_37fKZOIY5T{TNGmLa#FI&~`1pn!{nza2Z=O3Wx=22xEQzcd;& z)Smag@t8qFFE(WM#b4H`KhsWC@soh-p+55HQ{${-Q2S|`=1XJT*4Ea6ooDKvD66S0 zdg$13S3=*Qnxz(t=vmmbfm}my=E@Mn>fmwSGu0WsV@F=<>+9=1J+VjdsjK88*O{E5 zEUy-z3eTXR<=8TZ*Hthg_S@P@wtf8?l&!zV9`#VpnSb3n`s(UxwR%)MgQIoF zBZ0`kdC(11DY)77)J$max@ks4QBz9Y3SqSib^bA;64k z8aotFsVVK0J42jWktsVnJ1N^;Cc0x-`^^h&njObZ)qVNGRajWKG{D&2{63dI)f9#J z!->!fB;heW?qJ*B#EmH(2PR4d;37qgX#N0gLqmfY%CpLQTNr02>Wbc>E?Xt*uoQrA zlU?JN##-|&gf2xwC1u$z&m|s!H$W5DP_r9ie^Oynoc_EDJb>Sz0o8N&Z1BbuJanCL zOgx;is4n@qsMYC64@QnQ)L2U3j|O`Zk|PWXoObWsy9?9v_K|`=F4|}_qoJX(fSLJa zT9v$yUGEpcX~B&cL7*|UxFMg2gS?gZrI`;sIjWdv*PCL~d7xe4b)Nrq$@2}IC_$WB zZt#75apO5SfOmQJ7Lw+*3jmxxrJZ8dO*XgzqD!dmKF00Xww)4$W6ZN-YQ8wk1Yp4H z=;@#UiZ37_fK%2%jF1f^g5p{Z-;=V+e?0X90c7-RsGZc~NQ$23AL3h!W~W8-^Yd>8 z1Ps2C-WTK{-F$JG@S!KAo2z8q2fx02{`vC?jOZ>+O;$|o`gYCR-~k9P5+Z*|251)S zqc3*h3O2{$-q07I_z5T`OerD12`WQ<8G%jiKiYH#44l9G!~#U2l4S>F3Cda{FVAz_ zb$Ux;Vj_-Mf+*l5~B5VFDcvKS8NKFf{aRp!K=< zg&u9xY=(nvWk5B;9~7*a6C|6HYx?f<+`&?jpXgdcrVq2R6GcV9ojccn)X263T~V{9 zAO?Jwl=IKCXPXPT0e8z##VtC1{P>9zC-^pPT0+VL3{Buz`z$xNtb@9Y-H#0*oi(wv?ru4A=i<-Lr)YO*YH9UnaQO)C z$3xGa3>+yaIMm0VRdfi^>IxYFmB;|#xzFGyysZ=!6+yJE;o;#K`gp`bu(>|Zbq$EA z&2d^WexE*l+Kq(tqp9EsLO~R8dfCv?w39bV&iG`ugY^!~jOecd&9+udOkI-e1|C^e zes*eD_3bbwWoW2E)aUl?+cnDCW&kXT)GIHRHP`na$+Jr$0RbVP?8}$K*v%)voZh;1 zOY_Hcapl<4=JE7l+gzbv_+;@eVxSY+F>- zU4W!{_;BV~K@33Kj~^oQ=ohQ!?iU^sYb@5zKp_(V-fbZx<83^Q4^|9xfbK6D2?XD& zRYIE(I4Cz7n4O&s!uke5LeU38g^&}OkDhAZFJSyE;fXiTd&KE?QF}Q6Cp!XWCbwiF z3#6a@%vupFDw@`(R$hN|^{x~oOT@J|wQ;Cfdit=to&o-sUkcf#_5S-PHtcg`ReuYl z!SLhFIX0(`7GBKJ4jpFI*|NMhtFqRCiLZK%kXc{=s4vAN-LIg5B@|V3sv5tKqv9P0 zi#CYbQTJDysI+v}r!Cqf(__D?ty<*g)nzomwIf1R5@8*h7NLw&($lqb6HsKHNR^N4 zsz#X_0nYSvL$-KLP0awxS^n+YR{|9I#c2uKAi|V^S|4ch5q(xxMrUPZHU6{SqP4Tr z6Z-=)Ba4Q>OWK=VEE(V~kz5>5Wm)!q+2Y{nXw*@8mymt?Lk|ZL9y0M*9y@48>S*de0_YR}9Gt<;}T~Ck0 z9zTAZ1*?+RcBATupB^QDE#iG4a^>8!2;F>jl}#z}ppu&IlbVjW4}7w<9K8kS`_tb1 z8rnE4JGdVg)zLk%bCHUu|K<%d6yFwMj>zDbtA)frDBaarHFu5J@(s0&{2$jxJl5EB z{mV%UdEJMCI=lb)$ZJpe2D@+l2Wa^=Zcm;x&Q-9@_M``OlQl-oyaKFScj6 z5vLW)_Q!`TExn5mCee@O79FB1eqJ3ds}+CqkB^MPbywpVsq4!xD_1pNjMm8M&}K=c zFuV~$QJ#<1vooEg3;j4?n{ptbB;sf zOZ#_x#r66t7M3>rafR1AlXeL$A-<-ct7jJ%7pH%}_ckLjJLsGkS>p!wPEk$m`VbWCV2N`zkN4;mxyXTD zb%$Kp)>h`@F!QT3s2fxY!m%RYDXMgL(~3JrAQeH|Ut_%;7rvyT+!b z0uZb;zWDLT2~lfhWDYASp@Lm&o^cqlJ17K2uXki*B8XRmBf!jRcB3&~+jVwYnh1(c zqdi8&ZZuo%`&LnWQc~>Dbatw%-(;b zX*K0xE{|FZ^!lozLhB(4Bn878#I~X%tFn=L;#uKP-kK8veff=A%dcpgG(_kR6Z`D7OPK!glM=R}V9k zL3Vuh@gqB=6Cm*o;vH`tWj6%hBDxFmLiw6)+j4A$uV7Qg(h9z(9Y9(k{DQ2UTq{&d zVPWC3^_lzcx}cPVOuXds<;#>?aq*5-@)tj~oNojfN3LF1cgQ=(hDTXkc{n9A^IB0+ zk)ki697^=SkeY9rtl&)WH7;4)#7l_Vq)B6F;(Kxh*7{mO!9AdTHX>cO_4EjVtmWq) z`f?`OJ2*Ia3Q58mMwEq%7YE{w?W07^=cWK}A_)MVnl6Q6VrNzz;Xwo%2%-?L1YLyTHF zk{(Wq`!wlmY^+=R!W7-IHjC5>X~ifjoscAHq;Kinn^(L>y?4JZ*ZCrw!FAPhBEs`g z%)-rU_$7EA(yrs5a1B@}$XOu!Hf(&JTW#V|UjrUb z(zzKc8&(ews2~5yuvwNqXLoYCYNk z%`KDP8fa%?3gpuT+pnzSkL+&k^gdrG<2QfjpRfN^Jz=R{b6Ks~QkLpVe~zkP z8m%Q#qI#S*QeQ;Poxe@j-~Y$yI&%u7Mg+nDJT0f|GdCI=8!yH_=OAG_K6c-t;_1n; zuRzl~p))W3vL1pyk=Q|%e#)^~kK)Y>La)JHLN_5NBQcp&J(SbSJiCG@)GIch+#rt7 zLlSxLrejNadHubYtv4?9VgXz0!F*RT}%j9PTzj@u=6oQ|jBdQVw&;R7$0Q(E&ygjKXtx z7JT;a-M`->BV)^h2M-p1k&~6ZdGB6)X@maM^8KRs?21Bwmf`GJb|~3E8A5=!INXxL z7d8o?@MOoV^=%6!0a`-r3rq30Zzl--4a(0=MQh8;NqC!N5T*lFt*xKNX^~vwbXrB2 zGBGg;c=*r{l0`{P4J(u`GPk7qMEwp7G7nVJtIA4Ahhv8ibJ@ypQ#)ebFV9<+)%DBW zcojD{u^1gXbO_W)Ag4{IEKlIxJ!j9Jg^%p?015*B__Myc6!?JU5GPeGUd$gl-I%)t z8e7=ZIa5;)AP2$ZZ7x$=$$~}Jq;?n@@-5+&KWmwFUR#?DmxmuA;ZNX$1oa%2K|U2Y z=jt=hsreQa&CaudRe?V~-q(&P3{>&d)YRfH$f5Kc92`-X#s^vnHS{1fw6&|t3)ZsL zVq!}nKg^>DiwKs{p*`pj;p1S-XltIGk%ptf(8OdO<?j0e+dS0tadE6IUds)g$h1ZzrKntNpA8tl8P%<|(D{3?YC>F1+|lf~;dh z>{^GxGLAtgoraM8wq6`h@qWLC+|5MG*};KNT$}}U2!tS|Is2FN`3CqyJ^uVk+A0~_ z4HG|m8Nsoke!||Sp`i79qAtF>RgPRmLaG9o>seJ5GiqRRCnK`9fNG0jwlQlPAk8-c6MlUv3zHd_ z^!c;zLV|)kfH6%#(t2ILt~s%AZN$^%|Al?1CFtAvhWrbq6!O~RuRJ}n4igpJbpYbLB++RJ0;krRKb_nes^m21^ zBM`x&-@3m_EL&^J`QT^4eG2$V87PbPdtTsiV|-0ua4D|_-Az)7_iMCau==okHRxLU z!oKX28YJtEGN&mP!^p-`+QKdZvgmQ?)Go_xSffQyZ``@ zkk^0C$JdS(Be`hk14I8Rqyf#fXIs=xCAlft>NkfxLmn@F-1+TUW9v_|<2=7G0VYpn z9R~Q?+S=6B)!o1~(ozQ=Y^A}fg&FKVW`EcuTsJiVX;et-4%~3WKl#+OLLG7+RSXte z9O|g;k5AR5-GIp~0jY5)4OhdfrVOrp@j8@iVW5S;szi8eCXB;(1hq3DD2Pr~RaGly z7gkDC@w9XpsO=Zv%CGOww(Vwx?zHbFX(3<<61V^%m)s%oNY)ZqvtELT9)!|rgm6fz z>;0j;uve|Z6==DhUb!^Zg1I3nHt2Rw6My04}*pCU#E)ciCQbZ>uz zq`Q)-q)}LI= zcVBx~*9JoOBeR%@t%i(;&8vFyWVD$?!h8oNn%|Ej9rp42Xj!fc4J9m`VEmPqR8t!5 zF+?a6|J{I~VU};%N=qD;V9xG|ZJB#1#vbCs`26`Zd3`ch$QaMjFo2&B2{dkaF1W2p z15+r4ff+zFB@vP$@&~AdSEX@epfvOiRp0qv<_N%Ynw#Gqs5)PvXbDTTf<=7^%Tk5H z+?9m1dXV83aw8B*g~N9T_e;`3P4XxaPahY;Q#rm9nM1{Y$Xt8c;@;roq;cZvJ5rv691T3um2A`%`%A;d0`DUIyHvyld4Lt9q;Jj4UYLo1vjM-p3p?c^9V0EJvCNjDXldaF>ClCm!{$aFsm|Sfff& z*zV6TM?}aZn}L`@F~Z6Dg()dZ(bw^+s_G@g-juVSB?zelX+_-_B>hrW)+;a@@IeYg zLj~~kniNgnsh#t;UY{a*oWLP7i!z*bUH3;J7#t?>wA9HgrWbk_fl&Z-Nb@epl51G(Rj9 zPZ(p@mcf5?8SF~QRp_(CCeJJfNVt+8lS(`R@R5-g z9gw12!pCo8E(o~a7fO#kP;H_C^FK&B_WPDl)V6NjDm$_m58wLr>pWLiS0aIf$a@Ko z>c0?e0x;6Y1VKUf zBl`(kj#|pe>Gj-`d1XV=%H_*R<{F=v5E8e4bk7ecr2_^aCOoSWNFIgP}kcS>DEL}aamgr>KChItS;?pFu{0x*53_^lEojYIYv3LAoJT8EWLg;_t_fH` z_&|_sdqItZZGW(B4LYBE0j{qnBqZd|OnfKKT6ba{*^59|3e66FoAvNy3*M=KTeh^K zVlS4Z03Y9upM8yI#0n&HWEUvx`184hSXkckxF_cSg(S+}myplqk2SkFeST_6%9 z287FpaSJA|Wopf?sG}9})xWOc1g3Ps;>D#vJp%wVCD6;LrHBZqnhEH_9|3dDO5GF@mio%RuaVp0zROQ)7rizT#+v@WI7k_ zfE;;7Odt7kszMBoC>-^#iG7nv7p-`C&`9m5!Q485%L+*tMW)K=TB zrASbKm(|b76X-S5Rb|9_gFO_P!Fa2MxuAhy5M25iA)!ThpYp3Qk6gI0UAGvSbs|=dtu5fuFcVL_@PR|CCKr~*-4uDl&GjEaf?%7nyKFxlb{x_#zYavEFw@? z^=bUC1u`!w@1x(|z;Ak$yS0P4Cq0ga4cS&$>GLUAs!dQX6TPVE(%7+OF{E8s>BVDf zF!<>n5#Q-xml7Xotu?U8c2 zI>LBa;mpLD#hhZre~#4C`RAMGKfUl>cg{1O0&_W$QOGibuol#EQ2tk`q&_J#0;=^; zr-Z@CL%GeB21kz_BRyY0yaG3mJ)a}CnC{ePSx8@4GB%1tG*1VMTK-IZkF1kqFgE*Sw}o&Nk(00rJ< zfH7JMTHF=n?0Zt%$!kc+jN%bkknKRtv|_Pdf!%IS+9OkAeyt&T%Lqqz1Wz>xX^8UFg zDu-~(s|Lxccx0q(Cemk|lA21I)WqQL((0TeUsR$;=7m7|L#tvNxElZ}kAi}NIGcol zPySdkq!N=h)F1H#`-$AY|1Ob@S%T(OfMuR3q`_Jij?pF?C014sDy>%Mj^#=q8g2(( z9(iyyd;967(B&C3%4^N=TLA(3oAjTS)^1a$`)7HEFg+a8g#9Fl%ys71dh`PGnwQSPC<{hf1U(3S*XK%ZA_Ogt-56a*234SLKr?Ye2vV zY_u*gU*;bZy-8o4u<-ZrlbJ9BUiI)WLPP}r^9*JDA03Uyyzhef`j~v&pV+36qsYwl zG#o0@I~2BZdjzGeG#iN|5wNT zsPcBj_-E(7RmVIDo0WFv<;)uTV7DN#UvtR{vi6eVQ)XR`_CQZ3&Ogww*FcJbmnK>T z5Geikx@6bHyByq#wsv;(6o~#d#F7Pz7HJwoS(uLufB-%(``ASo3$z8{Pr(L8H4civ zixq77b>r(D0oV_pKIyr0W*oi|7RDzKEt%7>QB;%}U!(Ug-z2wwtVEHSg>0YYkG=r$ z2sAEC{9q2}_^9f`RLItVN+hu1m;xFKsy;#1>({S$*MngVv^F3&yQ}hhpXf%>bxJl?p+72 zZ40$;a{RW!)58PhX>00%)rTYjo>kGEGBk&7)ciLV*ROzB*Z1MG&!Vh;`XvK)o2m%Z zie0R~>2Uf2Sj(>hF+e<_8VVWfUc8`34gU;Au&DVj%gTbxqrvBv0WOoyR?RE0<)ed9G3LFXxP5cd5!+LJq2w?3No$TOaUPIn|k_*-u7#Iw}@m%qT&kvPwQ1}0$!QfVHZLMbK5FaW}+8vOHcrFC#Y@qXS`fM}w zbjo@l|Hn2*{Y8sLqYr?uf7KQLWoA-?lg$lW7mO~Z0}iow8|B^gexUE2RiFdBg9KWG z;|vONVHetixFjGTP-Z9(6K}w&hmI;}S}XhTLC3&#`a;fy?{^d{L7GQF%s>OlMlcwg z^;81Tmh1&f1u-NL=VN1DouQMNbfyolVa9xJ~d%OSQ zol%uscxQoNpcC8DsQ zsmWOSvh3uKv~_AX{&STCfq$3u@leGhFgy59ArjuD2~vv^>eft&@XlZOOAY~IDC`tgwvCg<0fbuKojiYJhEDWN2M?LE!Q>e**(-3 zuwyHEj=Krv>9d=Jg;3rzCgg93zzRjDLK?D!kZolcFbKf?R!|AX7QX;;#wGqo0RSbLv{s?9R{flFb_TxB% z(e`c3o?HuG}8PazT(tx5s zNS<^yI$_HGTcn~lp`6XPz=>iSEQ(Q^PpMa7q_m>P{`!XtnFC%4>4C7vm0tU!u8vS*t;4}sEcC2VakwSS(+e9pBHoA2aQRNluDkNQBdNTka-toWwWrbgPz{q#0Brk3EwALE4bf zo5Tke1pVs8ICkJN6a%66X=!QI=tuw+!+_>@qST4Qzx&%xPl85~tLtK3`BexxNsnZ zmhnTU9zmpkVR6v+Tj38uG=~Q!=K=h$?I7>9glici4jwC+Z4>32s9o+iN>-)D`a({cy zB{}DP(n?Uop3J+5JsI)xg$u>|4D%cI!90kR5H|{a|F(3i4&H;u>4<7rv?KHY1BD*3 z&8H9U-K!PdZ7T!BxN@K9h7CJZRIVxR+cTVPvtsOej?IL-wcu;g+Z+@WC5v3ZJMe_} zz?_E8A`tpxqq&dXqgQjB?TB7^5;gMk=d}Ydq^C466Lz2<^;yh+I$d$V89&aR@csq* z0H>{x04mVouP{@LOb{3X1FYoPPfMXdA2RMcdO%}ewYA#q|1j)`MTb20HCCPJTT6iq zWw{+_RND@+C%_SQQwOa0(w8qu|5b7C{f%fywCZcv2IIrN2U}T4yD(|2fzOr`>1!u8 z09ks}4j@?4rkg*}GuOS{*O(`clSlGVo5gBG(o;lvzJ_A|8Z)mS>BJ)n7F;ju4n9o$ zMubi!6&2&Yh8$7{o_zbj8!hvOD61*;_@nz)%f;9pdlyJJQm@#xDg_I_fj#M}iSilB zkKM2m&1}SihTYc+eU0?{qktei9Q+aztXyvtOd6h`%NbBy8GJ>!ZUX7BBW*k7w&?xB zP!XjN=Qj|h8;soIiACt!k|T$tO##@wIAT%YLC?h10{#5*qeqWeweB4)zn{0O`_8d1 zTM<7aZpA8H8lYwA)&S8w`9?En2fhIIHwKr9;^sX0j_7lzd68gAGgR zMU(3xm3+o*+TW}!V~bs?eN03rsOULbv?~?S*3fC((HK_DcV^eitD@`>NlYq;%2)oyPspONyc8x%{P`~@4LUFx z(y7OCG!2k2*?x^K%v*Vm9*-#DEdFi6{M=4xnV^LaX{EydcKy%s;t}z__+7RhI+pLt z_O2RA`hyN%6-Njvuf(_! zLsRAAKPgPD;&m7-gJCkr>z>o5Jot04#4LXgmSY{skU@+OX*a3UeV=ec+MNg0g9>QQ%Y|Gf_w6l^J;`0oeH)Q`?^TMs;` zfE|i4TV&rPyf-HXAY#+Sy12rv7@YPWK44Is5+c<8|2$aN|9!Cjp6~r@EYBO3>xCP9 z>Ll(Ioeg*qbwEA1q4@=b519?unnSIq_>BurI4sKcg=%9sGPKpbjhWI+vnHkef7Bb> z+1*V4o|fVlI)W?zt*6?{YlfGxSmIy&Mqa*vRV&bjGw1)EgDH0||I=X77SpEe`}@IM zhwbM(GB8|<)=nn;d=5Ih4jn?PFDrTaKTqX74wuGBB!jdEEP|Q6?2hfee#M&PX45Wkr_nTI*a4p*X zfl$ytoLdt;r{G`h>gr-Wej5ku<>$Nl{WLW-rJ+U~rFpXR8O?$NLVE{7^kJE){t71~ zJ>?bB->!f12xahFlOGru5Ia3KFi?zh57aKcK|UvSW8(C)h}FzTo3b5?vsL#5;wj6O za3&>)vAFkQ3)41Iyk$tw;$XlX#B1|3KRA}Dyl{Xy~iU!}z4 z&bwc~TwgMh&9pE1@i{*JSFc|yA}Q{B5ZWDbj;5=p2WJ{ELdaf`9y3R21&wq4W~(^U z!Kwste(&mfC7S1LwK9Y!OR=oD*U+hfKH7dvN~~^;t8(SfbxT2al5@w14fg);-W3S$ zr{^?Fukt&ed1&Ul{i98F%Y`{9&6G?78m(I)YooP_9^@f8C>1%EJx~0b5vVghS=szg zamGRlxcSR->f)B$@i>>etyTu^6X>|ejANKs0#-=W|ItOb{Li5HU-tQp(-3ZnCkci_ zg8V{4IJIK5&0rW~JohF2GJLxr&Z_|!a3Dnk-#Q%224^!4T)kXSAP4WPR-#G&|J0^a zG104|hQk@i@v$higQU*8KecatMu$^AmJ;jF@8ch>(5PdY>tzTB*}%xi)ug12I7Vgw z=j^SlTmv^NdRW~+^8%QPT^zS0eWiRIuo&!w)ik{Nw0fT1Ikc_E9Bn$T4BxXC7*PhC zG(;{vJux~6XW9zi)fMLqFS*h^9G``iK;gc8~pi>Hplk=ING#&vU zT*t6v73sg;0~q}mMTmYSYe=G>)6H%}Co9K74hjKn>k}Fp3J1PEs$|oWD|CPKAhl1p z2O;Ufr z#d2}ZqIW+n@;g7Y9@b&Z}5NdHeaX2h=O@{kN@w6>!{<*1+J=Bk_{3R6T~A&$@iM z0FDIET(kr0aO}lOi&r=S$FZv0sldQiCiLRe;7he5xEnE-pwU=aX)Wv})GPQIf44bCWug~@rHcwm;k zzCI$HlCwulop2lk0u0l~M{uSiYB6PAhFXtrCY8cDjh<^0$ct)~P=V+W+62}?CxPVl z67O6waZdL%j4z}=&$6v_Ayo{5HU(!p$qrX;rVwDmW$PSuW-b90&c{(DDhM5^EJ`Q= zA@-X`B*rg++7Ih^1pN5}!^86^lzliMnTr^BQth7%gZXC7+EblASq6jFS8!&ggLXne z*ov!hE(6wvkkpO(upgl~78b^vohWXI*UpzmjtffaOL%&zsk9~iWA)HS7C|a>?}j%X zCz83u_MUh`4gn>l4)GMAASO-$QY<05P{rpA7q#cS?O(37JkwjBnNBrm7lcI!SLDUC zVvKcB1I`sd%F2q7)gdz6X^&CNp-R38j{@a}jmMAUl(F3e;1WCA_M`GSYCNyy!n!RG z&vt+U{iE9^0&esMdhNKu1z{}M`cOzBJ~jHozWuJ-c3pYAIVb3Isk5KlFusxhrna>8 zAMKP0eNuW(Q?l1Z9MlH_+?Jp9aoRYk;J@n^ajy^ab#!t#*gEav z4A(vMRiGtY8uKw zPu~X4hF*adq6~V`we8|jF4GThxZY|vkDN>epBm~~14rTnBn7a z)Nxc21w}|2Tef7P0=8?gHH$M9Po7+du?Px*$&4HHH&h-SV{)P-cF$gK@9WohB6Ja* zPQuwWs05U0nC{9xp^YLUVRheWELVo?bbK-_G(CBuCr>?oF81RH1lT;aNjU!YDxeJ> zL(8!>K~1odDJOg=C;g`a0?Lkr-}QDxg9rqQV9jU}} zJqjKWFhIjCrIG+)kQUModV(p0K9`n1Kz`=D^KQ?*_qpHi{k&|lXEJF+9*4d$KR;Fv zA23R0oCzfh9bLOqI%3N;jjDtR?(U(U6DG80+fLVG9GH))?}7pDEBHoki*T%R0J4rwfWR@*xg<-4(n=_SYtizBR)3B7dy0`@L61j1ZMYpVK_wZ_ZqCH2R zSsQ&Sd5>fs1P6mtvSr)F+S-Eb!M1fEG_A5Sx@yNtdYXdkEYHyVLUY-wQ;Z(fW%k`y zixcm|+}vZMoIH|AQLXLmZkU1`?Ci#fC1`p0*5O$WYHV;X>yn#F$HUSqz!xWZjh6F# ztyXJuhfFX64%1szH*dazN2MKD(spr>RNVu@dk*S3Hp6!ttCvK0;UBlawr8^ z;8+`*(=dZ#HMRBjZ5M2c*C38Ewsbo15?QM#v%gQbbg36h#0S?svmV}gQ{Mkc z?{7A35vm8bz(9W7ZeI174Gz2n!C7)0BR7PKhYw7{$-oAeM%9H=1Z{_I3+~JCg09$( zfWDqWe`Zre7h`C?fEQ@n_kT^C<54j6jPF=!Gk9!jc;v4JJQa>3z&oWic;2CXcJ^Y} zBlctO$lJ7O^78NlC-FZUBa3xy&knVRJ#WECsVAKi^!jSv!?{r6c$~XlaFebWgyH%zYxEVIv-eC5EB0lJ| z8!~|cP~*wFqfhuCor`)vaVdjUFbH0Wo~Vd@nuy=S(`Ta0nn#PT=}1-g8Qc>T1m)<^ z^n6qCo?-u16epjB18Okm9EhYSrs?`Nkdo3Zrbi_Q7V0xK$RMVX=z2X56%|X8h=#hp zUbCu1%@LUOUuPT8Ye$hIZR4}|&V2-xOlY0lYrzF?^NiLejS|8X@i#ZLkYX52g-8_z2k&G$*XH=J@LH(K`g?OIlmff%~nHVuEu@;ddBc0K&6i3 z+XDuy;Wgx^-~-xPY9vYvhS!xER@~MG7|l^i4Mkjt>641aTPK!`#q_foW&(v}0rtKy zswmjL5faP*l6X61m2qHs#~H!Y(>1F_daJIlj}ZHq+ae57*RU;_L9m*ziFMIdhexG{7t!m+k^p&SmgFQBN|2^#{f?G0SKUzC1A+3-w> z+CTLu=4dJQ!z~qrlAP4VW@ZCsQ^E3=1zyx0EZS81RBZ2 zMXGMKut+r;;Zg%#+LeL0x{M-3E(HdS7Pv1fkq;O+vY#{KTrsgTaBy@S3-Z?M##=dg z9*&G5Mb3h|XTsL!w|L3*gB!%BqUn0qs)eHnJE2j${igC22$#)_sk*?A3Y&Qd;J;3v>^9c!t;yDg2BXTdvc?LIQA%J8} z*X8F?2Ge0*o{8wII|D?$Li$BWG_fDP7i--1N=pZ4B9LY(izzN3O4nuP9 zq}18i+}H#dI*8KMO_e{A#@1u8$WB=P(-jL^x0M)CZ@0Hc6?dZ&q)J&PQ&i|SY#ZM^ z+KKwE!?q(O2LyxopqE}qL$=y4ubf_&K*c@ze0B#8=Pg?s&4rGv4?M3+DADLTsm#vgx_Nh zXah?cEVH!6nx`AuQ8a&Dd;jIbk==WiCJKfPw~CPZWiA$Kys_fMa@>8Yb+OSS`P>Ds z@`hLA(yYYqiuLVFh9{MBW?O}GCmN4jX-0-W4V3jc%1Xf*dGxlmcYw1RHRSE zYb*K{TqQMyyUZ7}$+-RV&-sjIe+ufTGc?EexO{=e_8Zu#ihlFv{l`Zf%Pb`kjqq0K^+tqtM`|EcUA|G!=w5pB8DU-d2+ZIT@oWz`W-u7ODZ$>^psPj zm;Y9n$L>``c}(zLC}t{3m9tm7-1e_u%Pi|OKXQaf&usZGpfr3*JeL;zb3gQ6W~7+C zX&a$ad=z3r!^Nm5^05~Y^F(IndFbKEu-lcX307T(Q5?7`a=6pv7?JFuX|g~U zAJ51)>RO)73VtwSxL4fAcsLk{R Date: Mon, 4 Dec 2023 20:37:58 -0800 Subject: [PATCH 24/40] delete some comments --- ambersim/trajopt/shooting.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 60a80606..cc8aa305 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -149,15 +149,9 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j _us_samples = us_guess + jax.random.normal(key, shape=(nsamples, N, m.nu)) * stdev # clamping the samples to their control limits - # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- - # reasonable default value - # TODO(ahl): check whether joints with no limits have reasonable defaults for m.actuator_ctrlrange limits = m.actuator_ctrlrange clip_fn = partial(jnp.clip, a_min=limits[:, 0], a_max=limits[:, 1]) # clipping function with limits already set us_samples = vmap(vmap(clip_fn))(_us_samples) # apply limits only to the last dim, need a nested vmap - # limited = m.actuator_ctrllimited[:, None] # (nu, 1) whether each actuator has limited control authority - # default_limits = jnp.array([[-1000.0, 1000.0]] * m.nu) # (nu, 2) default limits for each actuator - # limits = jnp.where(limited, m.actuator_ctrlrange, default_limits) # (nu, 2) # predict many samples, evaluate them, and return the best trajectory tuple # vmap over the input data and the control trajectories From 8f32b49e1f6193d937f5043c73debda28a3e81cd Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 20:38:17 -0800 Subject: [PATCH 25/40] remove some dependencies to upgrade to cuda 12 --- environment.yml | 2 +- pyproject.toml | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/environment.yml b/environment.yml index 3857960e..77ce9b5f 100644 --- a/environment.yml +++ b/environment.yml @@ -1,5 +1,5 @@ channels: - - nvidia/label/cuda-11.8.0 + - nvidia/label/cuda-12.3.0 - conda-forge dependencies: - cuda diff --git a/pyproject.toml b/pyproject.toml index f1fe9a12..6e10f3b7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -18,15 +18,15 @@ dependencies = [ "coacd>=1.0.0", "dm_control>=1.0.0", "flax>=0.7.5", - "jax[cuda11_local]>=0.4.1", + "jax[cuda12_local]>=0.4.1", "jaxlib>=0.4.1", "matplotlib>=3.5.2", "mujoco>=3.0.0", "mujoco-mjx>=3.0.0", "numpy>=1.23.1", "scipy>=1.10.0", - "torch>=1.13.1", - "tensorboard>=2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + # "torch>=1.13.1", + "tensorboard==2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 ] [project.optional-dependencies] @@ -43,8 +43,8 @@ dev = [ # Test-specific packages for verification test = [ - "cvxpy>=1.4.1", - "drake>=1.21.0", + # "cvxpy>=1.4.1", + # "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", "tensorflow==2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 From 94409c49d2456a257565a41571d006918cfa4504 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 20:47:00 -0800 Subject: [PATCH 26/40] run code checks when ready for review --- .github/workflows/code_checks.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/code_checks.yml b/.github/workflows/code_checks.yml index 3a8a3d65..17aa5ce5 100644 --- a/.github/workflows/code_checks.yml +++ b/.github/workflows/code_checks.yml @@ -5,6 +5,7 @@ on: branches: [main] pull_request: branches: [main] + types: [ready_for_review] permissions: contents: read From b51d2de0fb37a4da17328129d09355500db18344 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 21:01:32 -0800 Subject: [PATCH 27/40] pray that tensorflow 2.13 passes tests --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 6e10f3b7..34259679 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,7 @@ dependencies = [ "numpy>=1.23.1", "scipy>=1.10.0", # "torch>=1.13.1", - "tensorboard==2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorboard==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 ] [project.optional-dependencies] @@ -47,7 +47,7 @@ test = [ # "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", - "tensorflow==2.12.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorflow==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 "tensorboard-plugin-profile==2.13.0", ] From 6a39ae22c57de736bd1f7b2f90b6fa9f534e29b6 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 21:15:59 -0800 Subject: [PATCH 28/40] quick gutcheck to see what is causing test failure --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 34259679..4a34105f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -47,8 +47,8 @@ test = [ # "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", - "tensorflow==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 - "tensorboard-plugin-profile==2.13.0", + # "tensorflow==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + # "tensorboard-plugin-profile==2.13.0", ] # All packages From f164fe5a6efc61ff458b351af09b359faadb1099 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 21:23:32 -0800 Subject: [PATCH 29/40] sanity check 2: whether pinning to an older version causes install errors --- pyproject.toml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 4a34105f..55573fd3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,7 @@ dependencies = [ "numpy>=1.23.1", "scipy>=1.10.0", # "torch>=1.13.1", - "tensorboard==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorboard>=2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 ] [project.optional-dependencies] @@ -47,8 +47,8 @@ test = [ # "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", - # "tensorflow==2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 - # "tensorboard-plugin-profile==2.13.0", + "tensorflow=>2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorboard-plugin-profile>=2.13.0", ] # All packages From 3d121cae614ef2c2588f0b6f7b71180587fa1375 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 21:35:50 -0800 Subject: [PATCH 30/40] typo --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 55573fd3..8ab06d84 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -47,7 +47,7 @@ test = [ # "drake>=1.21.0", "libigl>=2.4.0", "pin>=2.6.20", - "tensorflow=>2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 + "tensorflow>=2.13.0", # [Dec. 4, 2023] https://github.com/tensorflow/tensorflow/issues/62075#issuecomment-1808652131 "tensorboard-plugin-profile>=2.13.0", ] From 0f507e3143bca668ca0cefba725a72b9c7caa73b Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 21:47:53 -0800 Subject: [PATCH 31/40] refactor API to take xs instead of qs and vs --- ambersim/trajopt/base.py | 34 +++---- ambersim/trajopt/cost.py | 109 +++++++++------------ ambersim/trajopt/shooting.py | 48 ++++----- examples/trajopt/ex_predictive_sampling.py | 66 ++++++------- 4 files changed, 110 insertions(+), 147 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 8d705144..835378e9 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -103,14 +103,11 @@ class CostFunction: (4) histories of higher-order derivatives can be useful for updating their current estimates, e.g., BFGS. """ - def cost( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, CostFunctionParams]: + def cost(self, xs: jax.Array, us: jax.Array, params: CostFunctionParams) -> Tuple[jax.Array, CostFunctionParams]: """Computes the cost of a trajectory. Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: The parameters of the cost function. @@ -121,50 +118,45 @@ def cost( raise NotImplementedError def grad( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + self, xs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: """Computes the gradient of the cost of a trajectory. The default implementation of this function uses JAX's autodiff. Simply override this function if you would like to supply an analytical gradient. Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: The parameters of the cost function. Returns: - gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. - gcost_vs (shape=(N + 1, nv): The gradient of the cost wrt vs. + gcost_xs (shape=(N + 1, nq + nv): The gradient of the cost wrt xs. gcost_us (shape=(N, nu)): The gradient of the cost wrt us. gcost_params: The gradient of the cost wrt params. new_params: The updated parameters of the cost function. """ - return grad(self.cost, argnums=(0, 1, 2, 3))(qs, vs, us, params) + (params,) + return grad(self.cost, argnums=(0, 1, 2))(xs, us, params) + (params,) def hess( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + self, xs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: """Computes the Hessian of the cost of a trajectory. The default implementation of this function uses JAX's autodiff. Simply override this function if you would like to supply an analytical Hessian. Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: The parameters of the cost function. Returns: - Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. - Let t, s be times from 0 to N + 1. Then, d^2/dq_{t,i}dq_{s,j} = Hcost_qs[t, i, s, j]. - Hcost_vs (shape=(N + 1, nv, N + 1, nv)): The Hessian of the cost wrt vs. - Let t, s be times from 0 to N + 1. Then, d^2/dv_{t,i}dv_{s,j} = Hcost_vs[t, i, s, j]. + Hcost_xs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. + Let t, s be times from 0 to N + 1. Then, d^2/dx_{t,i}dx_{s,j} = Hcost_xs[t, i, s, j]. Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. Hcost_params: The Hessian of the cost wrt params. new_params: The updated parameters of the cost function. """ - return hessian(self.cost, argnums=(0, 1, 2, 3))(qs, vs, us, params) + (params,) + return hessian(self.cost, argnums=(0, 1, 2))(xs, us, params) + (params,) diff --git a/ambersim/trajopt/cost.py b/ambersim/trajopt/cost.py index 454d74e6..feb579bb 100644 --- a/ambersim/trajopt/cost.py +++ b/ambersim/trajopt/cost.py @@ -18,47 +18,43 @@ class StaticGoalQuadraticCost(CostFunction): dense. """ - def __init__(self, Q: jax.Array, Qf: jax.Array, R: jax.Array, qg: jax.Array, vg: jax.Array) -> None: + def __init__(self, Q: jax.Array, Qf: jax.Array, R: jax.Array, xg: jax.Array) -> None: """Initializes a quadratic cost function. Args: Q (shape=(nx, nx)): The state cost matrix. Qf (shape=(nx, nx)): The final state cost matrix. R (shape=(nu, nu)): The control cost matrix. - qg (shape=(nq,)): The goal generalized coordinates. - vg (shape=(nv,)): The goal generalized velocities. + xg (shape=(nq,)): The goal state. """ self.Q = Q self.Qf = Qf self.R = R - self.qg = qg - self.vg = vg - - @staticmethod - def _setup_util( - qs: jax.Array, vs: jax.Array, qg: jax.Array, vg: jax.Array - ) -> Tuple[jax.Array, jax.Array, jax.Array, jax.Array, int, int, int]: - """Utility function that sets up the cost function. - - Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. - qg (shape=(nx,)): The goal generalized position. - vg (shape=(nv,)): The goal generalized velocity. - - Returns: - xs (shape=(N + 1, nx)): The states over the trajectory. - xg (shape=(nx,)): The goal state. - xs_err (shape=(N, nx)): The state errors up to the final state. - xf_err (shape=(nx,)): The state error at the final state. - nq: The number of generalized coordinates. - nv: The number of generalized velocities. - """ - xs = jnp.concatenate((qs, vs), axis=-1) - xg = jnp.concatenate((qg, vg), axis=-1) - xs_err = xs[:-1, :] - xg - xf_err = xs[-1, :] - xg - return xs, xg, xs_err, xf_err, qs.shape[-1], vs.shape[-1] + self.xg = xg + + # @staticmethod + # def _setup_util( + # xs: jax.Array, qg: jax.Array, vg: jax.Array + # ) -> Tuple[jax.Array, jax.Array, jax.Array, jax.Array, int, int, int]: + # """Utility function that sets up the cost function. + + # Args: + # xs (shape=(N + 1, nq + nv)): The state trajectory + # xg (shape=(nq + nv,)): The goal state. + + # Returns: + # xs (shape=(N + 1, nx)): The states over the trajectory. + # xg (shape=(nx,)): The goal state. + # xs_err (shape=(N, nx)): The state errors up to the final state. + # xf_err (shape=(nx,)): The state error at the final state. + # nq: The number of generalized coordinates. + # nv: The number of generalized velocities. + # """ + # xs = jnp.concatenate((qs, vs), axis=-1) + # xg = jnp.concatenate((qg, vg), axis=-1) + # xs_err = xs[:-1, :] - xg + # xf_err = xs[-1, :] - xg + # return xs, xg, xs_err, xf_err, qs.shape[-1], vs.shape[-1] @staticmethod def batch_quadform(bs: jax.Array, A: jax.Array) -> jax.Array: @@ -86,16 +82,13 @@ def batch_matmul(bs: jax.Array, A: jax.Array) -> jax.Array: """ return jnp.einsum("...i,ij->...j", bs, A) - def cost( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, CostFunctionParams]: + def cost(self, xs: jax.Array, us: jax.Array, params: CostFunctionParams) -> Tuple[jax.Array, CostFunctionParams]: """Computes the cost of a trajectory. - cost = 0.5 * ([q; v] - [qg; vg])' @ Q @ ([q; v] - [qg; vg]) + 0.5 * u' @ R @ u + cost = 0.5 * (xs - xg)' @ Q @ (xs - xg) + 0.5 * us' @ R @ us Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: Unused. Included for API compliance. @@ -103,31 +96,31 @@ def cost( cost_val: The cost of the trajectory. new_params: Unused. Included for API compliance. """ - xs, xg, xs_err, xf_err, _, _ = self._setup_util(qs, vs, self.qg, self.vg) + xs_err = xs[:-1, :] - self.xg # errors before the terminal state + xf_err = xs[-1, :] - self.xg val = 0.5 * ( self.batch_quadform(xs_err, self.Q) + self.batch_quadform(xf_err, self.Qf) + self.batch_quadform(us, self.R) ) return val, params def grad( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + self, xs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: """Computes the gradient of the cost of a trajectory. Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: Unused. Included for API compliance. Returns: - gcost_qs (shape=(N + 1, nq): The gradient of the cost wrt qs. - gcost_vs (shape=(N + 1, nv): The gradient of the cost wrt vs. + gcost_xs (shape=(N + 1, nq + nv): The gradient of the cost wrt xs. gcost_us (shape=(N, nu)): The gradient of the cost wrt us. gcost_params: Unused. Included for API compliance. new_params: Unused. Included for API compliance. """ - xs, xg, xs_err, xf_err, nq, _, _ = self._setup_util(qs, vs, self.qg, self.vg) + xs_err = xs[:-1, :] - self.xg # errors before the terminal state + xf_err = xs[-1, :] - self.xg gcost_xs = jnp.concatenate( ( self.batch_matmul(xs_err, self.Q), @@ -135,39 +128,29 @@ def grad( ), axis=-1, ) - gcost_qs = gcost_xs[:, :nq] - gcost_vs = gcost_xs[:, nq:] gcost_us = self.batch_matmul(us, self.R) - return gcost_qs, gcost_vs, gcost_us, params, params + return gcost_xs, gcost_us, params, params def hess( - self, qs: jax.Array, vs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: - """Computes the Hessian of the cost of a trajectory. + self, xs: jax.Array, us: jax.Array, params: CostFunctionParams + ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + """Computes the gradient of the cost of a trajectory. Args: - qs (shape=(N + 1, nq)): The generalized positions over the trajectory. - vs (shape=(N + 1, nv)): The generalized velocities over the trajectory. + xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: Unused. Included for API compliance. Returns: - Hcost_qs (shape=(N + 1, nq, N + 1, nq)): The Hessian of the cost wrt qs. - Let t, s be times from 0 to N + 1. Then, d^2/dq_{t,i}dq_{s,j} = Hcost_qs[t, i, s, j]. - Hcost_vs (shape=(N + 1, nv, N + 1, nv)): The Hessian of the cost wrt vs. - Let t, s be times from 0 to N + 1. Then, d^2/dv_{t,i}dv_{s,j} = Hcost_vs[t, i, s, j]. + Hcost_xs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. + Let t, s be times from 0 to N + 1. Then, d^2/dx_{t,i}dx_{s,j} = Hcost_xs[t, i, s, j]. Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. Hcost_params: Unused. Included for API compliance. new_params: Unused. Included for API compliance. """ N = us.shape[0] - xs, xg, xs_err, xf_err, nq, _, _ = self._setup_util(qs, vs, self.qg, self.vg) Q_tiled = jnp.tile(self.Q[None, :, None, :], (N + 1, 1, N + 1, 1)) Hcost_xs = Q_tiled.at[-1, :, -1, :].set(self.Qf) - - Hcost_qs = Hcost_xs[:, :nq, :, :nq] - Hcost_vs = Hcost_xs[:, nq:, :, nq:] Hcost_us = jnp.tile(self.R[None, :, None, :], (N, 1, N, 1)) - - return Hcost_qs, Hcost_vs, Hcost_us, params, params + return Hcost_xs, Hcost_us, params, params diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index 60a80606..baea9d9f 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -19,22 +19,20 @@ # ##### # -def shoot(m: mjx.Model, q0: jax.Array, v0: jax.Array, us: jax.Array) -> Tuple[jax.Array, jax.Array]: +def shoot(m: mjx.Model, x0: jax.Array, us: jax.Array) -> jax.Array: """Utility function that shoots a model forward given a sequence of control inputs. Args: m: The model. - q0: The initial generalized coordinates. - v0: The initial generalized velocities. + x0: The initial state. us: The control inputs. Returns: - qs (shape=(N + 1, nq)): The generalized coordinates. - vs (shape=(N + 1, nv)): The generalized velocities. + xs (shape=(N + 1, nq + nv)): The state trajectory. """ # initializing the data d = mjx.make_data(m) - d = d.replace(qpos=q0, qvel=v0) # setting the initial state. + d = d.replace(qpos=x0[: m.nq], qvel=x0[m.nq :]) # setting the initial state. d = mjx.forward(m, d) # setting other internal states like acceleration without integrating def scan_fn(d, u): @@ -45,12 +43,9 @@ def scan_fn(d, u): return d, x # scan over the control inputs to get the trajectory. - _, xs = lax.scan(scan_fn, d, us, length=us.shape[0]) - _qs = xs[:, : m.nq] - _vs = xs[:, m.nq : m.nq + m.nv] - qs = jnp.concatenate((q0[None, :], _qs), axis=0) # (N + 1, nq) - vs = jnp.concatenate((v0[None, :], _vs), axis=0) # (N + 1, nv) - return qs, vs + _, _xs = lax.scan(scan_fn, d, us, length=us.shape[0]) + xs = jnp.concatenate((x0[None, :], _xs), axis=0) # (N + 1, nq + nv) + return xs # ################ # @@ -65,8 +60,7 @@ class ShootingParams(TrajectoryOptimizerParams): """Parameters for shooting methods.""" # inputs into the algorithm - q0: jax.Array # shape=(nq,) or (?) - v0: jax.Array # shape=(nv,) or (?) + x0: jax.Array # shape=(nq + nv,) or (?) us_guess: jax.Array # shape=(N, nu) or (?) @property @@ -83,16 +77,15 @@ def N(self) -> int: class ShootingAlgorithm(TrajectoryOptimizer): """A trajectory optimization algorithm based on shooting methods.""" - def optimize(self, params: ShootingParams) -> Tuple[jax.Array, jax.Array, jax.Array]: + def optimize(self, params: ShootingParams) -> Tuple[jax.Array, jax.Array]: """Optimizes a trajectory using a shooting method. Args: params: The parameters of the trajectory optimizer. Returns: - qs (shape=(N + 1, nq) or (?)): The optimized trajectory. - vs (shape=(N + 1, nv) or (?)): The optimized generalized velocities. - us (shape=(N, nu) or (?)): The optimized controls. + xs_star (shape=(N + 1, nq) or (?)): The optimized trajectory. + us_star (shape=(N, nu) or (?)): The optimized controls. """ raise NotImplementedError @@ -123,15 +116,14 @@ class VanillaPredictiveSampler(ShootingAlgorithm): nsamples: int = struct.field(pytree_node=False) stdev: float = struct.field(pytree_node=False) # noise scale, parameters theta_new ~ N(theta, (stdev ** 2) * I) - def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array, jax.Array]: + def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, jax.Array]: """Optimizes a trajectory using a vanilla predictive sampler. Args: params: The parameters of the trajectory optimizer. Returns: - qs (shape=(N + 1, nq)): The optimized trajectory. - vs (shape=(N + 1, nv)): The optimized generalized velocities. + xs (shape=(N + 1, nq + nv)): The optimized trajectory. us (shape=(N, nu)): The optimized controls. """ # unpack the params @@ -139,8 +131,7 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j nsamples = self.nsamples stdev = self.stdev - q0 = params.q0 - v0 = params.v0 + x0 = params.x0 us_guess = params.us_guess N = params.N key = params.key @@ -161,12 +152,9 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j # predict many samples, evaluate them, and return the best trajectory tuple # vmap over the input data and the control trajectories - qs_samples, vs_samples = vmap(shoot, in_axes=(None, None, None, 0))(m, q0, v0, us_samples) - costs, _ = vmap(self.cost_function.cost, in_axes=(0, 0, 0, None))( - qs_samples, vs_samples, us_samples, None - ) # (nsamples,) + xs_samples = vmap(shoot, in_axes=(None, None, 0))(m, x0, us_samples) + costs, _ = vmap(self.cost_function.cost, in_axes=(0, 0, None))(xs_samples, us_samples, None) # (nsamples,) best_idx = jnp.argmin(costs) - qs_star = lax.dynamic_slice(qs_samples, (best_idx, 0, 0), (1, N + 1, m.nq))[0] # (N + 1, nq) - vs_star = lax.dynamic_slice(vs_samples, (best_idx, 0, 0), (1, N + 1, m.nv))[0] # (N + 1, nv) + xs_star = lax.dynamic_slice(xs_samples, (best_idx, 0, 0), (1, N + 1, m.nq + m.nv))[0] # (N + 1, nq + nv) us_star = lax.dynamic_slice(us_samples, (best_idx, 0, 0), (1, N, m.nu))[0] # (N, nu) - return qs_star, vs_star, us_star + return xs_star, us_star diff --git a/examples/trajopt/ex_predictive_sampling.py b/examples/trajopt/ex_predictive_sampling.py index 73cca5c8..12f9686a 100644 --- a/examples/trajopt/ex_predictive_sampling.py +++ b/examples/trajopt/ex_predictive_sampling.py @@ -1,3 +1,5 @@ +import timeit + import jax import jax.numpy as jnp from jax import jit @@ -24,52 +26,50 @@ Q=jnp.eye(model.nq + model.nv), Qf=10.0 * jnp.eye(model.nq + model.nv), R=0.01 * jnp.eye(model.nu), - # qg=jnp.zeros(model.nq).at[6].set(1.0), # if force_float=True - qg=jnp.zeros(model.nq), - vg=jnp.zeros(model.nv), + # xg=jnp.zeros(model.nq + model.nv).at[6].set(1.0), # if force_float=True + xg=jnp.zeros(model.nq + model.nv), ) - nsamples = 100000 + nsamples = 100 stdev = 0.01 ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) # sampler parameters key = jax.random.PRNGKey(0) # random seed for the predictive sampler - q0 = jnp.zeros(model.nq).at[6].set(1.0) - v0 = jnp.zeros(model.nv) - num_steps = 25 + # x0 = jnp.zeros(model.nq + model.nv).at[6].set(1.0) # if force_float=True + x0 = jnp.zeros(model.nq + model.nv) + num_steps = 10 us_guess = jnp.zeros((num_steps, model.nu)) - params = VanillaPredictiveSamplerParams(key=key, q0=q0, v0=v0, us_guess=us_guess) + params = VanillaPredictiveSamplerParams(key=key, x0=x0, us_guess=us_guess) # sampling the best sequence of qs, vs, and us optimize_fn = jit(ps.optimize) # [DEBUG] profiling with nsight systems - # qs_star, vs_star, us_star = optimize_fn(params) # JIT compiling + # xs_star, us_star = optimize_fn(params) # JIT compiling # with jax.profiler.trace("/tmp/jax-trace", create_perfetto_link=True): - # qs_star, vs_star, us_star = optimize_fn(params) # after JIT + # xs_star us_star = optimize_fn(params) # after JIT - # def _time_fn(): - # qs_star, vs_star, us_star = optimize_fn(params) - # qs_star.block_until_ready() - # vs_star.block_until_ready() - # us_star.block_until_ready() + def _time_fn(): + xs_star, us_star = optimize_fn(params) + xs_star.block_until_ready() + us_star.block_until_ready() - # compile_time = timeit.timeit(_time_fn, number=1) - # print(f"Compile time: {compile_time}") + compile_time = timeit.timeit(_time_fn, number=1) + print(f"Compile time: {compile_time}") - # # informal timing test - # # TODO(ahl): identify bottlenecks and zap them - # # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number - # # of samples. Here are a few preliminary results: - # # * nsamples=100, numsteps=10. avg: 0.01s - # # * nsamples=1000, numsteps=10. avg: 0.015s - # # * nsamples=10000, numsteps=10. avg: 0.07s - # # * nsamples=100, numsteps=100. avg: 0.1s - # # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps - # # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind - # # that we've completely disabled contact for this example and set the number of solver iterations and line search - # # iterations to very runtime-friendly values - # num_timing_iters = 100 - # time = timeit.timeit(_time_fn, number=num_timing_iters) - # print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves - # breakpoint() + # informal timing test + # TODO(ahl): identify bottlenecks and zap them + # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number + # of samples. Here are a few preliminary results: + # * nsamples=100, numsteps=10. avg: 0.01s + # * nsamples=1000, numsteps=10. avg: 0.015s + # * nsamples=10000, numsteps=10. avg: 0.07s + # * nsamples=100, numsteps=100. avg: 0.1s + # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps + # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind + # that we've completely disabled contact for this example and set the number of solver iterations and line search + # iterations to very runtime-friendly values + num_timing_iters = 100 + time = timeit.timeit(_time_fn, number=num_timing_iters) + print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves + breakpoint() From b2d829661945dc5c4006b1013685e0004facb2ca Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:03:56 -0800 Subject: [PATCH 32/40] tests for cost function and its derivatives --- ambersim/trajopt/base.py | 30 +++++++---- ambersim/trajopt/cost.py | 100 ++++++++++++++++++++++--------------- tests/trajopt/test_cost.py | 55 ++++++++++++++++++++ 3 files changed, 136 insertions(+), 49 deletions(-) create mode 100644 tests/trajopt/test_cost.py diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 835378e9..7e4bb5d4 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -72,8 +72,7 @@ def optimize(self, params: TrajectoryOptimizerParams) -> Tuple[jax.Array, jax.Ar params: The parameters of the trajectory optimizer. Returns: - qs_star (shape=(N + 1, nq) or (?)): The optimized trajectory. - vs_star (shape=(N + 1, nv) or (?)): The optimized generalized velocities. + xs_star (shape=(N + 1, nq + nv) or (?)): The optimized trajectory. us_star (shape=(N, nu) or (?)): The optimized controls. """ raise NotImplementedError @@ -136,27 +135,38 @@ def grad( gcost_params: The gradient of the cost wrt params. new_params: The updated parameters of the cost function. """ - return grad(self.cost, argnums=(0, 1, 2))(xs, us, params) + (params,) + _fn = lambda xs, us, params: self.cost(xs, us, params)[0] # only differentiate wrt the cost val + return grad(_fn, argnums=(0, 1, 2))(xs, us, params) + (params,) def hess( self, xs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + ) -> Tuple[ + jax.Array, jax.Array, CostFunctionParams, jax.Array, CostFunctionParams, CostFunctionParams, CostFunctionParams + ]: """Computes the Hessian of the cost of a trajectory. The default implementation of this function uses JAX's autodiff. Simply override this function if you would like to supply an analytical Hessian. + Let t, s be times from 0 to N + 1. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. + Args: xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: The parameters of the cost function. Returns: - Hcost_xs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. - Let t, s be times from 0 to N + 1. Then, d^2/dx_{t,i}dx_{s,j} = Hcost_xs[t, i, s, j]. - Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. - Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. - Hcost_params: The Hessian of the cost wrt params. + Hcost_xsxs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. + Hcost_xsus (shape=(N + 1, nq + nv, N, nu)): The Hessian of the cost wrt xs and us. + Hcost_xsparams: The Hessian of the cost wrt xs and params. + Hcost_usus (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. + Hcost_usparams: The Hessian of the cost wrt us and params. + Hcost_paramsall: The Hessian of the cost wrt params and everything else. new_params: The updated parameters of the cost function. """ - return hessian(self.cost, argnums=(0, 1, 2))(xs, us, params) + (params,) + _fn = lambda xs, us, params: self.cost(xs, us, params)[0] # only differentiate wrt the cost val + hessians = hessian(_fn, argnums=(0, 1, 2))(xs, us, params) + Hcost_xsxs, Hcost_xsus, Hcost_xsparams = hessians[0] + _, Hcost_usus, Hcost_usparams = hessians[1] + Hcost_paramsall = hessians[2] + return Hcost_xsxs, Hcost_xsus, Hcost_xsparams, Hcost_usus, Hcost_usparams, Hcost_paramsall, params diff --git a/ambersim/trajopt/cost.py b/ambersim/trajopt/cost.py index feb579bb..cd3f502f 100644 --- a/ambersim/trajopt/cost.py +++ b/ambersim/trajopt/cost.py @@ -3,6 +3,7 @@ import jax import jax.numpy as jnp from flax import struct +from jax import lax, vmap from ambersim.trajopt.base import CostFunction, CostFunctionParams @@ -32,30 +33,6 @@ def __init__(self, Q: jax.Array, Qf: jax.Array, R: jax.Array, xg: jax.Array) -> self.R = R self.xg = xg - # @staticmethod - # def _setup_util( - # xs: jax.Array, qg: jax.Array, vg: jax.Array - # ) -> Tuple[jax.Array, jax.Array, jax.Array, jax.Array, int, int, int]: - # """Utility function that sets up the cost function. - - # Args: - # xs (shape=(N + 1, nq + nv)): The state trajectory - # xg (shape=(nq + nv,)): The goal state. - - # Returns: - # xs (shape=(N + 1, nx)): The states over the trajectory. - # xg (shape=(nx,)): The goal state. - # xs_err (shape=(N, nx)): The state errors up to the final state. - # xf_err (shape=(nx,)): The state error at the final state. - # nq: The number of generalized coordinates. - # nv: The number of generalized velocities. - # """ - # xs = jnp.concatenate((qs, vs), axis=-1) - # xg = jnp.concatenate((qg, vg), axis=-1) - # xs_err = xs[:-1, :] - xg - # xf_err = xs[-1, :] - xg - # return xs, xg, xs_err, xf_err, qs.shape[-1], vs.shape[-1] - @staticmethod def batch_quadform(bs: jax.Array, A: jax.Array) -> jax.Array: """Computes a batched quadratic form for a single instance of A. @@ -98,8 +75,12 @@ def cost(self, xs: jax.Array, us: jax.Array, params: CostFunctionParams) -> Tupl """ xs_err = xs[:-1, :] - self.xg # errors before the terminal state xf_err = xs[-1, :] - self.xg - val = 0.5 * ( - self.batch_quadform(xs_err, self.Q) + self.batch_quadform(xf_err, self.Qf) + self.batch_quadform(us, self.R) + val = 0.5 * jnp.squeeze( + ( + jnp.sum(self.batch_quadform(xs_err, self.Q)) + + self.batch_quadform(xf_err, self.Qf) + + jnp.sum(self.batch_quadform(us, self.R)) + ) ) return val, params @@ -126,31 +107,72 @@ def grad( self.batch_matmul(xs_err, self.Q), (self.Qf @ xf_err)[None, :], ), - axis=-1, + axis=-2, ) gcost_us = self.batch_matmul(us, self.R) return gcost_xs, gcost_us, params, params def hess( self, xs: jax.Array, us: jax.Array, params: CostFunctionParams - ) -> Tuple[jax.Array, jax.Array, CostFunctionParams, CostFunctionParams]: + ) -> Tuple[ + jax.Array, jax.Array, CostFunctionParams, jax.Array, CostFunctionParams, CostFunctionParams, CostFunctionParams + ]: """Computes the gradient of the cost of a trajectory. + Let t, s be times from 0 to N + 1. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. + Args: xs (shape=(N + 1, nq + nv)): The state trajectory. us (shape=(N, nu)): The controls over the trajectory. params: Unused. Included for API compliance. Returns: - Hcost_xs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. - Let t, s be times from 0 to N + 1. Then, d^2/dx_{t,i}dx_{s,j} = Hcost_xs[t, i, s, j]. - Hcost_us (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. - Let t, s be times from 0 to N. Then, d^2/du_{t,i}du_{s,j} = Hcost_us[t, i, s, j]. - Hcost_params: Unused. Included for API compliance. - new_params: Unused. Included for API compliance. + Hcost_xsxs (shape=(N + 1, nq + nv, N + 1, nq + nv)): The Hessian of the cost wrt xs. + Hcost_xsus (shape=(N + 1, nq + nv, N, nu)): The Hessian of the cost wrt xs and us. + Hcost_xsparams: The Hessian of the cost wrt xs and params. + Hcost_usus (shape=(N, nu, N, nu)): The Hessian of the cost wrt us. + Hcost_usparams: The Hessian of the cost wrt us and params. + Hcost_paramsall: The Hessian of the cost wrt params and everything else. + new_params: The updated parameters of the cost function. """ - N = us.shape[0] - Q_tiled = jnp.tile(self.Q[None, :, None, :], (N + 1, 1, N + 1, 1)) - Hcost_xs = Q_tiled.at[-1, :, -1, :].set(self.Qf) - Hcost_us = jnp.tile(self.R[None, :, None, :], (N, 1, N, 1)) - return Hcost_xs, Hcost_us, params, params + # setting up + nx = self.Q.shape[0] + N, nu = us.shape + Q = self.Q + Qf = self.Qf + R = self.R + dummy_params = CostFunctionParams() + + # Hessian for state + Hcost_xsxs = jnp.zeros((N + 1, nx, N + 1, nx)) + Hcost_xsxs = vmap( + lambda i: lax.dynamic_update_slice( + jnp.zeros((nx, N + 1, nx)), + Q[:, None, :], + (0, i, 0), + ) + )( + jnp.arange(N + 1) + ) # only the terms [i, :, i, :] are nonzero + Hcost_xsxs = Hcost_xsxs.at[-1, :, -1, :].set(Qf) # last one is different + + # trivial cross-terms of Hessian + Hcost_xsus = jnp.zeros((N + 1, nx, N, nu)) + Hcost_xsparams = dummy_params + + # Hessian for control inputs + Hcost_usus = jnp.zeros((N, nu, N, nu)) + Hcost_usus = vmap( + lambda i: lax.dynamic_update_slice( + jnp.zeros((nu, N, nu)), + R[:, None, :], + (0, i, 0), + ) + )( + jnp.arange(N) + ) # only the terms [i, :, i, :] are nonzero + + # trivial cross-terms and Hessian for params + Hcost_usparams = dummy_params + Hcost_paramsall = dummy_params + return Hcost_xsxs, Hcost_xsus, Hcost_xsparams, Hcost_usus, Hcost_usparams, Hcost_paramsall, params diff --git a/tests/trajopt/test_cost.py b/tests/trajopt/test_cost.py new file mode 100644 index 00000000..3eb0c8de --- /dev/null +++ b/tests/trajopt/test_cost.py @@ -0,0 +1,55 @@ +import jax +import jax.numpy as jnp +from jax import hessian, jacobian + +from ambersim.trajopt.base import CostFunctionParams +from ambersim.trajopt.cost import StaticGoalQuadraticCost +from ambersim.utils.io_utils import load_mjx_model_and_data_from_file + + +def test_sgqc(): + """Tests that the StaticGoalQuadraticCost works correctly.""" + # loading model and cost function + model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) + cost_function = StaticGoalQuadraticCost( + Q=jnp.eye(model.nq + model.nv), + Qf=10.0 * jnp.eye(model.nq + model.nv), + R=0.01 * jnp.eye(model.nu), + xg=jnp.zeros(model.nq + model.nv), + ) + + # generating dummy data + N = 10 + key = jax.random.PRNGKey(0) + xs = jax.random.normal(key=key, shape=(N + 1, model.nq + model.nv)) + us = jax.random.normal(key=key, shape=(N, model.nu)) + + # comparing cost value vs. ground truth for loop + val_test, _ = cost_function.cost(xs, us, params=CostFunctionParams()) + val_gt = 0.0 + for i in range(N): + xs_err = xs[i, :] - cost_function.xg + val_gt += 0.5 * (xs_err @ cost_function.Q @ xs_err + us[i, :] @ cost_function.R @ us[i, :]) + xs_err = xs[N, :] - cost_function.xg + val_gt += 0.5 * (xs_err @ cost_function.Qf @ xs_err) + val_gt = jnp.squeeze(val_gt) + assert jnp.allclose(val_test, val_gt) + + # comparing cost gradients vs. jax autodiff + gcost_xs_test, gcost_us_test, _, _ = cost_function.grad(xs, us, params=CostFunctionParams()) + gcost_xs_gt, gcost_us_gt, _, _ = super(StaticGoalQuadraticCost, cost_function).grad( + xs, us, params=CostFunctionParams() + ) + assert jnp.allclose(gcost_xs_test, gcost_xs_gt) + assert jnp.allclose(gcost_us_test, gcost_us_gt) + + # comparing cost Hessians vs. jax autodiff + Hcost_xsxs_test, Hcost_xsus_test, _, Hcost_usus_test, _, _, _ = cost_function.hess( + xs, us, params=CostFunctionParams() + ) + Hcost_xsxs_gt, Hcost_xsus_gt, _, Hcost_usus_gt, _, _, _ = super(StaticGoalQuadraticCost, cost_function).hess( + xs, us, params=CostFunctionParams() + ) + assert jnp.allclose(Hcost_xsxs_test, Hcost_xsxs_gt) + assert jnp.allclose(Hcost_xsus_test, Hcost_xsus_gt) + assert jnp.allclose(Hcost_usus_test, Hcost_usus_gt) From 0e24057b9966c85140b9ec6cc04ff4613038c325 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:13:07 -0800 Subject: [PATCH 33/40] added smoke test for vanilla predictive sampler --- tests/trajopt/test_predictive_sampler.py | 45 ++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 tests/trajopt/test_predictive_sampler.py diff --git a/tests/trajopt/test_predictive_sampler.py b/tests/trajopt/test_predictive_sampler.py new file mode 100644 index 00000000..f78b735f --- /dev/null +++ b/tests/trajopt/test_predictive_sampler.py @@ -0,0 +1,45 @@ +import jax +import jax.numpy as jnp +from jax import jit +from mujoco.mjx._src.types import DisableBit + +from ambersim.trajopt.cost import StaticGoalQuadraticCost +from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams +from ambersim.utils.io_utils import load_mjx_model_and_data_from_file + + +def test_smoke_VPS(): + """Simple smoke test to make sure we can run inputs through the vanilla predictive sampler + jit.""" + # initializing the predictive sampler + model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) + model = model.replace( + opt=model.opt.replace( + timestep=0.002, # dt + iterations=1, # number of Newton steps to take during solve + ls_iterations=4, # number of line search iterations along step direction + integrator=0, # Euler semi-implicit integration + solver=2, # Newton solver + disableflags=DisableBit.CONTACT, # disable contact for this test + ) + ) + cost_function = StaticGoalQuadraticCost( + Q=jnp.eye(model.nq + model.nv), + Qf=10.0 * jnp.eye(model.nq + model.nv), + R=0.01 * jnp.eye(model.nu), + xg=jnp.zeros(model.nq + model.nv), + ) + nsamples = 100 + stdev = 0.01 + ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) + + # sampler parameters + key = jax.random.PRNGKey(0) # random seed for the predictive sampler + # x0 = jnp.zeros(model.nq + model.nv).at[6].set(1.0) # if force_float=True + x0 = jnp.zeros(model.nq + model.nv) + num_steps = 10 + us_guess = jnp.zeros((num_steps, model.nu)) + params = VanillaPredictiveSamplerParams(key=key, x0=x0, us_guess=us_guess) + + # sampling the best sequence of qs, vs, and us + optimize_fn = jit(ps.optimize) + assert optimize_fn(params) From adf4243fb55eaba2bc396f74aaed4a99408f6d69 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:16:31 -0800 Subject: [PATCH 34/40] remove the example since it's implemented as a benchmark in an upstream PR --- examples/trajopt/ex_predictive_sampling.py | 75 ---------------------- 1 file changed, 75 deletions(-) delete mode 100644 examples/trajopt/ex_predictive_sampling.py diff --git a/examples/trajopt/ex_predictive_sampling.py b/examples/trajopt/ex_predictive_sampling.py deleted file mode 100644 index 12f9686a..00000000 --- a/examples/trajopt/ex_predictive_sampling.py +++ /dev/null @@ -1,75 +0,0 @@ -import timeit - -import jax -import jax.numpy as jnp -from jax import jit -from mujoco.mjx._src.types import DisableBit - -from ambersim.trajopt.cost import StaticGoalQuadraticCost -from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams -from ambersim.utils.io_utils import load_mjx_model_and_data_from_file - -if __name__ == "__main__": - # initializing the predictive sampler - model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) - model = model.replace( - opt=model.opt.replace( - timestep=0.002, # dt - iterations=1, # number of Newton steps to take during solve - ls_iterations=4, # number of line search iterations along step direction - integrator=0, # Euler semi-implicit integration - solver=2, # Newton solver - disableflags=DisableBit.CONTACT, # [IMPORTANT] disable contact for this example - ) - ) - cost_function = StaticGoalQuadraticCost( - Q=jnp.eye(model.nq + model.nv), - Qf=10.0 * jnp.eye(model.nq + model.nv), - R=0.01 * jnp.eye(model.nu), - # xg=jnp.zeros(model.nq + model.nv).at[6].set(1.0), # if force_float=True - xg=jnp.zeros(model.nq + model.nv), - ) - nsamples = 100 - stdev = 0.01 - ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) - - # sampler parameters - key = jax.random.PRNGKey(0) # random seed for the predictive sampler - # x0 = jnp.zeros(model.nq + model.nv).at[6].set(1.0) # if force_float=True - x0 = jnp.zeros(model.nq + model.nv) - num_steps = 10 - us_guess = jnp.zeros((num_steps, model.nu)) - params = VanillaPredictiveSamplerParams(key=key, x0=x0, us_guess=us_guess) - - # sampling the best sequence of qs, vs, and us - optimize_fn = jit(ps.optimize) - - # [DEBUG] profiling with nsight systems - # xs_star, us_star = optimize_fn(params) # JIT compiling - # with jax.profiler.trace("/tmp/jax-trace", create_perfetto_link=True): - # xs_star us_star = optimize_fn(params) # after JIT - - def _time_fn(): - xs_star, us_star = optimize_fn(params) - xs_star.block_until_ready() - us_star.block_until_ready() - - compile_time = timeit.timeit(_time_fn, number=1) - print(f"Compile time: {compile_time}") - - # informal timing test - # TODO(ahl): identify bottlenecks and zap them - # [Dec. 3, 2023] on vulcan, I've informally tested the scaling of runtime with the number of steps and the number - # of samples. Here are a few preliminary results: - # * nsamples=100, numsteps=10. avg: 0.01s - # * nsamples=1000, numsteps=10. avg: 0.015s - # * nsamples=10000, numsteps=10. avg: 0.07s - # * nsamples=100, numsteps=100. avg: 0.1s - # we conclude that the runtime scales predictably linearly with numsteps, but we also have some sort of (perhaps - # logarithmic) scaling of runtime with nsamples. this outlook is somewhat grim, and we need to also keep in mind - # that we've completely disabled contact for this example and set the number of solver iterations and line search - # iterations to very runtime-friendly values - num_timing_iters = 100 - time = timeit.timeit(_time_fn, number=num_timing_iters) - print(f"Avg. runtime: {time / num_timing_iters}") # timeit returns TOTAL time, so we compute the average ourselves - breakpoint() From 773be6a4eb6378f6fe8ea3d93476d30ac2d826c6 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:18:37 -0800 Subject: [PATCH 35/40] minor docstring edit --- ambersim/trajopt/base.py | 2 +- ambersim/trajopt/cost.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ambersim/trajopt/base.py b/ambersim/trajopt/base.py index 7e4bb5d4..2e50975e 100644 --- a/ambersim/trajopt/base.py +++ b/ambersim/trajopt/base.py @@ -148,7 +148,7 @@ def hess( The default implementation of this function uses JAX's autodiff. Simply override this function if you would like to supply an analytical Hessian. - Let t, s be times from 0 to N + 1. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. + Let t, s be times 0, 1, 2, etc. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. Args: xs (shape=(N + 1, nq + nv)): The state trajectory. diff --git a/ambersim/trajopt/cost.py b/ambersim/trajopt/cost.py index cd3f502f..fa560128 100644 --- a/ambersim/trajopt/cost.py +++ b/ambersim/trajopt/cost.py @@ -119,7 +119,7 @@ def hess( ]: """Computes the gradient of the cost of a trajectory. - Let t, s be times from 0 to N + 1. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. + Let t, s be times 0, 1, 2, etc. Then, d^2H/da_{t,i}db_{s,j} = Hcost_asbs[t, i, s, j]. Args: xs (shape=(N + 1, nq + nv)): The state trajectory. From 544f9643f1f2e85bca07b8c4fc55afea6567fc34 Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:25:49 -0800 Subject: [PATCH 36/40] ensure predictive sampler also accounts for cost of guess vs. samples --- ambersim/trajopt/shooting.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ambersim/trajopt/shooting.py b/ambersim/trajopt/shooting.py index baea9d9f..414917a1 100644 --- a/ambersim/trajopt/shooting.py +++ b/ambersim/trajopt/shooting.py @@ -136,19 +136,16 @@ def optimize(self, params: VanillaPredictiveSamplerParams) -> Tuple[jax.Array, j N = params.N key = params.key - # sample over the control inputs - _us_samples = us_guess + jax.random.normal(key, shape=(nsamples, N, m.nu)) * stdev + # sample over the control inputs - the first sample is the guess, since it's possible that it's the best one + noise = jnp.concatenate( + (jnp.zeros((1, N, m.nu)), jax.random.normal(key, shape=(nsamples - 1, N, m.nu)) * stdev), axis=0 + ) + _us_samples = us_guess + noise # clamping the samples to their control limits - # TODO(ahl): write a create classmethod that allows the user to set default_limits optionally with some semi- - # reasonable default value - # TODO(ahl): check whether joints with no limits have reasonable defaults for m.actuator_ctrlrange limits = m.actuator_ctrlrange clip_fn = partial(jnp.clip, a_min=limits[:, 0], a_max=limits[:, 1]) # clipping function with limits already set us_samples = vmap(vmap(clip_fn))(_us_samples) # apply limits only to the last dim, need a nested vmap - # limited = m.actuator_ctrllimited[:, None] # (nu, 1) whether each actuator has limited control authority - # default_limits = jnp.array([[-1000.0, 1000.0]] * m.nu) # (nu, 2) default limits for each actuator - # limits = jnp.where(limited, m.actuator_ctrlrange, default_limits) # (nu, 2) # predict many samples, evaluate them, and return the best trajectory tuple # vmap over the input data and the control trajectories From 4e22073b2370858a197741e621e95df47879c5fa Mon Sep 17 00:00:00 2001 From: alberthli Date: Mon, 4 Dec 2023 23:54:33 -0800 Subject: [PATCH 37/40] add sanity check for predictive sampling --- tests/trajopt/test_predictive_sampler.py | 46 +++++++++++++++++++++--- 1 file changed, 41 insertions(+), 5 deletions(-) diff --git a/tests/trajopt/test_predictive_sampler.py b/tests/trajopt/test_predictive_sampler.py index f78b735f..0d29a1b8 100644 --- a/tests/trajopt/test_predictive_sampler.py +++ b/tests/trajopt/test_predictive_sampler.py @@ -1,15 +1,16 @@ import jax import jax.numpy as jnp -from jax import jit +from jax import jit, vmap from mujoco.mjx._src.types import DisableBit +from ambersim.trajopt.base import CostFunctionParams from ambersim.trajopt.cost import StaticGoalQuadraticCost -from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams +from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams, shoot from ambersim.utils.io_utils import load_mjx_model_and_data_from_file -def test_smoke_VPS(): - """Simple smoke test to make sure we can run inputs through the vanilla predictive sampler + jit.""" +def _make_vps_data(): + """Makes data required for testing vanilla predictive sampling.""" # initializing the predictive sampler model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) model = model.replace( @@ -31,10 +32,15 @@ def test_smoke_VPS(): nsamples = 100 stdev = 0.01 ps = VanillaPredictiveSampler(model=model, cost_function=cost_function, nsamples=nsamples, stdev=stdev) + return ps, model, cost_function + + +def test_smoke_VPS(): + """Simple smoke test to make sure we can run inputs through the vanilla predictive sampler + jit.""" + ps, model, _ = _make_vps_data() # sampler parameters key = jax.random.PRNGKey(0) # random seed for the predictive sampler - # x0 = jnp.zeros(model.nq + model.nv).at[6].set(1.0) # if force_float=True x0 = jnp.zeros(model.nq + model.nv) num_steps = 10 us_guess = jnp.zeros((num_steps, model.nu)) @@ -43,3 +49,33 @@ def test_smoke_VPS(): # sampling the best sequence of qs, vs, and us optimize_fn = jit(ps.optimize) assert optimize_fn(params) + + +def test_VPS_cost_decrease(): + """Tests to make sure vanilla predictive sampling decreases (or maintains) the cost.""" + # set up sampler and cost function + ps, model, cost_function = _make_vps_data() + + # batched sampler parameters + batch_size = 10 + key = jax.random.PRNGKey(0) # random seed for the predictive sampler + x0 = jax.random.normal(key=key, shape=(batch_size, model.nq + model.nv)) + + key, subkey = jax.random.split(key) + num_steps = 10 + us_guess = jax.random.normal(key=subkey, shape=(batch_size, num_steps, model.nu)) + + keys = jax.random.split(key, num=batch_size) + params = VanillaPredictiveSamplerParams(key=keys, x0=x0, us_guess=us_guess) + + # sampling with the vanilla predictive sampler + xs_stars, us_stars = vmap(ps.optimize)(params) + + # "optimal" rollout from predictive sampling + vmap_cost = vmap(lambda xs, us: cost_function.cost(xs, us, CostFunctionParams())[0], in_axes=(0, 0)) + costs_star = vmap_cost(xs_stars, us_stars) + + # simply shooting the random initial guess + xs_guess = vmap(shoot, in_axes=(None, 0, 0))(model, x0, us_guess) + costs_guess = vmap_cost(xs_guess, us_guess) + assert jnp.all(costs_star <= costs_guess) From 0c49505398e49f950e9996fb95b6628860effad9 Mon Sep 17 00:00:00 2001 From: alberthli Date: Tue, 5 Dec 2023 12:21:45 -0800 Subject: [PATCH 38/40] added fixture to tests --- tests/trajopt/test_predictive_sampler.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/tests/trajopt/test_predictive_sampler.py b/tests/trajopt/test_predictive_sampler.py index 0d29a1b8..52fab13a 100644 --- a/tests/trajopt/test_predictive_sampler.py +++ b/tests/trajopt/test_predictive_sampler.py @@ -1,5 +1,8 @@ +import os + import jax import jax.numpy as jnp +import pytest from jax import jit, vmap from mujoco.mjx._src.types import DisableBit @@ -8,8 +11,11 @@ from ambersim.trajopt.shooting import VanillaPredictiveSampler, VanillaPredictiveSamplerParams, shoot from ambersim.utils.io_utils import load_mjx_model_and_data_from_file +os.environ["XLA_PYTHON_CLIENT_PREALLOCATE"] = "false" # fixes OOM error + -def _make_vps_data(): +@pytest.fixture +def vps_data(): """Makes data required for testing vanilla predictive sampling.""" # initializing the predictive sampler model, _ = load_mjx_model_and_data_from_file("models/barrett_hand/bh280.xml", force_float=False) @@ -35,9 +41,9 @@ def _make_vps_data(): return ps, model, cost_function -def test_smoke_VPS(): +def test_smoke_VPS(vps_data): """Simple smoke test to make sure we can run inputs through the vanilla predictive sampler + jit.""" - ps, model, _ = _make_vps_data() + ps, model, _ = vps_data # sampler parameters key = jax.random.PRNGKey(0) # random seed for the predictive sampler @@ -51,10 +57,10 @@ def test_smoke_VPS(): assert optimize_fn(params) -def test_VPS_cost_decrease(): +def test_VPS_cost_decrease(vps_data): """Tests to make sure vanilla predictive sampling decreases (or maintains) the cost.""" # set up sampler and cost function - ps, model, cost_function = _make_vps_data() + ps, model, cost_function = vps_data # batched sampler parameters batch_size = 10 @@ -72,7 +78,7 @@ def test_VPS_cost_decrease(): xs_stars, us_stars = vmap(ps.optimize)(params) # "optimal" rollout from predictive sampling - vmap_cost = vmap(lambda xs, us: cost_function.cost(xs, us, CostFunctionParams())[0], in_axes=(0, 0)) + vmap_cost = jit(vmap(lambda xs, us: cost_function.cost(xs, us, CostFunctionParams()))[0], in_axes=(0, 0)) costs_star = vmap_cost(xs_stars, us_stars) # simply shooting the random initial guess From 90ea8f4ced3eedf90fc7c4ca66ea382e54e7edad Mon Sep 17 00:00:00 2001 From: alberthli Date: Tue, 5 Dec 2023 13:00:40 -0800 Subject: [PATCH 39/40] fix stray parenthetical --- tests/trajopt/test_predictive_sampler.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/trajopt/test_predictive_sampler.py b/tests/trajopt/test_predictive_sampler.py index 52fab13a..7111fc05 100644 --- a/tests/trajopt/test_predictive_sampler.py +++ b/tests/trajopt/test_predictive_sampler.py @@ -78,7 +78,7 @@ def test_VPS_cost_decrease(vps_data): xs_stars, us_stars = vmap(ps.optimize)(params) # "optimal" rollout from predictive sampling - vmap_cost = jit(vmap(lambda xs, us: cost_function.cost(xs, us, CostFunctionParams()))[0], in_axes=(0, 0)) + vmap_cost = jit(vmap(lambda xs, us: cost_function.cost(xs, us, CostFunctionParams())[0], in_axes=(0, 0))) costs_star = vmap_cost(xs_stars, us_stars) # simply shooting the random initial guess From bd3add2f15abdbe271ee92202ad5e135f0e81399 Mon Sep 17 00:00:00 2001 From: alberthli Date: Tue, 5 Dec 2023 13:05:44 -0800 Subject: [PATCH 40/40] update README + pull upstream changes --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f2240173..bbce0098 100644 --- a/README.md +++ b/README.md @@ -138,7 +138,7 @@ jit_fn = jit(fn) with jax.profiler.trace("/dir/to/profiling/results"): jit_fn() ``` -Now, the traced results will specifically show the time spent in `fn2` under the name you chose. +Now, the traced results will specifically show the time spent in `fn2` under the name you chose. Note that you can also use `jax.profiler.TraceAnnotation` or `jax.profiler.annotate_function()` instead, [as recommended](https://jax.readthedocs.io/en/latest/profiling.html#adding-custom-trace-events). ### Tooling We use various tools to ensure code quality.